GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/processing/avf2_prc2.c Lines: 107 107 100.0 %
Date: 2018-11-13 11:16:07 Branches: 39 40 97.5 %

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 "avf2_prc2.h"
35
36
nb_sm_before_bp_asm_f2 nb_sm_before_f2 = {0};
37
38
//***
39
// F2
40
ring_node_asm asm_ring_norm_f2     [ NB_RING_NODES_ASM_NORM_F2      ] = {0};
41
42
ring_node ring_to_send_asm_f2      [ NB_RING_NODES_ASM_F2 ] = {0};
43
int buffer_asm_f2                  [ NB_RING_NODES_ASM_F2 * TOTAL_SIZE_SM ] = {0};
44
45
float asm_f2_patched_norm   [ TOTAL_SIZE_SM ] = {0};
46
float asm_f2_reorganized    [ TOTAL_SIZE_SM ] = {0};
47
48
float compressed_sm_norm_f2[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F2] = {0};
49
50
float k_coeff_intercalib_f2[ NB_BINS_COMPRESSED_SM_F2 * NB_K_COEFF_PER_BIN ] = {0}; // 12 * 32 = 384
51
52
//************
53
// RTEMS TASKS
54
55
//***
56
// F2
57
1606
rtems_task avf2_task( rtems_task_argument argument )
58
{
59
    rtems_event_set event_out;
60
    rtems_status_code status;
61
    rtems_id queue_id_prc2;
62
    asm_msg msgForPRC;
63
    ring_node *nodeForAveraging;
64
    ring_node_asm *current_ring_node_asm_norm_f2;
65
66
    unsigned int nb_norm_bp1;
67
    unsigned int nb_norm_bp2;
68
    unsigned int nb_norm_asm;
69
70
1606
    event_out = EVENT_SETS_NONE_PENDING;
71
1606
    queue_id_prc2 = RTEMS_ID_NONE;
72
1606
    nb_norm_bp1 = 0;
73
1606
    nb_norm_bp2 = 0;
74
1606
    nb_norm_asm = 0;
75
76
1606
    reset_nb_sm_f2( );   // reset the sm counters that drive the BP and ASM computations / transmissions
77
1606
    ASM_generic_init_ring( asm_ring_norm_f2, NB_RING_NODES_ASM_NORM_F2 );
78
1606
    current_ring_node_asm_norm_f2 = asm_ring_norm_f2;
79
80
    BOOT_PRINTF("in AVF2 ***\n")
81
82
1606
    status = get_message_queue_id_prc2( &queue_id_prc2 );
83
    if (status != RTEMS_SUCCESSFUL)
84
    {
85
        PRINTF1("in AVF2 *** ERR get_message_queue_id_prc2 %d\n", status)
86
    }
87
88
    while(1){
89
108855
        rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
90
91
        //****************************************
92
        // initialize the mesage for the MATR task
93
107392
        msgForPRC.norm       = current_ring_node_asm_norm_f2;
94
107392
        msgForPRC.burst_sbm  = NULL;
95
107392
        msgForPRC.event      = EVENT_SETS_NONE_PENDING;  // this composite event will be sent to the PRC2 task
96
        //
97
        //****************************************
98
99
107392
        nodeForAveraging = getRingNodeForAveraging( CHANNELF2 );
100
101
        // compute the average and store it in the averaged_sm_f2 buffer
102
107392
        SM_average_f2( current_ring_node_asm_norm_f2->matrix,
103
                       nodeForAveraging,
104
                       nb_norm_bp1,
105
                       &msgForPRC );
106
107
        // update nb_average
108
107251
        nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF2;
109
107251
        nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF2;
110
107251
        nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF2;
111
112
107251
        if (nb_norm_bp1 == nb_sm_before_f2.norm_bp1)
113
        {
114
26517
            nb_norm_bp1 = 0;
115
            // set another ring for the ASM storage
116
26517
            current_ring_node_asm_norm_f2 = current_ring_node_asm_norm_f2->next;
117

35161
            if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
118
35161
                 || (lfrCurrentMode == LFR_MODE_SBM2) )
119
            {
120
23944
                msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP1_F2;
121
            }
122
        }
123
124
107251
        if (nb_norm_bp2 == nb_sm_before_f2.norm_bp2)
125
        {
126
5170
            nb_norm_bp2 = 0;
127

6839
            if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
128
6839
                 || (lfrCurrentMode == LFR_MODE_SBM2) )
129
            {
130
4682
                msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP2_F2;
131
            }
132
        }
133
134
107251
        if (nb_norm_asm == nb_sm_before_f2.norm_asm)
135
        {
136
17241
            nb_norm_asm = 0;
137

22692
            if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
138
22692
                 || (lfrCurrentMode == LFR_MODE_SBM2) )
139
            {
140
16100
                msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_ASM_F2;
141
            }
