Line data Source code
1 : /*------------------------------------------------------------------------------
2 : -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 : -- This file is a part of the LFR FSW
4 : -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 : --
6 : -- This program is free software; you can redistribute it and/or modify
7 : -- it under the terms of the GNU General Public License as published by
8 : -- the Free Software Foundation; either version 2 of the License, or
9 : -- (at your option) any later version.
10 : --
11 : -- This program is distributed in the hope that it will be useful,
12 : -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 : -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 : -- GNU General Public License for more details.
15 : --
16 : -- You should have received a copy of the GNU General Public License
17 : -- along with this program; if not, write to the Free Software
18 : -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 : -------------------------------------------------------------------------------*/
20 : /*-- Author : Paul Leroy
21 : -- Contact : Alexis Jeandet
22 : -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 : ----------------------------------------------------------------------------*/
24 :
25 : /** Functions and tasks related to waveform packet generation.
26 : *
27 : * @file
28 : * @author P. LEROY
29 : *
30 : * A group of functions to handle waveforms, in snapshot or continuous format.\n
31 : *
32 : */
33 :
34 : #include "hw/wf_handler.h"
35 : #include "fsw_compile_warnings.h"
36 : #include "fsw_debug.h"
37 : #include "fsw_misc.h"
38 : #include "hw/lfr_regs.h"
39 : #include "lfr_common_headers/fsw_params.h"
40 :
41 : //***************
42 : // waveform rings
43 : // F0
44 : DISABLE_MISSING_FIELD_INITIALIZER_WARNING
45 : ring_node waveform_ring_f0[NB_RING_NODES_F0] = { { 0 } };
46 : ENABLE_MISSING_FIELD_INITIALIZER_WARNING
47 : ring_node* current_ring_node_f0 = NULL;
48 : ring_node* ring_node_to_send_swf_f0 = NULL;
49 : // F1
50 : DISABLE_MISSING_FIELD_INITIALIZER_WARNING
51 : ring_node waveform_ring_f1[NB_RING_NODES_F1] = { { 0 } };
52 : ENABLE_MISSING_FIELD_INITIALIZER_WARNING
53 : ring_node* current_ring_node_f1 = NULL;
54 : ring_node* ring_node_to_send_swf_f1 = NULL;
55 : ring_node* ring_node_to_send_cwf_f1 = NULL;
56 : // F2
57 : DISABLE_MISSING_FIELD_INITIALIZER_WARNING
58 : ring_node waveform_ring_f2[NB_RING_NODES_F2] = { { 0 } };
59 : ENABLE_MISSING_FIELD_INITIALIZER_WARNING
60 : ring_node* current_ring_node_f2 = NULL;
61 : ring_node* ring_node_to_send_swf_f2 = NULL;
62 : ring_node* ring_node_to_send_cwf_f2 = NULL;
63 : // F3
64 : DISABLE_MISSING_FIELD_INITIALIZER_WARNING
65 : ring_node waveform_ring_f3[NB_RING_NODES_F3] = { { 0 } };
66 : ENABLE_MISSING_FIELD_INITIALIZER_WARNING
67 : ring_node* current_ring_node_f3 = NULL;
68 : ring_node* ring_node_to_send_cwf_f3 = NULL;
69 : char wf_cont_f3_light[NB_SAMPLES_PER_SNAPSHOT * NB_BYTES_CWF3_LIGHT_BLK] = { 0 };
70 :
71 : bool extractSWF1 = false;
72 : bool extractSWF2 = false;
73 : bool swf0_ready_flag_f1 = false;
74 : bool swf0_ready_flag_f2 = false;
75 : bool swf1_ready = false;
76 : bool swf2_ready = false;
77 :
78 : int swf1_extracted[(NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK)] = { 0 };
79 : int swf2_extracted[(NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK)] = { 0 };
80 :
81 : DISABLE_MISSING_FIELD_INITIALIZER_WARNING
82 : ring_node ring_node_swf1_extracted = { 0 };
83 : ring_node ring_node_swf2_extracted = { 0 };
84 : ENABLE_MISSING_FIELD_INITIALIZER_WARNING
85 :
86 : typedef enum resynchro_state_t
87 : {
88 : MEASURE,
89 : CORRECTION
90 : } resynchro_state;
91 :
92 : //*********************
93 : // Interrupt SubRoutine
94 :
95 0 : ring_node* getRingNodeToSendCWF(unsigned char frequencyChannel)
96 : {
97 : ring_node* node;
98 :
99 120 : node = NULL;
100 120 : switch (frequencyChannel)
101 : {
102 : case CHANNELF1:
103 114 : node = ring_node_to_send_cwf_f1;
104 0 : break;
105 : case CHANNELF2:
106 6 : node = ring_node_to_send_cwf_f2;
107 0 : break;
108 : case CHANNELF3:
109 0 : node = ring_node_to_send_cwf_f3;
110 : break;
111 : default:
112 : break;
113 : }
114 :
115 120 : return node;
116 : }
117 :
118 0 : ring_node* getRingNodeToSendSWF(unsigned char frequencyChannel)
119 : {
120 : ring_node* node;
121 :
122 0 : node = NULL;
123 0 : switch (frequencyChannel)
124 : {
125 : case CHANNELF0:
126 0 : node = ring_node_to_send_swf_f0;
127 0 : break;
128 : case CHANNELF1:
129 0 : node = ring_node_to_send_swf_f1;
130 0 : break;
131 : case CHANNELF2:
132 0 : node = ring_node_to_send_swf_f2;
133 : break;
134 : default:
135 : break;
136 : }
137 :
138 0 : return node;
139 : }
140 :
141 7 : void reset_extractSWF(void)
142 : {
143 7 : extractSWF1 = false;
144 7 : extractSWF2 = false;
145 7 : swf0_ready_flag_f1 = false;
146 7 : swf0_ready_flag_f2 = false;
147 7 : swf1_ready = false;
148 7 : swf2_ready = false;
149 7 : }
150 :
151 120 : void waveforms_isr_f3(void)
152 : {
153 120 : if ((lfrCurrentMode == LFR_MODE_NORMAL)
154 : || (lfrCurrentMode
155 : == LFR_MODE_BURST) // in BURST the data are used to place v, e1 and e2 in the HK packet
156 : || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2))
157 : { // in modes other than STANDBY and BURST, send the CWF_F3 data
158 : //***
159 : // F3
160 120 : if ((waveform_picker_regs->status & BITS_WFP_STATUS_F3) != INIT_CHAR)
161 : { // [1100 0000] check the f3 full bits
162 0 : ring_node_to_send_cwf_f3 = current_ring_node_f3->previous;
163 0 : current_ring_node_f3 = current_ring_node_f3->next;
164 0 : if ((waveform_picker_regs->status & BIT_WFP_BUF_F3_0) == BIT_WFP_BUF_F3_0)
165 : { // [0100 0000] f3 buffer 0 is full
166 0 : ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_0_coarse_time;
167 0 : ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_0_fine_time;
168 0 : waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->buffer_address;
169 0 : waveform_picker_regs->status
170 0 : = waveform_picker_regs->status & RST_WFP_F3_0; // [1000 1000 0100 0000]
171 : }
172 0 : else if ((waveform_picker_regs->status & BIT_WFP_BUF_F3_1) == BIT_WFP_BUF_F3_1)
173 : { // [1000 0000] f3 buffer 1 is full
174 0 : ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_1_coarse_time;
175 0 : ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_1_fine_time;
176 0 : waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address;
177 0 : waveform_picker_regs->status
178 0 : = waveform_picker_regs->status & RST_WFP_F3_1; // [1000 1000 1000 0000]
179 : }
180 0 : if (rtems_event_send(Task_id[TASKID_CWF3], RTEMS_EVENT_0) != RTEMS_SUCCESSFUL)
181 : {
182 : DEBUG_CHECK_STATUS(send_event_dumb_task(RTEMS_EVENT_0));
183 : }
184 : }
185 : }
186 120 : }
187 :
188 0 : void waveforms_isr_burst(void)
189 : {
190 : unsigned char status;
191 :
192 0 : status = (waveform_picker_regs->status & BITS_WFP_STATUS_F2)
193 : >> SHIFT_WFP_STATUS_F2; // [0011 0000] get the status bits for f2
194 :
195 0 : switch (status)
196 : {
197 : case BIT_WFP_BUFFER_0:
198 0 : ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
199 0 : ring_node_to_send_cwf_f2->packet_id = SID_BURST_CWF_F2;
200 0 : ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
201 0 : ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
202 0 : current_ring_node_f2 = current_ring_node_f2->next;
203 0 : waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
204 0 : if (rtems_event_send(Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST) != RTEMS_SUCCESSFUL)
205 : {
206 : DEBUG_CHECK_STATUS(send_event_dumb_task(RTEMS_EVENT_0));
207 : }
208 0 : waveform_picker_regs->status
209 0 : = waveform_picker_regs->status & RST_WFP_F2_0; // [0100 0100 0001 0000]
210 0 : break;
211 : case BIT_WFP_BUFFER_1:
212 0 : ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
213 0 : ring_node_to_send_cwf_f2->packet_id = SID_BURST_CWF_F2;
214 0 : ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
215 0 : ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
216 0 : current_ring_node_f2 = current_ring_node_f2->next;
217 0 : waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
218 0 : if (rtems_event_send(Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST) != RTEMS_SUCCESSFUL)
219 : {
220 : DEBUG_CHECK_STATUS(send_event_dumb_task(RTEMS_EVENT_0));
221 : }
222 0 : waveform_picker_regs->status
223 0 : = waveform_picker_regs->status & RST_WFP_F2_1; // [0100 0100 0010 0000]
224 : break;
225 : default:
226 : break;
227 : }
228 0 : }
229 :
230 120 : void waveform_isr_normal_sbm1_sbm2(void)
231 : {
232 : //***
233 : // F0
234 120 : if ((waveform_picker_regs->status & BITS_WFP_STATUS_F0)
235 : != INIT_CHAR) // [0000 0011] check the f0 full bits
236 : {
237 0 : swf0_ready_flag_f1 = true;
238 0 : swf0_ready_flag_f2 = true;
239 0 : ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
240 0 : current_ring_node_f0 = current_ring_node_f0->next;
241 0 : if ((waveform_picker_regs->status & BIT_WFP_BUFFER_0) == BIT_WFP_BUFFER_0)
242 : {
243 :
244 0 : ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
245 0 : ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
246 0 : waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
247 0 : waveform_picker_regs->status
248 0 : = waveform_picker_regs->status & RST_WFP_F0_0; // [0001 0001 0000 0001]
249 : }
250 0 : else if ((waveform_picker_regs->status & BIT_WFP_BUFFER_1) == BIT_WFP_BUFFER_1)
251 : {
252 0 : ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
253 0 : ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
254 0 : waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
255 0 : waveform_picker_regs->status
256 0 : = waveform_picker_regs->status & RST_WFP_F0_1; // [0001 0001 0000 0010]
257 : }
258 : // send an event to the WFRM task for resynchro activities
259 0 : DEBUG_CHECK_STATUS(rtems_event_send(Task_id[TASKID_WFRM], RTEMS_EVENT_SWF_RESYNCH));
260 0 : DEBUG_CHECK_STATUS(rtems_event_send(Task_id[TASKID_CALI], RTEMS_EVENT_CAL_SWEEP_WAKE));
261 : }
262 :
263 : //***
264 : // F1
265 120 : if ((waveform_picker_regs->status & BITS_WFP_STATUS_F1) != INIT_CHAR)
266 : { // [0000 1100] check the f1 full bits
267 : // (1) change the receiving buffer for the waveform picker
268 114 : ring_node_to_send_cwf_f1 = current_ring_node_f1->previous;
269 114 : current_ring_node_f1 = current_ring_node_f1->next;
270 114 : if ((waveform_picker_regs->status & BIT_WFP_BUF_F1_0) == BIT_WFP_BUF_F1_0)
271 : {
272 58 : ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
273 58 : ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
274 58 : waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
275 116 : waveform_picker_regs->status
276 58 : = waveform_picker_regs->status & RST_WFP_F1_0; // [0010 0010 0000 0100] f1 bits = 0
277 : }
278 56 : else if ((waveform_picker_regs->status & BIT_WFP_BUF_F1_1) == BIT_WFP_BUF_F1_1)
279 : {
280 56 : ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
281 56 : ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
282 56 : waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
283 112 : waveform_picker_regs->status
284 56 : = waveform_picker_regs->status & RST_WFP_F1_1; // [0010 0010 0000 1000] f1 bits = 0
285 : }
286 : // (2) send an event for the the CWF1 task for transmission (and snapshot extraction if
287 : // needed)
288 114 : DEBUG_CHECK_STATUS(rtems_event_send(Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_NORM_S1_S2));
289 : }
290 :
291 : //***
292 : // F2
293 120 : if ((waveform_picker_regs->status & BITS_WFP_STATUS_F2) != INIT_CHAR)
294 : { // [0011 0000] check the f2 full bit
295 : // (1) change the receiving buffer for the waveform picker
296 6 : ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
297 6 : ring_node_to_send_cwf_f2->packet_id = SID_SBM2_CWF_F2;
298 6 : current_ring_node_f2 = current_ring_node_f2->next;
299 6 : if ((waveform_picker_regs->status & BIT_WFP_BUF_F2_0) == BIT_WFP_BUF_F2_0)
300 : {
301 3 : ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
302 3 : ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
303 3 : waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
304 6 : waveform_picker_regs->status
305 3 : = waveform_picker_regs->status & RST_WFP_F2_0; // [0100 0100 0001 0000]
306 : }
307 3 : else if ((waveform_picker_regs->status & BIT_WFP_BUF_F2_1) == BIT_WFP_BUF_F2_1)
308 : {
309 3 : ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
310 3 : ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
311 3 : waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
312 6 : waveform_picker_regs->status
313 3 : = waveform_picker_regs->status & RST_WFP_F2_1; // [0100 0100 0010 0000]
314 : }
315 : // (2) send an event for the waveforms transmission
316 6 : DEBUG_CHECK_STATUS(rtems_event_send(Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_NORM_S1_S2));
317 : }
318 120 : }
319 :
320 120 : rtems_isr waveforms_isr(rtems_vector_number vector)
321 : {
322 : /** This is the interrupt sub routine called by the waveform picker core.
323 : *
324 : * This ISR launch different actions depending mainly on two pieces of information:
325 : * 1. the values read in the registers of the waveform picker.
326 : * 2. the current LFR mode.
327 : *
328 : */
329 :
330 : // STATUS
331 : // new error error buffer full
332 : // 15 14 13 12 11 10 9 8
333 : // f3 f2 f1 f0 f3 f2 f1 f0
334 : //
335 : // ready buffer
336 : // 7 6 5 4 3 2 1 0
337 : // f3_1 f3_0 f2_1 f2_0 f1_1 f1_0 f0_1 f0_0
338 : IGNORE_UNUSED_PARAMETER(vector);
339 :
340 120 : waveforms_isr_f3();
341 :
342 : //*************************************************
343 : // copy the status bits in the housekeeping packets
344 120 : housekeeping_packet.hk_lfr_vhdl_iir_cal
345 120 : = (unsigned char)((waveform_picker_regs->status & BYTE0_MASK) >> SHIFT_1_BYTE);
346 :
347 120 : if ((waveform_picker_regs->status & BYTE0_MASK)
348 : != INIT_CHAR) // [1111 1111 0000 0000] check the error bits
349 : {
350 : DEBUG_CHECK_STATUS(send_event_dumb_task(RTEMS_EVENT_10));
351 : }
352 :
353 120 : switch (lfrCurrentMode)
354 : {
355 : //********
356 : // STANDBY
357 : case LFR_MODE_STANDBY:
358 : break;
359 : //**************************
360 : // LFR NORMAL, SBM1 and SBM2
361 : case LFR_MODE_NORMAL:
362 : case LFR_MODE_SBM1:
363 : case LFR_MODE_SBM2:
364 120 : waveform_isr_normal_sbm1_sbm2();
365 120 : break;
366 : //******
367 : // BURST
368 : case LFR_MODE_BURST:
369 0 : waveforms_isr_burst();
370 : break;
371 : //********
372 : // DEFAULT
373 : default:
374 : break;
375 : }
376 120 : }
377 :
378 : //************
379 : // RTEMS TASKS
380 :
381 6 : LFR_NO_RETURN rtems_task wfrm_task(
382 : rtems_task_argument argument) // used with the waveform picker VHDL IP
383 : {
384 : /** This RTEMS task is dedicated to the transmission of snapshots of the NORMAL mode.
385 : *
386 : * @param unused is the starting argument of the RTEMS task
387 : *
388 : * The following data packets are sent by this task:
389 : * - TM_LFR_SCIENCE_NORMAL_SWF_F0
390 : * - TM_LFR_SCIENCE_NORMAL_SWF_F1
391 : * - TM_LFR_SCIENCE_NORMAL_SWF_F2
392 : *
393 : */
394 :
395 : IGNORE_UNUSED_PARAMETER(argument);
396 :
397 6 : rtems_event_set event_out = EVENT_SETS_NONE_PENDING;
398 6 : rtems_id queue_id = RTEMS_ID_NONE;
399 : ring_node* ring_node_swf1_extracted_ptr;
400 : ring_node* ring_node_swf2_extracted_ptr;
401 :
402 6 : ring_node_swf1_extracted_ptr = &ring_node_swf1_extracted;
403 6 : ring_node_swf2_extracted_ptr = &ring_node_swf2_extracted;
404 :
405 6 : DEBUG_CHECK_STATUS(get_message_queue_id_send(&queue_id));
406 :
407 : BOOT_PRINTF("in WFRM ***\n");
408 :
409 : while (1)
410 : {
411 : // wait for an RTEMS_EVENT
412 6 : rtems_event_receive(RTEMS_EVENT_MODE_NORMAL | RTEMS_EVENT_SWF_RESYNCH,
413 : RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
414 :
415 0 : if (event_out == RTEMS_EVENT_MODE_NORMAL)
416 : {
417 : DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM2\n");
418 0 : ring_node_to_send_swf_f0->packet_id = SID_NORM_SWF_F0;
419 0 : ring_node_swf1_extracted_ptr->packet_id = SID_NORM_SWF_F1;
420 0 : ring_node_swf2_extracted_ptr->packet_id = SID_NORM_SWF_F2;
421 0 : DEBUG_CHECK_STATUS(
422 : rtems_message_queue_send(queue_id, &ring_node_to_send_swf_f0, sizeof(ring_node*)));
423 0 : DEBUG_CHECK_STATUS(rtems_message_queue_send(
424 : queue_id, &ring_node_swf1_extracted_ptr, sizeof(ring_node*)));
425 0 : DEBUG_CHECK_STATUS(rtems_message_queue_send(
426 : queue_id, &ring_node_swf2_extracted_ptr, sizeof(ring_node*)));
427 : }
428 0 : if (event_out == RTEMS_EVENT_SWF_RESYNCH)
429 : {
430 0 : snapshot_resynchronization((unsigned char*)&ring_node_to_send_swf_f0->coarseTime);
431 : }
432 : }
433 : }
434 :
435 6 : LFR_NO_RETURN rtems_task cwf3_task(
436 : rtems_task_argument argument) // used with the waveform picker VHDL IP
437 : {
438 : /** This RTEMS task is dedicated to the transmission of continuous waveforms at f3.
439 : *
440 : * @param unused is the starting argument of the RTEMS task
441 : *
442 : * The following data packet is sent by this task:
443 : * - TM_LFR_SCIENCE_NORMAL_CWF_F3
444 : *
445 : */
446 :
447 : IGNORE_UNUSED_PARAMETER(argument);
448 :
449 6 : rtems_event_set event_out = EVENT_SETS_NONE_PENDING;
450 6 : rtems_id queue_id = RTEMS_ID_NONE;
451 : ring_node ring_node_cwf3_light;
452 : ring_node* ring_node_to_send_cwf;
453 :
454 :
455 6 : DEBUG_CHECK_STATUS(get_message_queue_id_send(&queue_id));
456 :
457 6 : ring_node_to_send_cwf_f3->packet_id = SID_NORM_CWF_LONG_F3;
458 :
459 : // init the ring_node_cwf3_light structure
460 6 : ring_node_cwf3_light.buffer_address = wf_cont_f3_light;
461 6 : ring_node_cwf3_light.coarseTime = INIT_CHAR;
462 6 : ring_node_cwf3_light.fineTime = INIT_CHAR;
463 6 : ring_node_cwf3_light.next = NULL;
464 6 : ring_node_cwf3_light.previous = NULL;
465 6 : ring_node_cwf3_light.packet_id = SID_NORM_CWF_F3;
466 6 : ring_node_cwf3_light.status = INIT_CHAR;
467 :
468 : BOOT_PRINTF("in CWF3 ***\n");
469 :
470 : while (1)
471 : {
472 : // wait for an RTEMS_EVENT
473 6 : rtems_event_receive(
474 : RTEMS_EVENT_0, RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
475 0 : if ((lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
476 0 : || (lfrCurrentMode == LFR_MODE_SBM2))
477 : {
478 0 : ring_node_to_send_cwf = getRingNodeToSendCWF(CHANNELF3);
479 0 : if ((parameter_dump_packet.sy_lfr_n_cwf_long_f3 & BIT_CWF_LONG_F3) == BIT_CWF_LONG_F3)
480 : {
481 : LFR_PRINTF("send CWF_LONG_F3\n");
482 0 : ring_node_to_send_cwf_f3->packet_id = SID_NORM_CWF_LONG_F3;
483 0 : DEBUG_CHECK_STATUS(
484 : rtems_message_queue_send(queue_id, &ring_node_to_send_cwf, sizeof(ring_node*)));
485 : }
486 : else
487 : {
488 : LFR_PRINTF("send CWF_F3 (light)\n");
489 0 : send_waveform_CWF3_light(ring_node_to_send_cwf, &ring_node_cwf3_light, queue_id);
490 : }
491 : }
492 : else
493 : {
494 : LFR_PRINTF("in CWF3 *** lfrCurrentMode is %d, no data will be sent\n", lfrCurrentMode);
495 : }
496 : }
497 : }
498 :
499 6 : LFR_NO_RETURN rtems_task cwf2_task(rtems_task_argument argument) // ONLY USED IN BURST AND SBM2
500 : {
501 : /** This RTEMS task is dedicated to the transmission of continuous waveforms at f2.
502 : *
503 : * @param unused is the starting argument of the RTEMS task
504 : *
505 : * The following data packet is sent by this function:
506 : * - TM_LFR_SCIENCE_BURST_CWF_F2
507 : * - TM_LFR_SCIENCE_SBM2_CWF_F2
508 : *
509 : */
510 :
511 : IGNORE_UNUSED_PARAMETER(argument);
512 :
513 6 : rtems_event_set event_out = EVENT_SETS_NONE_PENDING;
514 6 : rtems_id queue_id = RTEMS_ID_NONE;
515 : ring_node* ring_node_to_send;
516 : unsigned long long int acquisitionTimeF0_asLong;
517 :
518 6 : acquisitionTimeF0_asLong = INIT_CHAR;
519 :
520 6 : DEBUG_CHECK_STATUS(get_message_queue_id_send(&queue_id));
521 :
522 : BOOT_PRINTF("in CWF2 ***\n");
523 :
524 : while (1)
525 : {
526 : // wait for an RTEMS_EVENT// send the snapshot when built
527 12 : DEBUG_CHECK_STATUS(rtems_event_send(Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM2));
528 12 : DEBUG_CHECK_STATUS(rtems_event_receive(RTEMS_EVENT_MODE_NORM_S1_S2 | RTEMS_EVENT_MODE_BURST,
529 : RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out));
530 6 : ring_node_to_send = getRingNodeToSendCWF(CHANNELF2);
531 6 : if (event_out == RTEMS_EVENT_MODE_BURST)
532 : { // data are sent whatever the transition time
533 0 : DEBUG_CHECK_STATUS(
534 : rtems_message_queue_send(queue_id, &ring_node_to_send, sizeof(ring_node*)));
535 : }
536 6 : else if (event_out == RTEMS_EVENT_MODE_NORM_S1_S2)
537 : {
538 6 : if (lfrCurrentMode == LFR_MODE_SBM2
539 6 : && time_management_regs->coarse_time >= lastValidEnterModeTime)
540 : {
541 0 : DEBUG_CHECK_STATUS(
542 : rtems_message_queue_send(queue_id, &ring_node_to_send, sizeof(ring_node*)));
543 : }
544 : // launch snapshot extraction if needed
545 6 : if (extractSWF2 == true)
546 : {
547 0 : ring_node_to_send_swf_f2 = ring_node_to_send_cwf_f2;
548 : // extract the snapshot
549 0 : build_snapshot_from_ring(ring_node_to_send_swf_f2, CHANNELF2,
550 : acquisitionTimeF0_asLong, &ring_node_swf2_extracted, swf2_extracted);
551 0 : extractSWF2 = false;
552 0 : swf2_ready = true; // once the snapshot at f2 is ready the CWF1 task will send an
553 : // event to WFRM
554 : }
555 6 : if (swf0_ready_flag_f2 == true)
556 : {
557 0 : extractSWF2 = true;
558 : // record the acquition time of the f0 snapshot to use to build the snapshot at f2
559 0 : acquisitionTimeF0_asLong
560 0 : = get_acquisition_time((unsigned char*)&ring_node_to_send_swf_f0->coarseTime);
561 0 : swf0_ready_flag_f2 = false;
562 : }
563 : }
564 : }
565 : }
566 :
567 6 : LFR_NO_RETURN rtems_task cwf1_task(rtems_task_argument argument) // ONLY USED IN SBM1
568 : {
569 : /** This RTEMS task is dedicated to the transmission of continuous waveforms at f1.
570 : *
571 : * @param unused is the starting argument of the RTEMS task
572 : *
573 : * The following data packet is sent by this function:
574 : * - TM_LFR_SCIENCE_SBM1_CWF_F1
575 : *
576 : */
577 :
578 : IGNORE_UNUSED_PARAMETER(argument);
579 :
580 6 : rtems_event_set event_out = EVENT_SETS_NONE_PENDING;
581 6 : rtems_id queue_id = RTEMS_ID_NONE;
582 :
583 : ring_node* ring_node_to_send_cwf;
584 :
585 6 : DEBUG_CHECK_STATUS(get_message_queue_id_send(&queue_id));
586 :
587 : BOOT_PRINTF("in CWF1 ***\n");
588 :
589 : while (1)
590 : {
591 : // wait for an RTEMS_EVENT
592 120 : rtems_event_receive(RTEMS_EVENT_MODE_NORM_S1_S2, RTEMS_WAIT | RTEMS_EVENT_ANY,
593 : RTEMS_NO_TIMEOUT, &event_out);
594 114 : ring_node_to_send_cwf = getRingNodeToSendCWF(1);
595 114 : ring_node_to_send_cwf_f1->packet_id = SID_SBM1_CWF_F1;
596 179 : if (lfrCurrentMode == LFR_MODE_SBM1
597 179 : && time_management_regs->coarse_time >= lastValidEnterModeTime)
598 : {
599 65 : DEBUG_CHECK_STATUS(
600 : rtems_message_queue_send(queue_id, &ring_node_to_send_cwf, sizeof(ring_node*)));
601 : }
602 : // launch snapshot extraction if needed
603 114 : if (extractSWF1 == true)
604 : {
605 0 : ring_node_to_send_swf_f1 = ring_node_to_send_cwf;
606 : // launch the snapshot extraction
607 0 : DEBUG_CHECK_STATUS(rtems_event_send(Task_id[TASKID_SWBD], RTEMS_EVENT_MODE_NORM_S1_S2));
608 0 : extractSWF1 = false;
609 : }
610 114 : if (swf0_ready_flag_f1 == true)
611 : {
612 0 : extractSWF1 = true;
613 0 : swf0_ready_flag_f1 = false; // this step shall be executed only one time
614 : }
615 114 : if ((swf1_ready == true) && (swf2_ready == true)) // swf_f1 is ready after the extraction
616 : {
617 0 : DEBUG_CHECK_STATUS(rtems_event_send(Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_NORMAL));
618 0 : swf1_ready = false;
619 0 : swf2_ready = false;
620 : }
621 : }
622 : }
623 :
624 1 : LFR_NO_RETURN rtems_task swbd_task(rtems_task_argument argument)
625 : {
626 : /** This RTEMS task is dedicated to the building of snapshots from different continuous
627 : * waveforms buffers.
628 : *
629 : * @param unused is the starting argument of the RTEMS task
630 : *
631 : */
632 :
633 : IGNORE_UNUSED_PARAMETER(argument);
634 :
635 1 : rtems_event_set event_out = EVENT_SETS_NONE_PENDING;
636 1 : unsigned long long int acquisitionTimeF0_asLong = 0;
637 :
638 : BOOT_PRINTF("in SWBD ***\n");
639 :
640 : while (1)
641 : {
642 : // wait for an RTEMS_EVENT
643 1 : rtems_event_receive(RTEMS_EVENT_MODE_NORM_S1_S2, RTEMS_WAIT | RTEMS_EVENT_ANY,
644 : RTEMS_NO_TIMEOUT, &event_out);
645 0 : if (event_out == RTEMS_EVENT_MODE_NORM_S1_S2)
646 : {
647 0 : acquisitionTimeF0_asLong
648 0 : = get_acquisition_time((unsigned char*)&ring_node_to_send_swf_f0->coarseTime);
649 0 : build_snapshot_from_ring(ring_node_to_send_swf_f1, CHANNELF1, acquisitionTimeF0_asLong,
650 : &ring_node_swf1_extracted, swf1_extracted);
651 0 : swf1_ready = true; // the snapshot has been extracted and is ready to be sent
652 : }
653 : else
654 : {
655 : LFR_PRINTF("in SWBD *** unexpected rtems event received %x\n", (int)event_out);
656 : }
657 : }
658 : }
659 :
660 : //******************
661 : // general functions
662 :
663 1 : void WFP_init_rings(void)
664 : {
665 : // F0 RING
666 1 : init_ring(waveform_ring_f0, NB_RING_NODES_F0, wf_buffer_f0, WFRM_BUFFER);
667 : // F1 RING
668 1 : init_ring(waveform_ring_f1, NB_RING_NODES_F1, wf_buffer_f1, WFRM_BUFFER);
669 : // F2 RING
670 1 : init_ring(waveform_ring_f2, NB_RING_NODES_F2, wf_buffer_f2, WFRM_BUFFER);
671 : // F3 RING
672 1 : init_ring(waveform_ring_f3, NB_RING_NODES_F3, wf_buffer_f3, WFRM_BUFFER);
673 :
674 1 : ring_node_swf1_extracted.buffer_address = swf1_extracted;
675 1 : ring_node_swf2_extracted.buffer_address = swf2_extracted;
676 :
677 : DEBUG_PRINTF("waveform_ring_f0 @%x\n", (unsigned int)waveform_ring_f0);
678 : DEBUG_PRINTF("waveform_ring_f1 @%x\n", (unsigned int)waveform_ring_f1);
679 : DEBUG_PRINTF("waveform_ring_f2 @%x\n", (unsigned int)waveform_ring_f2);
680 : DEBUG_PRINTF("waveform_ring_f3 @%x\n", (unsigned int)waveform_ring_f3);
681 : DEBUG_PRINTF("wf_buffer_f0 @%x\n", (unsigned int)wf_buffer_f0);
682 : DEBUG_PRINTF("wf_buffer_f1 @%x\n", (unsigned int)wf_buffer_f1);
683 : DEBUG_PRINTF("wf_buffer_f2 @%x\n", (unsigned int)wf_buffer_f2);
684 : DEBUG_PRINTF("wf_buffer_f3 @%x\n", (unsigned int)wf_buffer_f3);
685 1 : }
686 :
687 7 : void WFP_reset_current_ring_nodes(void)
688 : {
689 7 : current_ring_node_f0 = waveform_ring_f0[0].next;
690 7 : current_ring_node_f1 = waveform_ring_f1[0].next;
691 7 : current_ring_node_f2 = waveform_ring_f2[0].next;
692 7 : current_ring_node_f3 = waveform_ring_f3[0].next;
693 :
694 7 : ring_node_to_send_swf_f0 = waveform_ring_f0;
695 7 : ring_node_to_send_swf_f1 = waveform_ring_f1;
696 7 : ring_node_to_send_swf_f2 = waveform_ring_f2;
697 :
698 7 : ring_node_to_send_cwf_f1 = waveform_ring_f1;
699 7 : ring_node_to_send_cwf_f2 = waveform_ring_f2;
700 7 : ring_node_to_send_cwf_f3 = waveform_ring_f3;
701 7 : }
702 :
703 0 : int send_waveform_CWF3_light(
704 : ring_node* ring_node_to_send, ring_node* ring_node_cwf3_light, rtems_id queue_id)
705 : {
706 : /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
707 : *
708 : * @param waveform points to the buffer containing the data that will be send.
709 : * @param headerCWF points to a table of headers that have been prepared for the data
710 : * transmission.
711 : * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be
712 : * send. The structures contain information to setup the transmission of the data packets.
713 : *
714 : * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a
715 : * data buffer from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1
716 : * one containing 8 blocks.
717 : *
718 : */
719 :
720 0 : int ret = LFR_SUCCESSFUL;
721 : rtems_status_code status;
722 :
723 : const char* sample;
724 0 : const int* const dataPtr = (int*)ring_node_to_send->buffer_address;
725 :
726 0 : ring_node_cwf3_light->coarseTime = ring_node_to_send->coarseTime;
727 0 : ring_node_cwf3_light->fineTime = ring_node_to_send->fineTime;
728 :
729 : //**********************
730 : // BUILD CWF3_light DATA
731 0 : for (unsigned int i = 0; i < NB_SAMPLES_PER_SNAPSHOT; i++)
732 : {
733 0 : sample = (const char*)&dataPtr[i * NB_WORDS_SWF_BLK];
734 0 : for (unsigned int j = 0; j < CWF_BLK_SIZE; j++)
735 : {
736 0 : wf_cont_f3_light[(i * NB_BYTES_CWF3_LIGHT_BLK) + j] = sample[j];
737 : }
738 : }
739 :
740 : // SEND PACKET
741 0 : status = rtems_message_queue_send(queue_id, &ring_node_cwf3_light, sizeof(ring_node*));
742 0 : if (status != RTEMS_SUCCESSFUL)
743 : {
744 0 : ret = LFR_DEFAULT;
745 : }
746 :
747 0 : return ret;
748 : }
749 :
750 520 : void compute_acquisition_time(unsigned int coarseTime, unsigned int fineTime, unsigned int sid,
751 : unsigned char pa_lfr_pkt_nr, unsigned char* acquisitionTime)
752 : {
753 : unsigned long long int acquisitionTimeAsLong;
754 : unsigned char localAcquisitionTime[BYTES_PER_TIME];
755 520 : double deltaT = 0.;
756 :
757 520 : localAcquisitionTime[BYTE_0] = (unsigned char)(coarseTime >> SHIFT_3_BYTES);
758 520 : localAcquisitionTime[BYTE_1] = (unsigned char)(coarseTime >> SHIFT_2_BYTES);
759 520 : localAcquisitionTime[BYTE_2] = (unsigned char)(coarseTime >> SHIFT_1_BYTE);
760 520 : localAcquisitionTime[BYTE_3] = (unsigned char)(coarseTime);
761 520 : localAcquisitionTime[BYTE_4] = (unsigned char)(fineTime >> SHIFT_1_BYTE);
762 520 : localAcquisitionTime[BYTE_5] = (unsigned char)(fineTime);
763 :
764 1040 : acquisitionTimeAsLong = ((unsigned long long int)localAcquisitionTime[BYTE_0] << SHIFT_5_BYTES)
765 520 : + ((unsigned long long int)localAcquisitionTime[BYTE_1] << SHIFT_4_BYTES)
766 520 : + ((unsigned long long int)localAcquisitionTime[BYTE_2] << SHIFT_3_BYTES)
767 520 : + ((unsigned long long int)localAcquisitionTime[BYTE_3] << SHIFT_2_BYTES)
768 520 : + ((unsigned long long int)localAcquisitionTime[BYTE_4] << SHIFT_1_BYTE)
769 520 : + ((unsigned long long int)localAcquisitionTime[BYTE_5]);
770 :
771 520 : switch (sid)
772 : {
773 : case SID_NORM_SWF_F0:
774 0 : deltaT = ((double)(pa_lfr_pkt_nr)) * BLK_NR_304 * T0_IN_FINETIME;
775 0 : break;
776 :
777 : case SID_NORM_SWF_F1:
778 0 : deltaT = ((double)(pa_lfr_pkt_nr)) * BLK_NR_304 * T1_IN_FINETIME;
779 0 : break;
780 :
781 : case SID_NORM_SWF_F2:
782 0 : deltaT = ((double)(pa_lfr_pkt_nr)) * BLK_NR_304 * T2_IN_FINETIME;
783 0 : break;
784 :
785 : case SID_SBM1_CWF_F1:
786 520 : deltaT = ((double)(pa_lfr_pkt_nr)) * BLK_NR_CWF * T1_IN_FINETIME;
787 520 : break;
788 :
789 : case SID_SBM2_CWF_F2:
790 0 : deltaT = ((double)(pa_lfr_pkt_nr)) * BLK_NR_CWF * T2_IN_FINETIME;
791 0 : break;
792 :
793 : case SID_BURST_CWF_F2:
794 0 : deltaT = ((double)(pa_lfr_pkt_nr)) * BLK_NR_CWF * T2_IN_FINETIME;
795 0 : break;
796 :
797 : case SID_NORM_CWF_F3:
798 0 : deltaT = ((double)(pa_lfr_pkt_nr)) * BLK_NR_CWF_SHORT_F3 * T3_IN_FINETIME;
799 0 : break;
800 :
801 : case SID_NORM_CWF_LONG_F3:
802 0 : deltaT = ((double)(pa_lfr_pkt_nr)) * BLK_NR_CWF * T3_IN_FINETIME;
803 0 : break;
804 :
805 : default:
806 : LFR_PRINTF("in compute_acquisition_time *** ERR unexpected sid %d\n", sid);
807 0 : deltaT = 0.;
808 : break;
809 : }
810 :
811 520 : acquisitionTimeAsLong = acquisitionTimeAsLong + (unsigned long long int)deltaT;
812 : //
813 520 : acquisitionTime[BYTE_0] = (unsigned char)(acquisitionTimeAsLong >> SHIFT_5_BYTES);
814 520 : acquisitionTime[BYTE_1] = (unsigned char)(acquisitionTimeAsLong >> SHIFT_4_BYTES);
815 520 : acquisitionTime[BYTE_2] = (unsigned char)(acquisitionTimeAsLong >> SHIFT_3_BYTES);
816 520 : acquisitionTime[BYTE_3] = (unsigned char)(acquisitionTimeAsLong >> SHIFT_2_BYTES);
817 520 : acquisitionTime[BYTE_4] = (unsigned char)(acquisitionTimeAsLong >> SHIFT_1_BYTE);
818 520 : acquisitionTime[BYTE_5] = (unsigned char)(acquisitionTimeAsLong);
819 520 : }
820 :
821 0 : void build_snapshot_from_ring(ring_node* ring_node_to_send, unsigned char frequencyChannel,
822 : unsigned long long int acquisitionTimeF0_asLong, ring_node* ring_node_swf_extracted,
823 : int* swf_extracted)
824 : {
825 : unsigned int i;
826 : unsigned int node;
827 : unsigned long long int centerTime_asLong;
828 : unsigned long long int acquisitionTime_asLong;
829 : unsigned long long int bufferAcquisitionTime_asLong;
830 : const unsigned char* ptr1;
831 : unsigned char* ptr2;
832 : const unsigned char* timeCharPtr;
833 : unsigned char nb_ring_nodes;
834 : unsigned long long int frequency_asLong;
835 : // set to default value (Don_Initialisation_P2)
836 0 : unsigned long long int nbTicksPerSample_asLong = TICKS_PER_T2;
837 : long long int nbSamplesPart1_asLong;
838 0 : unsigned long long int sampleOffset_asLong = 0;
839 :
840 0 : unsigned int deltaT_F0 = DELTAT_F0;
841 0 : unsigned int deltaT_F1 = DELTAT_F1;
842 0 : unsigned long long int deltaT_F2 = DELTAT_F2;
843 :
844 : // (1) get the f0 acquisition time => the value is passed in argument
845 :
846 : // (2) compute the central reference time
847 0 : centerTime_asLong = acquisitionTimeF0_asLong + deltaT_F0;
848 0 : acquisitionTime_asLong = centerTime_asLong; // set to default value (Don_Initialisation_P2)
849 0 : bufferAcquisitionTime_asLong = centerTime_asLong; // set to default value
850 : // (Don_Initialisation_P2)
851 :
852 : // (3) compute the acquisition time of the current snapshot
853 0 : switch (frequencyChannel)
854 : {
855 : case CHANNELF1: // 1 is for F1 = 4096 Hz
856 0 : acquisitionTime_asLong = centerTime_asLong - deltaT_F1;
857 0 : nb_ring_nodes = NB_RING_NODES_F1;
858 0 : frequency_asLong = FREQ_F1;
859 0 : nbTicksPerSample_asLong = TICKS_PER_T1; // 65536 / 4096;
860 0 : break;
861 : case CHANNELF2: // 2 is for F2 = 256 Hz
862 0 : acquisitionTime_asLong = centerTime_asLong - deltaT_F2;
863 0 : nb_ring_nodes = NB_RING_NODES_F2;
864 0 : frequency_asLong = FREQ_F2;
865 0 : nbTicksPerSample_asLong = TICKS_PER_T2; // 65536 / 256;
866 0 : break;
867 : default:
868 0 : acquisitionTime_asLong = centerTime_asLong;
869 0 : nb_ring_nodes = 0;
870 0 : frequency_asLong = FREQ_F2;
871 0 : nbTicksPerSample_asLong = TICKS_PER_T2;
872 : break;
873 : }
874 :
875 : //*****************************************************************************
876 : // (4) search the ring_node with the acquisition time <= acquisitionTime_asLong
877 0 : node = 0;
878 0 : while (node < nb_ring_nodes)
879 : {
880 0 : bufferAcquisitionTime_asLong
881 0 : = get_acquisition_time((unsigned char*)&ring_node_to_send->coarseTime);
882 0 : if (bufferAcquisitionTime_asLong <= acquisitionTime_asLong)
883 : {
884 0 : node = nb_ring_nodes;
885 : }
886 : else
887 : {
888 0 : node = node + 1;
889 0 : ring_node_to_send = ring_node_to_send->previous;
890 : }
891 : }
892 :
893 : // (5) compute the number of samples to take in the current buffer
894 0 : sampleOffset_asLong
895 0 : = ((acquisitionTime_asLong - bufferAcquisitionTime_asLong) * frequency_asLong)
896 : >> SHIFT_2_BYTES;
897 0 : nbSamplesPart1_asLong = NB_SAMPLES_PER_SNAPSHOT - sampleOffset_asLong;
898 :
899 : // (6) compute the final acquisition time
900 0 : acquisitionTime_asLong
901 0 : = bufferAcquisitionTime_asLong + (sampleOffset_asLong * nbTicksPerSample_asLong);
902 :
903 : // (7) copy the acquisition time at the beginning of the extrated snapshot
904 0 : ptr1 = (unsigned char*)&acquisitionTime_asLong;
905 : // fine time
906 0 : ptr2 = (unsigned char*)&ring_node_swf_extracted->fineTime;
907 0 : ptr2[BYTE_2] = ptr1[BYTE_4 + OFFSET_2_BYTES];
908 0 : ptr2[BYTE_3] = ptr1[BYTE_5 + OFFSET_2_BYTES];
909 : // coarse time
910 0 : ptr2 = (unsigned char*)&ring_node_swf_extracted->coarseTime;
911 0 : ptr2[BYTE_0] = ptr1[BYTE_0 + OFFSET_2_BYTES];
912 0 : ptr2[BYTE_1] = ptr1[BYTE_1 + OFFSET_2_BYTES];
913 0 : ptr2[BYTE_2] = ptr1[BYTE_2 + OFFSET_2_BYTES];
914 0 : ptr2[BYTE_3] = ptr1[BYTE_3 + OFFSET_2_BYTES];
915 :
916 : // re set the synchronization bit
917 0 : timeCharPtr = (unsigned char*)&ring_node_to_send->coarseTime;
918 0 : ptr2[0] = ptr2[0] | (timeCharPtr[0] & SYNC_BIT); // [1000 0000]
919 :
920 0 : if ((nbSamplesPart1_asLong > NB_SAMPLES_PER_SNAPSHOT) | (nbSamplesPart1_asLong < 0))
921 : {
922 0 : nbSamplesPart1_asLong = 0;
923 : }
924 : // copy the part 1 of the snapshot in the extracted buffer
925 0 : for (i = 0; i < (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i++)
926 : {
927 0 : swf_extracted[i] = ((
928 0 : int*)ring_node_to_send->buffer_address)[i + (sampleOffset_asLong * NB_WORDS_SWF_BLK)];
929 : }
930 : // copy the part 2 of the snapshot in the extracted buffer
931 0 : ring_node_to_send = ring_node_to_send->next;
932 0 : for (i = (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK);
933 0 : i < (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK); i++)
934 : {
935 0 : swf_extracted[i] = ((
936 0 : int*)ring_node_to_send->buffer_address)[i - (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK)];
937 : }
938 0 : }
939 :
940 0 : double computeCorrection(const unsigned char* const timePtr)
941 : {
942 : unsigned long long int acquisitionTime;
943 : unsigned long long int centerTime;
944 : unsigned long long int previousTick;
945 : unsigned long long int nextTick;
946 : unsigned long long int deltaPreviousTick;
947 : unsigned long long int deltaNextTick;
948 : double deltaPrevious_ms;
949 : double deltaNext_ms;
950 0 : double correctionInF2 = 0.; // set to default value (Don_Initialisation_P2)
951 :
952 :
953 : // get acquisition time in fine time ticks
954 0 : acquisitionTime = get_acquisition_time(timePtr);
955 :
956 : // compute center time
957 0 : centerTime = acquisitionTime + DELTAT_F0; // (2048. / 24576. / 2.) * 65536. = 2730.667;
958 0 : previousTick = centerTime - (centerTime & INT16_ALL_F);
959 0 : nextTick = previousTick + (unsigned long long)TICKS_PER_S;
960 :
961 0 : deltaPreviousTick = centerTime - previousTick;
962 0 : deltaNextTick = nextTick - centerTime;
963 :
964 0 : deltaPrevious_ms = (((double)deltaPreviousTick) / TICKS_PER_S) * MS_PER_S;
965 0 : deltaNext_ms = (((double)deltaNextTick) / TICKS_PER_S) * MS_PER_S;
966 :
967 : LFR_PRINTF(
968 : " delta previous = %.3f ms, delta next = %.2f ms\n", deltaPrevious_ms, deltaNext_ms);
969 :
970 : // which tick is the closest?
971 0 : if (deltaPreviousTick > deltaNextTick)
972 : {
973 : // the snapshot center is just before the second => increase delta_snapshot
974 0 : correctionInF2 = +(deltaNext_ms * FREQ_F2 / MS_PER_S);
975 : }
976 : else
977 : {
978 : // the snapshot center is just after the second => decrease delta_snapshot
979 0 : correctionInF2 = -(deltaPrevious_ms * FREQ_F2 / MS_PER_S);
980 : }
981 :
982 : LFR_PRINTF(" correctionInF2 = %.2f\n", correctionInF2);
983 :
984 0 : return correctionInF2;
985 : }
986 :
987 0 : void applyCorrection(double correction)
988 : {
989 0 : int correctionInt = 0;
990 :
991 0 : if (correction >= 0.)
992 : {
993 0 : if ((ONE_TICK_CORR_INTERVAL_0_MIN < correction)
994 0 : && (correction < ONE_TICK_CORR_INTERVAL_0_MAX))
995 : {
996 0 : correctionInt = ONE_TICK_CORR;
997 : }
998 : else
999 : {
1000 0 : correctionInt = (int)((double)CORR_MULT * floor(correction));
1001 : }
1002 : }
1003 : else
1004 : {
1005 0 : if ((ONE_TICK_CORR_INTERVAL_1_MIN < correction)
1006 0 : && (correction < ONE_TICK_CORR_INTERVAL_1_MAX))
1007 : {
1008 0 : correctionInt = -ONE_TICK_CORR;
1009 : }
1010 : else
1011 : {
1012 0 : correctionInt = (int)((double)CORR_MULT * ceil(correction));
1013 : }
1014 : }
1015 0 : waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot + correctionInt;
1016 0 : }
1017 :
1018 0 : void snapshot_resynchronization(const unsigned char* const timePtr)
1019 : {
1020 : /** This function compute a correction to apply on delta_snapshot.
1021 : *
1022 : *
1023 : * @param timePtr is a pointer to the acquisition time of the snapshot being considered.
1024 : *
1025 : * @return void
1026 : *
1027 : */
1028 :
1029 : static double correction = 0.;
1030 : static resynchro_state state = MEASURE;
1031 : static unsigned int nbSnapshots = 0;
1032 :
1033 0 : switch (state)
1034 : {
1035 :
1036 : case MEASURE:
1037 : // ********
1038 : LFR_PRINTF("MEASURE === %d\n", nbSnapshots);
1039 0 : state = CORRECTION;
1040 0 : correction = computeCorrection(timePtr);
1041 : LFR_PRINTF("MEASURE === correction = %.2f\n", correction);
1042 0 : applyCorrection(correction);
1043 : LFR_PRINTF("MEASURE === delta_snapshot = %u\n",
1044 : (unsigned int)waveform_picker_regs->delta_snapshot);
1045 : //****
1046 0 : break;
1047 :
1048 : case CORRECTION:
1049 : //************
1050 : LFR_PRINTF("CORRECTION === %u\n", nbSnapshots);
1051 0 : state = MEASURE;
1052 0 : computeCorrection(timePtr);
1053 : set_wfp_delta_snapshot();
1054 : LFR_PRINTF("CORRECTION === delta_snapshot = %u\n",
1055 : (unsigned int)waveform_picker_regs->delta_snapshot);
1056 : //****
1057 : break;
1058 :
1059 : default:
1060 : break;
1061 : }
1062 :
1063 0 : nbSnapshots++;
1064 0 : }
1065 :
1066 : //**************
1067 : // wfp registers
1068 7 : void reset_wfp_burst_enable(void)
1069 : {
1070 : /** This function resets the waveform picker burst_enable register.
1071 : *
1072 : * The burst bits [f2 f1 f0] and the enable bits [f3 f2 f1 f0] are set to 0.
1073 : *
1074 : */
1075 :
1076 : // [1000 000] burst f2, f1, f0 enable f3, f2, f1, f0
1077 28 : waveform_picker_regs->run_burst_enable
1078 14 : = waveform_picker_regs->run_burst_enable & RST_BITS_RUN_BURST_EN;
1079 7 : }
1080 :
1081 7 : void reset_wfp_status(void)
1082 : {
1083 : /** This function resets the waveform picker status register.
1084 : *
1085 : * All status bits are set to 0 [new_err full_err full].
1086 : *
1087 : */
1088 :
1089 14 : waveform_picker_regs->status = INT16_ALL_F;
1090 7 : }
1091 :
1092 0 : void reset_wfp_buffer_addresses(void)
1093 : {
1094 : // F0
1095 7 : waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->previous->buffer_address; // 0x08
1096 7 : waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address; // 0x0c
1097 : // F1
1098 7 : waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->previous->buffer_address; // 0x10
1099 7 : waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address; // 0x14
1100 : // F2
1101 7 : waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->previous->buffer_address; // 0x18
1102 7 : waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; // 0x1c
1103 : // F3
1104 7 : waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->previous->buffer_address; // 0x20
1105 7 : waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address; // 0x24
1106 0 : }
1107 :
1108 7 : void reset_waveform_picker_regs(void)
1109 : {
1110 : /** This function resets the waveform picker module registers.
1111 : *
1112 : * The registers affected by this function are located at the following offset addresses:
1113 : * - 0x00 data_shaping
1114 : * - 0x04 run_burst_enable
1115 : * - 0x08 addr_data_f0
1116 : * - 0x0C addr_data_f1
1117 : * - 0x10 addr_data_f2
1118 : * - 0x14 addr_data_f3
1119 : * - 0x18 status
1120 : * - 0x1C delta_snapshot
1121 : * - 0x20 delta_f0
1122 : * - 0x24 delta_f0_2
1123 : * - 0x28 delta_f1 (obsolet parameter)
1124 : * - 0x2c delta_f2
1125 : * - 0x30 nb_data_by_buffer
1126 : * - 0x34 nb_snapshot_param
1127 : * - 0x38 start_date
1128 : * - 0x3c nb_word_in_buffer
1129 : *
1130 : */
1131 :
1132 7 : set_wfp_data_shaping(); // 0x00 *** R1 R0 SP1 SP0 BW
1133 :
1134 : reset_wfp_burst_enable(); // 0x04 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
1135 :
1136 : reset_wfp_buffer_addresses();
1137 :
1138 : reset_wfp_status(); // 0x18
1139 :
1140 : set_wfp_delta_snapshot(); // 0x1c *** 300 s => 0x12bff
1141 :
1142 7 : set_wfp_delta_f0_f0_2(); // 0x20, 0x24
1143 :
1144 : // the parameter delta_f1 [0x28] is not used anymore
1145 :
1146 7 : set_wfp_delta_f2(); // 0x2c
1147 :
1148 : DEBUG_PRINTF("delta_snapshot %x\n", waveform_picker_regs->delta_snapshot);
1149 : DEBUG_PRINTF("delta_f0 %x\n", waveform_picker_regs->delta_f0);
1150 : DEBUG_PRINTF("delta_f0_2 %x\n", waveform_picker_regs->delta_f0_2);
1151 : DEBUG_PRINTF("delta_f1 %x\n", waveform_picker_regs->delta_f1);
1152 : DEBUG_PRINTF("delta_f2 %x\n", waveform_picker_regs->delta_f2);
1153 : // 2688 = 8 * 336
1154 7 : waveform_picker_regs->nb_data_by_buffer
1155 : = DFLT_WFP_NB_DATA_BY_BUFFER; // 0x30 *** 2688 - 1 => nb samples -1
1156 7 : waveform_picker_regs->snapshot_param = DFLT_WFP_SNAPSHOT_PARAM; // 0x34 *** 2688 => nb samples
1157 7 : waveform_picker_regs->start_date = COARSE_TIME_MASK;
1158 : //
1159 : // coarse time and fine time registers are not initialized, they are volatile
1160 : //
1161 7 : waveform_picker_regs->buffer_length
1162 : = DFLT_WFP_BUFFER_LENGTH; // buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
1163 7 : }
1164 :
1165 18 : void set_wfp_data_shaping(void)
1166 : {
1167 : /** This function sets the data_shaping register of the waveform picker module.
1168 : *
1169 : * The value is read from one field of the parameter_dump_packet structure:\n
1170 : * bw_sp0_sp1_r0_r1
1171 : *
1172 : */
1173 :
1174 : unsigned char data_shaping;
1175 :
1176 : // get the parameters for the data shaping [BW SP0 SP1 R0 R1] in sy_lfr_common1 and configure
1177 : // the register waveform picker : [R1 R0 SP1 SP0 BW]
1178 :
1179 18 : data_shaping = parameter_dump_packet.sy_lfr_common_parameters;
1180 :
1181 36 : waveform_picker_regs->data_shaping = ((data_shaping & BIT_5) >> SHIFT_5_BITS) // BW
1182 18 : + ((data_shaping & BIT_4) >> SHIFT_3_BITS) // SP0
1183 18 : + ((data_shaping & BIT_3) >> 1) // SP1
1184 18 : + ((data_shaping & BIT_2) << 1) // R0
1185 18 : + ((data_shaping & BIT_1) << SHIFT_3_BITS) // R1
1186 18 : + ((data_shaping & BIT_0) << SHIFT_5_BITS); // R2
1187 18 : }
1188 :
1189 6 : void set_wfp_burst_enable_register(unsigned char mode)
1190 : {
1191 : /** This function sets the waveform picker burst_enable register depending on the mode.
1192 : *
1193 : * @param mode is the LFR mode to launch.
1194 : *
1195 : * The burst bits shall be before the enable bits.
1196 : *
1197 : */
1198 :
1199 : // [0000 0000] burst f2, f1, f0 enable f3 f2 f1 f0
1200 : // the burst bits shall be set first, before the enable bits
1201 6 : switch (mode)
1202 : {
1203 : case LFR_MODE_NORMAL:
1204 : case LFR_MODE_SBM1:
1205 : case LFR_MODE_SBM2:
1206 3 : waveform_picker_regs->run_burst_enable
1207 : = RUN_BURST_ENABLE_SBM2; // [0110 0000] enable f2 and f1 burst
1208 3 : waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable
1209 : | BITS_WFP_ENABLE_ALL; // [1111] enable f3 f2 f1 f0
1210 3 : break;
1211 : case LFR_MODE_BURST:
1212 3 : waveform_picker_regs->run_burst_enable
1213 : = RUN_BURST_ENABLE_BURST; // [0100 0000] f2 burst enabled
1214 3 : waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable
1215 : | BITS_WFP_ENABLE_BURST; // [1100] enable f3 and f2
1216 3 : break;
1217 : default:
1218 0 : waveform_picker_regs->run_burst_enable
1219 : = INIT_CHAR; // [0000 0000] no burst enabled, no waveform enabled
1220 : break;
1221 : }
1222 6 : }
1223 :
1224 0 : void set_wfp_delta_snapshot(void)
1225 : {
1226 : /** This function sets the delta_snapshot register of the waveform picker module.
1227 : *
1228 : * The value is read from two (unsigned char) of the parameter_dump_packet structure:
1229 : * - sy_lfr_n_swf_p[0]
1230 : * - sy_lfr_n_swf_p[1]
1231 : *
1232 : */
1233 :
1234 : unsigned int delta_snapshot;
1235 : unsigned int delta_snapshot_in_T2;
1236 :
1237 14 : delta_snapshot = (parameter_dump_packet.sy_lfr_n_swf_p[0] * CONST_256)
1238 7 : + parameter_dump_packet.sy_lfr_n_swf_p[1];
1239 :
1240 7 : delta_snapshot_in_T2 = delta_snapshot * (unsigned int)FREQ_F2;
1241 7 : waveform_picker_regs->delta_snapshot = delta_snapshot_in_T2 - 1; // max 4 bytes
1242 0 : }
1243 :
1244 7 : void set_wfp_delta_f0_f0_2(void)
1245 : {
1246 : unsigned int delta_snapshot;
1247 : unsigned int nb_samples_per_snapshot;
1248 : float delta_f0_in_float;
1249 :
1250 7 : delta_snapshot = waveform_picker_regs->delta_snapshot;
1251 14 : nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256)
1252 7 : + parameter_dump_packet.sy_lfr_n_swf_l[1];
1253 14 : delta_f0_in_float = (float)(((double)nb_samples_per_snapshot / 2.)
1254 7 : * ((1. / FREQ_F2) - (1. / FREQ_F0)) * FREQ_F2);
1255 :
1256 7 : waveform_picker_regs->delta_f0 = delta_snapshot - (int)floor(delta_f0_in_float);
1257 7 : waveform_picker_regs->delta_f0_2 = DFLT_WFP_DELTA_F0_2;
1258 7 : }
1259 :
1260 0 : void set_wfp_delta_f1(void)
1261 : {
1262 : /** Sets the value of the delta_f1 parameter
1263 : *
1264 : * @param void
1265 : *
1266 : * @return void
1267 : *
1268 : * delta_f1 is not used, the snapshots are extracted from CWF_F1 waveforms.
1269 : *
1270 : */
1271 :
1272 : unsigned int delta_snapshot;
1273 : unsigned int nb_samples_per_snapshot;
1274 : float delta_f1_in_float;
1275 :
1276 0 : delta_snapshot = waveform_picker_regs->delta_snapshot;
1277 0 : nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256)
1278 0 : + parameter_dump_packet.sy_lfr_n_swf_l[1];
1279 0 : delta_f1_in_float = (float)(((double)nb_samples_per_snapshot / 2.)
1280 0 : * ((1. / FREQ_F2) - (1. / FREQ_F1)) * FREQ_F2);
1281 :
1282 0 : waveform_picker_regs->delta_f1 = delta_snapshot - (int)floor(delta_f1_in_float);
1283 0 : }
1284 :
1285 7 : void set_wfp_delta_f2(void) // parameter not used, only delta_f0 and delta_f0_2 are used
1286 : {
1287 : /** Sets the value of the delta_f2 parameter
1288 : *
1289 : * @param void
1290 : *
1291 : * @return void
1292 : *
1293 : * delta_f2 is used only for the first snapshot generation, even when the snapshots are
1294 : * extracted from CWF_F2 waveforms (see lpp_waveform_snapshot_controler.vhd for details).
1295 : *
1296 : */
1297 :
1298 : unsigned int delta_snapshot;
1299 : unsigned int nb_samples_per_snapshot;
1300 :
1301 7 : delta_snapshot = waveform_picker_regs->delta_snapshot;
1302 14 : nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256)
1303 7 : + parameter_dump_packet.sy_lfr_n_swf_l[1];
1304 :
1305 7 : waveform_picker_regs->delta_f2 = delta_snapshot - (nb_samples_per_snapshot / 2) - 1;
1306 7 : }
1307 :
1308 : //*****************
1309 : // local parameters
1310 :
1311 790 : void increment_seq_counter_source_id(unsigned char* packet_sequence_control, unsigned char sid)
1312 : {
1313 : /** This function increments the parameter "sequence_cnt" depending on the sid passed in
1314 : * argument.
1315 : *
1316 : * @param packet_sequence_control is a pointer toward the parameter sequence_cnt to update.
1317 : * @param sid is the source identifier of the packet being updated.
1318 : *
1319 : * REQ-LFR-SRS-5240 / SSS-CP-FS-590
1320 : * The sequence counters shall wrap around from 2^14 to zero.
1321 : * The sequence counter shall start at zero at startup.
1322 : *
1323 : * REQ-LFR-SRS-5239 / SSS-CP-FS-580
1324 : * All TM_LFR_SCIENCE_ packets are sent to ground, i.e. destination id = 0
1325 : *
1326 : */
1327 :
1328 790 : unsigned short* sequence_cnt = NULL;
1329 : unsigned short segmentation_grouping_flag;
1330 : unsigned short new_packet_sequence_control;
1331 790 : rtems_mode initial_mode_set = RTEMS_DEFAULT_MODES;
1332 790 : rtems_mode current_mode_set = RTEMS_DEFAULT_MODES;
1333 :
1334 : //******************************************
1335 : // CHANGE THE MODE OF THE CALLING RTEMS TASK
1336 790 : DEBUG_CHECK_STATUS(rtems_task_mode(RTEMS_NO_PREEMPT, RTEMS_PREEMPT_MASK, &initial_mode_set));
1337 :
1338 6195 : if ((sid == SID_NORM_SWF_F0) || (sid == SID_NORM_SWF_F1) || (sid == SID_NORM_SWF_F2)
1339 1580 : || (sid == SID_NORM_CWF_F3) || (sid == SID_NORM_CWF_LONG_F3) || (sid == SID_BURST_CWF_F2)
1340 1580 : || (sid == SID_NORM_ASM_F0) || (sid == SID_NORM_ASM_F1) || (sid == SID_NORM_ASM_F2)
1341 1563 : || (sid == SID_NORM_BP1_F0) || (sid == SID_NORM_BP1_F1) || (sid == SID_NORM_BP1_F2)
1342 1472 : || (sid == SID_NORM_BP2_F0) || (sid == SID_NORM_BP2_F1) || (sid == SID_NORM_BP2_F2)
1343 1463 : || (sid == SID_BURST_BP1_F0) || (sid == SID_BURST_BP2_F0) || (sid == SID_BURST_BP1_F1)
1344 : || (sid == SID_BURST_BP2_F1))
1345 : {
1346 60 : sequence_cnt = &sequenceCounters_SCIENCE_NORMAL_BURST;
1347 : }
1348 2234 : else if ((sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) || (sid == SID_SBM1_BP1_F0)
1349 774 : || (sid == SID_SBM1_BP2_F0) || (sid == SID_SBM2_BP1_F0) || (sid == SID_SBM2_BP2_F0)
1350 1 : || (sid == SID_SBM2_BP1_F1) || (sid == SID_SBM2_BP2_F1))
1351 : {
1352 730 : sequence_cnt = &sequenceCounters_SCIENCE_SBM1_SBM2;
1353 : }
1354 : else
1355 : {
1356 0 : sequence_cnt = (unsigned short*)NULL;
1357 : LFR_PRINTF(
1358 : "in increment_seq_counter_source_id *** ERR apid_destid %d not known\n", (int)sid);
1359 : }
1360 :
1361 790 : if (sequence_cnt != NULL)
1362 : {
1363 790 : segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
1364 790 : *sequence_cnt = (*sequence_cnt) & SEQ_CNT_MASK;
1365 :
1366 790 : new_packet_sequence_control = segmentation_grouping_flag | (*sequence_cnt);
1367 :
1368 790 : packet_sequence_control[0] = (unsigned char)(new_packet_sequence_control >> SHIFT_1_BYTE);
1369 790 : packet_sequence_control[1] = (unsigned char)(new_packet_sequence_control);
1370 :
1371 : // increment the sequence counter
1372 790 : if (*sequence_cnt < SEQ_CNT_MAX)
1373 : {
1374 790 : *sequence_cnt = *sequence_cnt + 1;
1375 : }
1376 : else
1377 : {
1378 0 : *sequence_cnt = 0;
1379 : }
1380 : }
1381 :
1382 : //*************************************
1383 : // RESTORE THE MODE OF THE CALLING TASK
1384 790 : DEBUG_CHECK_STATUS(rtems_task_mode(initial_mode_set, RTEMS_PREEMPT_MASK, ¤t_mode_set));
1385 790 : }
|