GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/processing/fsw_processing.c Lines: 36 355 10.1 %
Date: 2018-10-22 12:27:55 Branches: 4 96 4.2 %

Line Branch Exec Source
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 related to data processing.
26
 *
27
 * @file
28
 * @author P. LEROY
29
 *
30
 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
31
 *
32
 */
33
34
#include "fsw_processing.h"
35
#include "fsw_processing_globals.c"
36
#include "fsw_init.h"
37
38
unsigned int nb_sm_f0       = 0;
39
unsigned int nb_sm_f0_aux_f1= 0;
40
unsigned int nb_sm_f1       = 0;
41
unsigned int nb_sm_f0_aux_f2= 0;
42
43
typedef enum restartState_t
44
{
45
    WAIT_FOR_F2,
46
    WAIT_FOR_F1,
47
    WAIT_FOR_F0
48
} restartState;
49
50
//************************
51
// spectral matrices rings
52
ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ] = {0};
53
ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ] = {0};
54
ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ] = {0};
55
ring_node *current_ring_node_sm_f0      = NULL;
56
ring_node *current_ring_node_sm_f1      = NULL;
57
ring_node *current_ring_node_sm_f2      = NULL;
58
ring_node *ring_node_for_averaging_sm_f0= NULL;
59
ring_node *ring_node_for_averaging_sm_f1= NULL;
60
ring_node *ring_node_for_averaging_sm_f2= NULL;
61
62
//
63
ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
64
{
65
    ring_node *node;
66
67
    node = NULL;
68
    switch ( frequencyChannel ) {
69
    case CHANNELF0:
70
        node = ring_node_for_averaging_sm_f0;
71
        break;
72
    case CHANNELF1:
73
        node = ring_node_for_averaging_sm_f1;
74
        break;
75
    case CHANNELF2:
76
        node = ring_node_for_averaging_sm_f2;
77
        break;
78
    default:
79
        break;
80
    }
81
82
    return node;
83
}
84
85
//***********************************************************
86
// Interrupt Service Routine for spectral matrices processing
87
88
void spectral_matrices_isr_f0( int statusReg )
89
{
90
    unsigned char status;
91
    rtems_status_code status_code;
92
    ring_node *full_ring_node;
93
94
    status = (unsigned char) (statusReg & BITS_STATUS_F0);   // [0011] get the status_ready_matrix_f0_x bits
95
96
    switch(status)
97
    {
98
    case 0:
99
        break;
100
    case BIT_READY_0_1:
101
        // UNEXPECTED VALUE
102
        spectral_matrix_regs->status = BIT_READY_0_1;   // [0011]
103
        status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
104
        break;
105
    case BIT_READY_0:
106
        full_ring_node = current_ring_node_sm_f0->previous;
107
        full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
108
        full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time;
109
        current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
110
        spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
111
        // if there are enough ring nodes ready, wake up an AVFx task
112
        nb_sm_f0 = nb_sm_f0 + 1;
113
        if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
114
        {
115
            ring_node_for_averaging_sm_f0 = full_ring_node;
116
            if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
117
            {
118
                status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
119
            }
120
            nb_sm_f0 = 0;
121
        }
122
        spectral_matrix_regs->status = BIT_READY_0;   // [0000 0001]
123
        break;
124
    case BIT_READY_1:
125
        full_ring_node = current_ring_node_sm_f0->previous;
126
        full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
127
        full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time;
128
        current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
129
        spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
130
        // if there are enough ring nodes ready, wake up an AVFx task
131
        nb_sm_f0 = nb_sm_f0 + 1;
132
        if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
133
        {
134
            ring_node_for_averaging_sm_f0 = full_ring_node;
135
            if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
136
            {
137
                status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
138
            }
139
            nb_sm_f0 = 0;
140
        }
141
        spectral_matrix_regs->status = BIT_READY_1;   // [0000 0010]
142
        break;
143
    default:
144
        break;
145
    }
146
}
147
148
void spectral_matrices_isr_f1( int statusReg )
149
{
150
    rtems_status_code status_code;
151
    unsigned char status;
152
    ring_node *full_ring_node;
153
154
    status = (unsigned char) ((statusReg & BITS_STATUS_F1) >> SHIFT_2_BITS);   // [1100] get the status_ready_matrix_f1_x bits
155
156
    switch(status)
157
    {
158
    case 0:
159
        break;
160
    case BIT_READY_0_1:
161
        // UNEXPECTED VALUE
162
        spectral_matrix_regs->status = BITS_STATUS_F1;   // [1100]
163
        status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
164
        break;
165
    case BIT_READY_0:
166
        full_ring_node = current_ring_node_sm_f1->previous;
167
        full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
168
        full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
169
        current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
170
        spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
171
        // if there are enough ring nodes ready, wake up an AVFx task
172
        nb_sm_f1 = nb_sm_f1 + 1;
173
        if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
174
        {
175
            ring_node_for_averaging_sm_f1 = full_ring_node;
176
            if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
177
            {
178
                status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
179
            }
180
            nb_sm_f1 = 0;
181
        }
182
        spectral_matrix_regs->status = BIT_STATUS_F1_0;   // [0000 0100]
183
        break;
184
    case BIT_READY_1:
185
        full_ring_node = current_ring_node_sm_f1->previous;
186
        full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
187
        full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
188
        current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
189
        spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
190
        // if there are enough ring nodes ready, wake up an AVFx task
191
        nb_sm_f1 = nb_sm_f1 + 1;
192
        if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
193
        {
194
            ring_node_for_averaging_sm_f1 = full_ring_node;
195
            if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
196
            {
197
                status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
198
            }
199
            nb_sm_f1 = 0;
200
        }
201
        spectral_matrix_regs->status = BIT_STATUS_F1_1;   // [1000 0000]
202
        break;
203
    default:
204
        break;
205
    }
206
}
207
208
void spectral_matrices_isr_f2( int statusReg )
209
{
210
    unsigned char status;
211
    rtems_status_code status_code;
212
213
    status = (unsigned char) ((statusReg & BITS_STATUS_F2) >> SHIFT_4_BITS);   // [0011 0000] get the status_ready_matrix_f2_x bits
214
215
    switch(status)
216
    {
217
    case 0:
218
        break;
219
    case BIT_READY_0_1:
220
        // UNEXPECTED VALUE
221
        spectral_matrix_regs->status = BITS_STATUS_F2;   // [0011 0000]
222
        status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
223
        break;
224
    case BIT_READY_0:
225
        ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
226
        current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
227
        ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
228
        ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
229
        spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
230
        spectral_matrix_regs->status = BIT_STATUS_F2_0;   // [0001 0000]
231
        if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
232
        {
233
            status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
234
        }
235
        break;
236
    case BIT_READY_1:
237
        ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
238
        current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
239
        ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
240
        ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
241
        spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
242
        spectral_matrix_regs->status = BIT_STATUS_F2_1;   // [0010 0000]
243
        if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
244
        {
245
            status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
246
        }
247
        break;
248
    default:
249
        break;
250
    }
251
}
252
253
void spectral_matrix_isr_error_handler( int statusReg )
254
{
255
    // STATUS REGISTER
256
    // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
257
    //           10                    9                       8
258
    // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
259
    //      7                  6             5       4       3       2       1       0
260
    // [bad_component_err] not defined in the last version of the VHDL code
261
262
    rtems_status_code status_code;
263
264
    //***************************************************
265
    // the ASM status register is copied in the HK packet
266
    housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) ((statusReg & BITS_HK_AA_SM) >> SHIFT_7_BITS);    // [0111 1000 0000]