142
        }
143
144
        //*************************
145
        // send the message to PRC2
146
107251
        if (msgForPRC.event != EVENT_SETS_NONE_PENDING)
147
        {
148
23944
            status =  rtems_message_queue_send( queue_id_prc2, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC2);
149
        }
150
151
        if (status != RTEMS_SUCCESSFUL) {
152
            PRINTF1("in AVF2 *** Error sending message to PRC2, code %d\n", status)
153
        }
154
    }
155
}
156
157
1606
rtems_task prc2_task( rtems_task_argument argument )
158
{
159
    char incomingData[MSG_QUEUE_SIZE_SEND];  // incoming data buffer
160
    size_t size;                            // size of the incoming TC packet
161
    asm_msg *incomingMsg;
162
    //
163
    rtems_status_code status;
164
    rtems_id queue_id_send;
165
    rtems_id queue_id_q_p2;
166
    bp_packet                   __attribute__((aligned(4))) packet_norm_bp1;
167
    bp_packet                   __attribute__((aligned(4))) packet_norm_bp2;
168
    ring_node                   *current_ring_node_to_send_asm_f2;
169
    float nbSMInASMNORM;
170
171
    unsigned long long int localTime;
172
173
1606
    size = 0;
174
1606
    queue_id_send = RTEMS_ID_NONE;
175
1606
    queue_id_q_p2 = RTEMS_ID_NONE;
176
1606
    memset( &packet_norm_bp1, 0, sizeof(bp_packet) );
177
1606
    memset( &packet_norm_bp2, 0, sizeof(bp_packet) );
178
179
    // init the ring of the averaged spectral matrices which will be transmitted to the DPU
180
1606
    init_ring( ring_to_send_asm_f2, NB_RING_NODES_ASM_F2, (volatile int*) buffer_asm_f2, TOTAL_SIZE_SM );
181
1606
    current_ring_node_to_send_asm_f2 = ring_to_send_asm_f2;
182
183
    //*************
184
    // NORM headers
185
1606
    BP_init_header( &packet_norm_bp1,
186
                    APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F2,
187
                    PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2, NB_BINS_COMPRESSED_SM_F2 );
188
1606
    BP_init_header( &packet_norm_bp2,
189
                    APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F2,
190
                    PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2, NB_BINS_COMPRESSED_SM_F2 );
191
192
1606
    status =  get_message_queue_id_send( &queue_id_send );
193
    if (status != RTEMS_SUCCESSFUL)
194
    {
195
        PRINTF1("in PRC2 *** ERR get_message_queue_id_send %d\n", status)
196
    }
197
1606
    status = get_message_queue_id_prc2( &queue_id_q_p2);
198
    if (status != RTEMS_SUCCESSFUL)
199
    {
200
        PRINTF1("in PRC2 *** ERR get_message_queue_id_prc2 %d\n", status)
201
    }
202
203
    BOOT_PRINTF("in PRC2 ***\n")
204
205
    while(1){
206
25548
        status = rtems_message_queue_receive( queue_id_q_p2, incomingData, &size, //************************************
207
                                             RTEMS_WAIT, RTEMS_NO_TIMEOUT );      // wait for a message coming from AVF2
208
209
23942
        incomingMsg = (asm_msg*) incomingData;
210
211
23942
        ASM_patch( incomingMsg->norm->matrix, asm_f2_patched_norm );
212
213
23942
        localTime = getTimeAsUnsignedLongLongInt( );
214
215
23942
        nbSMInASMNORM = incomingMsg->numberOfSMInASMNORM;
216
217
        //*****
218
        //*****
219
        // NORM
220
        //*****
221
        //*****
222
        // 1) compress the matrix for Basic Parameters calculation
223
23942
        ASM_compress_reorganize_and_divide_mask( asm_f2_patched_norm, compressed_sm_norm_f2,
224
                                     nbSMInASMNORM,
225
                                     NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2,
226
                                     ASM_F2_INDICE_START, CHANNELF2 );
227
        // BP1_F2
228
23942
        if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F2)
229
        {
230
            // 1) compute the BP1 set
231
23942
            BP1_set( compressed_sm_norm_f2, k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp1.data );
232
            // 2) send the BP1 set
233
23942
            set_time( packet_norm_bp1.time,            (unsigned char *) &incomingMsg->coarseTimeNORM );
234
23942
            set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
235
23942
            packet_norm_bp1.pa_bia_status_info = pa_bia_status_info;
236
23942
            packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
237
23942
            BP_send( (char *) &packet_norm_bp1, queue_id_send,
238
                     PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2 + PACKET_LENGTH_DELTA,
239
                     SID_NORM_BP1_F2 );
240
        }
241
        // BP2_F2
242
23942
        if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F2)