267
268
    if (statusReg & BITS_SM_ERR)    // [0111 1100 0000]
269
    {
270
        status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
271
    }
272
273
    spectral_matrix_regs->status = spectral_matrix_regs->status & BITS_SM_ERR;
274
275
}
276
277
rtems_isr spectral_matrices_isr( rtems_vector_number vector )
278
{
279
    // STATUS REGISTER
280
    // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
281
    //           10                    9                       8
282
    // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
283
    //      7                  6             5       4       3       2       1       0
284
285
    int statusReg;
286
287
    static restartState state = WAIT_FOR_F2;
288
289
    statusReg = spectral_matrix_regs->status;
290
291
    if (thisIsAnASMRestart == 0)
292
    {   // this is not a restart sequence, process incoming matrices normally
293
        spectral_matrices_isr_f0( statusReg );
294
295
        spectral_matrices_isr_f1( statusReg );
296
297
        spectral_matrices_isr_f2( statusReg );
298
    }
299
    else
300
    {   // a restart sequence has to be launched
301
        switch (state) {
302
        case WAIT_FOR_F2:
303
            if ((statusReg & BITS_STATUS_F2) != INIT_CHAR)   // [0011 0000] check the status_ready_matrix_f2_x bits
304
            {
305
                state = WAIT_FOR_F1;
306
            }
307
            break;
308
        case WAIT_FOR_F1:
309
            if ((statusReg & BITS_STATUS_F1) != INIT_CHAR)   // [0000 1100] check the status_ready_matrix_f1_x bits
310
            {
311
                state = WAIT_FOR_F0;
312
            }
313
            break;
314
        case WAIT_FOR_F0:
315
            if ((statusReg & BITS_STATUS_F0) != INIT_CHAR)   // [0000 0011] check the status_ready_matrix_f0_x bits
316
            {
317
                state = WAIT_FOR_F2;
318
                thisIsAnASMRestart = 0;
319
            }
320
            break;
321
        default:
322
            break;
323
        }
324
        reset_sm_status();
325
    }
326
327
    spectral_matrix_isr_error_handler( statusReg );
328
329
}
330
331
//******************
332
// Spectral Matrices
333
334
void reset_nb_sm( void )
335
{
336
    nb_sm_f0 = 0;
337
    nb_sm_f0_aux_f1 = 0;
338
    nb_sm_f0_aux_f2 = 0;
339
340
    nb_sm_f1 = 0;
341
}
342
343
1
void SM_init_rings( void )
344
{
345
1
    init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
346
1
    init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
347
1
    init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
348
349
    DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
350
    DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
351
    DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
352
    DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
353
    DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
354
    DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
355
1
}
356
357
void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
358
{
359
    unsigned char i;
360
361
    ring[ nbNodes - 1 ].next
362
            = (ring_node_asm*) &ring[ 0 ];
363
364
    for(i=0; i<nbNodes-1; i++)
365
    {
366
        ring[ i ].next            = (ring_node_asm*) &ring[ i + 1 ];
367
    }
368
}
369
370
1
void SM_reset_current_ring_nodes( void )
371
{
372
1
    current_ring_node_sm_f0 = sm_ring_f0[0].next;
373
1
    current_ring_node_sm_f1 = sm_ring_f1[0].next;
374
1
    current_ring_node_sm_f2 = sm_ring_f2[0].next;
375
376
1
    ring_node_for_averaging_sm_f0 = NULL;
377
1
    ring_node_for_averaging_sm_f1 = NULL;
378
1
    ring_node_for_averaging_sm_f2 = NULL;
379
1
}
380
381
//*****************
382
// Basic Parameters
383
384
void BP_init_header( bp_packet *packet,
385
                     unsigned int apid, unsigned char sid,
386
                     unsigned int packetLength, unsigned char blkNr )
387
{
388
    packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
389
    packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
390
    packet->reserved = INIT_CHAR;
391
    packet->userApplication = CCSDS_USER_APP;
392
    packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
393
    packet->packetID[1] = (unsigned char) (apid);
394
    packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
395
    packet->packetSequenceControl[1] = INIT_CHAR;
396
    packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
397
    packet->packetLength[1] = (unsigned char) (packetLength);
398
    // DATA FIELD HEADER
399
    packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
400
    packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
401
    packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
402
    packet->destinationID = TM_DESTINATION_ID_GROUND;
403
    packet->time[BYTE_0] = INIT_CHAR;
404
    packet->time[BYTE_1] = INIT_CHAR;
405
    packet->time[BYTE_2] = INIT_CHAR;
406
    packet->time[BYTE_3] = INIT_CHAR;
407
    packet->time[BYTE_4] = INIT_CHAR;
408
    packet->time[BYTE_5] = INIT_CHAR;
409
    // AUXILIARY DATA HEADER
410
    packet->sid = sid;
411
    packet->pa_bia_status_info = INIT_CHAR;
412
    packet->sy_lfr_common_parameters_spare = INIT_CHAR;
413
    packet->sy_lfr_common_parameters = INIT_CHAR;
414
    packet->acquisitionTime[BYTE_0] = INIT_CHAR;
415
    packet->acquisitionTime[BYTE_1] = INIT_CHAR;
416
    packet->acquisitionTime[BYTE_2] = INIT_CHAR;
417
    packet->acquisitionTime[BYTE_3] = INIT_CHAR;
418
    packet->acquisitionTime[BYTE_4] = INIT_CHAR;
419
    packet->acquisitionTime[BYTE_5] = INIT_CHAR;
420
    packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR;  // BLK_NR MSB
421
    packet->pa_lfr_bp_blk_nr[1] = blkNr;  // BLK_NR LSB
422
}
423
424
void BP_init_header_with_spare( bp_packet_with_spare *packet,
425
                                unsigned int apid, unsigned char sid,
426
                                unsigned int packetLength , unsigned char blkNr)