243
        {
244
            // 1) compute the BP2 set
245
4681
            BP2_set( compressed_sm_norm_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp2.data );
246
            // 2) send the BP2 set
247
4681
            set_time( packet_norm_bp2.time,            (unsigned char *) &incomingMsg->coarseTimeNORM );
248
4681
            set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
249
4681
            packet_norm_bp2.pa_bia_status_info = pa_bia_status_info;
250
4681
            packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
251
4681
            BP_send( (char *) &packet_norm_bp2, queue_id_send,
252
                     PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2 + PACKET_LENGTH_DELTA,
253
                     SID_NORM_BP2_F2 );
254
        }
255
256
23942
        if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F2)
257
        {
258
            // 1) reorganize the ASM and divide
259
32200
            ASM_reorganize_and_divide( asm_f2_patched_norm,
260
16100
                                       (float*) current_ring_node_to_send_asm_f2->buffer_address,
261
16100
                                       nb_sm_before_f2.norm_bp1 );
262
16100
            current_ring_node_to_send_asm_f2->coarseTime    = incomingMsg->coarseTimeNORM;
263
16100
            current_ring_node_to_send_asm_f2->fineTime      = incomingMsg->fineTimeNORM;
264
16100
            current_ring_node_to_send_asm_f2->sid           = SID_NORM_ASM_F2;
265
266
            // 3) send the spectral matrix packets
267
16100
            status =  rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f2, sizeof( ring_node* ) );
268
269
            // change asm ring node
270
16100
            current_ring_node_to_send_asm_f2 = current_ring_node_to_send_asm_f2->next;
271
        }
272
273
23942
        update_queue_max_count( queue_id_q_p2, &hk_lfr_q_p2_fifo_size_max );
274
275
23942
    }
276
}
277
278
//**********
279
// FUNCTIONS
280
281
1606
void reset_nb_sm_f2( void )
282
{
283
1606
    nb_sm_before_f2.norm_bp1  = parameter_dump_packet.sy_lfr_n_bp_p0;
284
1606
    nb_sm_before_f2.norm_bp2  = parameter_dump_packet.sy_lfr_n_bp_p1;
285
1606
    nb_sm_before_f2.norm_asm  = (parameter_dump_packet.sy_lfr_n_asm_p[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_asm_p[1];
286
1606
}
287
288
107392
void SM_average_f2( float *averaged_spec_mat_f2,
289
                    ring_node *ring_node,
290
                    unsigned int nbAverageNormF2,
291
                    asm_msg *msgForMATR )
292
{
293
    float sum;
294
    unsigned int i;
295
    unsigned char keepMatrix;
296
297
    // test acquisitionTime validity
298
107392
    keepMatrix = acquisitionTimeIsValid( ring_node->coarseTime, ring_node->fineTime, CHANNELF2 );
299
300
343310451
    for(i=0; i<TOTAL_SIZE_SM; i++)
301
    {
302
343203200
        sum = ( (int *) (ring_node->buffer_address) ) [ i ];
303
343203200
        if ( (nbAverageNormF2 == 0) )   // average initialization
304
        {
305
86352682
            if (keepMatrix == MATRIX_IS_NOT_POLLUTED)   // keep the matrix and add it to the average
306
            {
307
83210376
                averaged_spec_mat_f2[ i ] = sum;
308
            }
309
            else                    // drop the matrix and initialize the average
310
            {
311
3142306
                averaged_spec_mat_f2[ i ] = INIT_FLOAT;
312
            }
313
86352682
            msgForMATR->coarseTimeNORM  = ring_node->coarseTime;
314
86352682
            msgForMATR->fineTimeNORM    = ring_node->fineTime;
315
        }
316
        else
317
        {
318
256857768
            if (keepMatrix == MATRIX_IS_NOT_POLLUTED)   // keep the matrix and add it to the average
319
            {
320
250149302
                averaged_spec_mat_f2[ i ] = ( averaged_spec_mat_f2[  i ] + sum );
321
            }
322
            else
323
            {
324
                // nothing to do, the matrix is not valid
325
            }
326
        }
327
    }
328
329
107251
    if (keepMatrix == 1)
330
    {
331
104134
        if ( (nbAverageNormF2 == 0) )
332
        {
333
25991
            msgForMATR->numberOfSMInASMNORM = 1;
334
        }
335
        else
336
        {
337
78143
            msgForMATR->numberOfSMInASMNORM++;
338
        }
339
    }
340
    else
341
    {
342
3117
        if ( (nbAverageNormF2 == 0) )
343
        {
344
981
            msgForMATR->numberOfSMInASMNORM = 0;
345
        }
346
        else
347
        {
348
            // nothing to do
349
        }
350
    }
351
107251
}
352
353
87
void init_k_coefficients_prc2( void )
354
{
355
87
    init_k_coefficients( k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2);
356
87
}