427
{
428
    packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
429
    packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
430
    packet->reserved = INIT_CHAR;
431
    packet->userApplication = CCSDS_USER_APP;
432
    packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
433
    packet->packetID[1] = (unsigned char) (apid);
434
    packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
435
    packet->packetSequenceControl[1] = INIT_CHAR;
436
    packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
437
    packet->packetLength[1] = (unsigned char) (packetLength);
438
    // DATA FIELD HEADER
439
    packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
440
    packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
441
    packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
442
    packet->destinationID = TM_DESTINATION_ID_GROUND;
443
    // AUXILIARY DATA HEADER
444
    packet->sid = sid;
445
    packet->pa_bia_status_info = INIT_CHAR;
446
    packet->sy_lfr_common_parameters_spare = INIT_CHAR;
447
    packet->sy_lfr_common_parameters = INIT_CHAR;
448
    packet->time[BYTE_0] = INIT_CHAR;
449
    packet->time[BYTE_1] = INIT_CHAR;
450
    packet->time[BYTE_2] = INIT_CHAR;
451
    packet->time[BYTE_3] = INIT_CHAR;
452
    packet->time[BYTE_4] = INIT_CHAR;
453
    packet->time[BYTE_5] = INIT_CHAR;
454
    packet->source_data_spare = INIT_CHAR;
455
    packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR;  // BLK_NR MSB
456
    packet->pa_lfr_bp_blk_nr[1] = blkNr;  // BLK_NR LSB
457
}
458
459
void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
460
{
461
    rtems_status_code status;
462
463
    // SEND PACKET
464
    status =  rtems_message_queue_send( queue_id, data, nbBytesToSend);
465
    if (status != RTEMS_SUCCESSFUL)
466
    {
467
        PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
468
    }
469
}
470
471
void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
472
{
473
    /** This function is used to send the BP paquets when needed.
474
     *
475
     * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
476
     *
477
     * @return void
478
     *
479
     * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition.
480
     * BURST paquets are sent everytime.
481
     *
482
     */
483
484
    rtems_status_code status;
485
486
    // SEND PACKET
487
    // before lastValidTransitionDate, the data are drops even if they are ready
488
    // this guarantees that no SBM packets will be received before the requested enter mode time
489
    if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
490
    {
491
        status =  rtems_message_queue_send( queue_id, data, nbBytesToSend);
492
        if (status != RTEMS_SUCCESSFUL)
493
        {
494
            PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
495
        }
496
    }
497
}
498
499
//******************
500
// general functions
501
502
1
void reset_sm_status( void )
503
{
504
    // error
505
    // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
506
    // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
507
    // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
508
    // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
509
510
1
    spectral_matrix_regs->status = BITS_STATUS_REG;   // [0111 1111 1111]
511
1
}
512
513
1
void reset_spectral_matrix_regs( void )
514
{
515
    /** This function resets the spectral matrices module registers.
516
     *
517
     * The registers affected by this function are located at the following offset addresses:
518
     *
519
     * - 0x00 config
520
     * - 0x04 status
521
     * - 0x08 matrixF0_Address0
522
     * - 0x10 matrixFO_Address1
523
     * - 0x14 matrixF1_Address
524
     * - 0x18 matrixF2_Address
525
     *
526
     */
527
528
1
    set_sm_irq_onError( 0 );
529
530
1
    set_sm_irq_onNewMatrix( 0 );
531
532
1
    reset_sm_status();
533
534
    // F1
535
1
    spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
536
1
    spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
537
    // F2
538
1
    spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
539
1
    spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
540
    // F3
541
1
    spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
542
1
    spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
543
544
1
    spectral_matrix_regs->matrix_length = DEFAULT_MATRIX_LENGTH; // 25 * 128 / 16 = 200 = 0xc8
545
1
}
546
547
void set_time( unsigned char *time, unsigned char * timeInBuffer )
548
{
549
    time[BYTE_0] = timeInBuffer[BYTE_0];
550
    time[BYTE_1] = timeInBuffer[BYTE_1];
551
    time[BYTE_2] = timeInBuffer[BYTE_2];
552
    time[BYTE_3] = timeInBuffer[BYTE_3];
553
    time[BYTE_4] = timeInBuffer[BYTE_6];
554
    time[BYTE_5] = timeInBuffer[BYTE_7];
555
}
556
557
unsigned long long int get_acquisition_time( unsigned char *timePtr )
558
{
559
    unsigned long long int acquisitionTimeAslong;
560
    acquisitionTimeAslong = INIT_CHAR;
561
    acquisitionTimeAslong =
562
            ( (unsigned long long int) (timePtr[BYTE_0] & SYNC_BIT_MASK) << SHIFT_5_BYTES ) // [0111 1111] mask the synchronization bit
563
            + ( (unsigned long long int) timePtr[BYTE_1] << SHIFT_4_BYTES )
564
            + ( (unsigned long long int) timePtr[BYTE_2] << SHIFT_3_BYTES )
565
            + ( (unsigned long long int) timePtr[BYTE_3] << SHIFT_2_BYTES )
566
            + ( (unsigned long long int) timePtr[BYTE_6] << SHIFT_1_BYTE  )
567
            + ( (unsigned long long int) timePtr[BYTE_7]       );
568
    return acquisitionTimeAslong;
569
}
570
571
unsigned char getSID( rtems_event_set event )
572
{
573
    unsigned char sid;
574
575
    rtems_event_set eventSetBURST;
576
    rtems_event_set eventSetSBM;
577
578
    sid = 0;
579
580
    //******
581
    // BURST
582
    eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
583
            | RTEMS_EVENT_BURST_BP1_F1
584
            | RTEMS_EVENT_BURST_BP2_F0
585
            | RTEMS_EVENT_BURST_BP2_F1;
586
587
    //****
588
    // SBM
589
    eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
590
            | RTEMS_EVENT_SBM_BP1_F1
591
            | RTEMS_EVENT_SBM_BP2_F0
592
            | RTEMS_EVENT_SBM_BP2_F1;
593
594
    if (event & eventSetBURST)
595
    {
596
        sid = SID_BURST_BP1_F0;
597
    }
598
    else if (event & eventSetSBM)
599
    {
600
        sid = SID_SBM1_BP1_F0;
601
    }
602
    else
603
    {
604
        sid = 0;
605
    }
606
607
    return sid;
608
}
609
610
/**
611
 * @brief extractReImVectors converts a given ASM component from interleaved to split representation
612
 * @param inputASM
613
 * @param outputASM
614
 * @param asmComponent
615
 */
616
void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
617
{
618
    unsigned int i;
619
    float re;
620
    float im;
621
622
    for (i=0; i<NB_BINS_PER_SM; i++){
623
        re = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL)    ];
624
        im = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) + 1];
625
        outputASM[ ( asmComponent   *NB_BINS_PER_SM) +  i] = re;
626
        outputASM[ ((asmComponent+1)*NB_BINS_PER_SM) +  i] = im;
627
    }
628
}
629
630
/**
631
 * @brief copyReVectors copies real part of a given ASM from inputASM to outputASM
632
 * @param inputASM
633
 * @param outputASM
634
 * @param asmComponent
635
 */
636
void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
637
{
638
    unsigned int i;
639
    float re;
640
641
    for (i=0; i<NB_BINS_PER_SM; i++){
642
        re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
643
        outputASM[ (asmComponent*NB_BINS_PER_SM)  +  i] = re;
644
    }
645
}
646
647
/**
648
 * @brief ASM_patch, converts ASM from interleaved to split representation
649
 * @param inputASM
650
 * @param outputASM
651
 * @note inputASM and outputASM must be different, in other words this function can't do in place convertion
652
 * @see extractReImVectors
653
 */
654
void ASM_patch( float *inputASM, float *outputASM )
655
{
656
    extractReImVectors( inputASM, outputASM, ASM_COMP_B1B2);    // b1b2
657
    extractReImVectors( inputASM, outputASM, ASM_COMP_B1B3 );   // b1b3
658
    extractReImVectors( inputASM, outputASM, ASM_COMP_B1E1 );   // b1e1
659
    extractReImVectors( inputASM, outputASM, ASM_COMP_B1E2 );   // b1e2
660
    extractReImVectors( inputASM, outputASM, ASM_COMP_B2B3 );   // b2b3
661
    extractReImVectors( inputASM, outputASM, ASM_COMP_B2E1 );   // b2e1
662
    extractReImVectors( inputASM, outputASM, ASM_COMP_B2E2 );   // b2e2
663
    extractReImVectors( inputASM, outputASM, ASM_COMP_B3E1 );   // b3e1
664
    extractReImVectors( inputASM, outputASM, ASM_COMP_B3E2 );   // b3e2
665
    extractReImVectors( inputASM, outputASM, ASM_COMP_E1E2 );   // e1e2
666
667
    copyReVectors(inputASM, outputASM, ASM_COMP_B1B1 );     // b1b1
668
    copyReVectors(inputASM, outputASM, ASM_COMP_B2B2 );     // b2b2
669
    copyReVectors(inputASM, outputASM, ASM_COMP_B3B3);      // b3b3
670
    copyReVectors(inputASM, outputASM, ASM_COMP_E1E1);      // e1e1
671
    copyReVectors(inputASM, outputASM, ASM_COMP_E2E2);      // e2e2
672
}
673
674
void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
675
                                             unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage,
676
                                             unsigned char ASMIndexStart,
677
                                             unsigned char channel )
678
{
679
    //*************
680
    // input format
681
    // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
682
    //**************
683
    // output format
684
    // matr0[0 .. 24]      matr1[0 .. 24]       .. matr127[0 .. 24]
685
    //************
686
    // compression
687
    // matr0[0 .. 24]      matr1[0 .. 24]       .. matr11[0 .. 24] => f0 NORM
688
    // matr0[0 .. 24]      matr1[0 .. 24]       .. matr22[0 .. 24] => f0 BURST, SBM
689
690
    int frequencyBin;
691
    int asmComponent;
692
    int offsetASM;
693
    int offsetCompressed;
694
    int offsetFBin;
695
    int fBinMask;
696
    int k;
697
698
    // BUILD DATA
699
    for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
700
    {
701
        for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
702
        {
703
            offsetCompressed =  // NO TIME OFFSET
704
                    (frequencyBin * NB_VALUES_PER_SM)
705
                    + asmComponent;
706
            offsetASM =         // NO TIME OFFSET
707
                    (asmComponent * NB_BINS_PER_SM)
708
                    + ASMIndexStart
709
                    + (frequencyBin * nbBinsToAverage);
710
            offsetFBin = ASMIndexStart
711
                    + (frequencyBin * nbBinsToAverage);
712
            compressed_spec_mat[ offsetCompressed ] = 0;
713
            for ( k = 0; k < nbBinsToAverage; k++ )
714
            {
715
                fBinMask = getFBinMask( offsetFBin + k, channel );
716
                compressed_spec_mat[offsetCompressed ] = compressed_spec_mat[ offsetCompressed ]
717
                        + (averaged_spec_mat[ offsetASM + k ] * fBinMask);
718
            }
719
            if (divider != 0)
720
            {
721
                compressed_spec_mat[ offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
722
            }
723
            else
724
            {
725
                compressed_spec_mat[ offsetCompressed ] = INIT_FLOAT;
726
            }
727
        }
728
    }
729
730
}
731
732
int getFBinMask( int index, unsigned char channel )
733
{
734
    unsigned int indexInChar;
735
    unsigned int indexInTheChar;
736
    int fbin;
737
    unsigned char *sy_lfr_fbins_fx_word1;
738
739
    sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
740
741
    switch(channel)
742
    {
743
    case CHANNELF0:
744
        sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0;
745
        break;
746
    case CHANNELF1:
747
        sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1;
748
        break;
749
    case CHANNELF2:
750
        sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2;
751
        break;
752
    default:
753
        PRINTF("ERR *** in getFBinMask, wrong frequency channel")
754
    }
755
756
    indexInChar = index >> SHIFT_3_BITS;
757
    indexInTheChar = index - (indexInChar * BITS_PER_BYTE);
758
759
    fbin = (int) ((sy_lfr_fbins_fx_word1[ BYTES_PER_MASK - 1 - indexInChar] >> indexInTheChar) & 1);
760
761
    return fbin;
762
}
763
764
/**
765
 * @brief isPolluted returns MATRIX_IS_POLLUTED if there is any overlap between t0:t1 and tbad0:tbad1 ranges
766
 * @param t0 Start acquisition time
767
 * @param t1 End of acquisition time
768
 * @param tbad0 Start time of poluting signal
769
 * @param tbad1 End time of poluting signal
770
 * @return
771
 */
772
unsigned char isPolluted( u_int64_t t0, u_int64_t t1, u_int64_t tbad0, u_int64_t tbad1 )
773
{
774
    unsigned char polluted;
775
776
    polluted = MATRIX_IS_NOT_POLLUTED;
777
778
    if ( ((tbad0 < t0) && (t0 < tbad1))     // t0 is inside the polluted range
779
         || ((tbad0 < t1) && (t1 < tbad1))  // t1 is inside the polluted range
780
         || ((t0 < tbad0) && (tbad1 < t1))  // the polluted range is inside the signal range
781
         || ((tbad0 < t0) && (t1 < tbad1))) // the signal range is inside the polluted range
782
    {
783
        polluted = MATRIX_IS_POLLUTED;
784
    }
785
786
    return polluted;
787
}
788
789
/**
790
 * @brief acquisitionTimeIsValid checks if the given acquisition time is poluted by PAS
791
 * @param coarseTime Coarse acquisition time of the given SM
792
 * @param fineTime Fine acquisition time of the given ASM
793
 * @param channel Frequency channel to check, will impact SM time footprint
794
 * @return MATRIX_IS_POLLUTED if there is any time overlap between SM and PAS poluting signal
795
 */
796
unsigned char acquisitionTimeIsValid( unsigned int coarseTime, unsigned int fineTime, unsigned char channel)
797
{
798
    u_int64_t t0;
799
    u_int64_t t1;
800
    u_int64_t tc;
801
    u_int64_t tbad0;
802
    u_int64_t tbad1;
803
804
    u_int64_t modulusInFineTime;
805
    u_int64_t offsetInFineTime;
806
    u_int64_t shiftInFineTime;
807
    u_int64_t tbadInFineTime;
808
809
    u_int64_t timecodeReference;
810
811
    unsigned char pasFilteringIsEnabled;
812
    unsigned char ret;
813
814
    // compute acquisition time from caoarseTime and fineTime
815
    t0 = ( ((u_int64_t)coarseTime) <<  SHIFT_2_BYTES ) + (u_int64_t) fineTime;
816
    t1 = t0;
817
    tc = t0;
818
    tbad0 = t0;
819
    tbad1 = t0;
820
821
    switch(channel)
822
    {
823
    case CHANNELF0:
824
        t1 = t0 + ACQUISITION_DURATION_F0;
825
        tc = t0 + HALF_ACQUISITION_DURATION_F0;
826
        break;
827
    case CHANNELF1:
828
        t1 = t0 + ACQUISITION_DURATION_F1;
829
        tc = t0 + HALF_ACQUISITION_DURATION_F1;
830
        break;
831
    case CHANNELF2:
832
        t1 = t0 + ACQUISITION_DURATION_F2;
833
        tc = t0 + HALF_ACQUISITION_DURATION_F2;
834
        break;
835
    default:
836
        break;
837
    }
838
839
    // compute the acquitionTime range
840
    modulusInFineTime   = filterPar.modulus_in_finetime;
841
    offsetInFineTime    = filterPar.offset_in_finetime;
842
    shiftInFineTime     = filterPar.shift_in_finetime;
843
    tbadInFineTime      = filterPar.tbad_in_finetime;
844
    timecodeReference   = INIT_INT;
845
846
    pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 1); // [0000 0001]
847
    ret = MATRIX_IS_NOT_POLLUTED;
848
849
    if ( (tbadInFineTime == 0) || (pasFilteringIsEnabled == 0) )
850
    {
851
        ret = MATRIX_IS_NOT_POLLUTED;
852
    }
853
    else
854
    {
855
        // INTERSECTION TEST #1
856
        timecodeReference = (tc - (tc % modulusInFineTime)) - modulusInFineTime ;
857
        tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
858
        tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
859
        ret = isPolluted( t0, t1, tbad0, tbad1 );
860
861
        // INTERSECTION TEST #2
862
        if (ret == MATRIX_IS_NOT_POLLUTED)
863
        {
864
            timecodeReference = (tc - (tc % modulusInFineTime)) ;
865
            tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
866
            tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
867
            ret = isPolluted( t0, t1, tbad0, tbad1 );
868
        }
869
870
        // INTERSECTION TEST #3
871
        if (ret == MATRIX_IS_NOT_POLLUTED)
872
        {
873
            timecodeReference = (tc - (tc % modulusInFineTime)) + modulusInFineTime ;
874
            tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
875
            tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
876
            ret = isPolluted( t0, t1, tbad0, tbad1 );
877
        }
878
    }
879
880
    return ret;
881
}
882
883
2
void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
884
{
885
    unsigned char bin;
886
    unsigned char kcoeff;
887
888
26
    for (bin=0; bin<nb_bins_norm; bin++)
889
    {
890
792
        for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
891
        {
892
1536
            output_kcoeff[   ( (bin * NB_K_COEFF_PER_BIN) + kcoeff ) * SBM_COEFF_PER_NORM_COEFF       ]
893
768
                    = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
894
1536
            output_kcoeff[ ( ( (bin * NB_K_COEFF_PER_BIN ) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ) + 1 ]
895
768
                    = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
896
        }
897
    }
898
2
}