Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/processing/avf1_prc1.c	Lines:	149	149	100.0 %
Date:	2018-11-13 11:16:07	Branches:	52	52	100.0 %


/*------------------------------------------------------------------------------
--  Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
--  This file is a part of the LFR FSW
--  Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
--
--  This program is free software; you can redistribute it and/or modify
--  it under the terms of the GNU General Public License as published by
--  the Free Software Foundation; either version 2 of the License, or
--  (at your option) any later version.
--
--  This program is distributed in the hope that it will be useful,
--  but WITHOUT ANY WARRANTY; without even the implied warranty of
--  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
--  GNU General Public License for more details.
--
--  You should have received a copy of the GNU General Public License
--  along with this program; if not, write to the Free Software
--  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
-------------------------------------------------------------------------------*/
/*--                  Author : Paul Leroy
--                   Contact : Alexis Jeandet
--                      Mail : alexis.jeandet@lpp.polytechnique.fr
----------------------------------------------------------------------------*/

/** Functions related to data processing.
 *
 * @file
 * @author P. LEROY
 *
 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
 *
 */

#include "avf1_prc1.h"

nb_sm_before_bp_asm_f1 nb_sm_before_f1 = {0};

//***
// F1
ring_node_asm asm_ring_norm_f1      [ NB_RING_NODES_ASM_NORM_F1      ] = {0};
ring_node_asm asm_ring_burst_sbm_f1 [ NB_RING_NODES_ASM_BURST_SBM_F1 ] = {0};

ring_node ring_to_send_asm_f1       [ NB_RING_NODES_ASM_F1 ] = {0};
int buffer_asm_f1                   [ NB_RING_NODES_ASM_F1 * TOTAL_SIZE_SM ] = {0};

float asm_f1_patched_norm       [ TOTAL_SIZE_SM ] = {0};
float asm_f1_patched_burst_sbm  [ TOTAL_SIZE_SM ] = {0};
float asm_f1_reorganized        [ TOTAL_SIZE_SM ] = {0};

float compressed_sm_norm_f1[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F1] = {0};
float compressed_sm_sbm_f1 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 ] = {0};

float k_coeff_intercalib_f1_norm[ NB_BINS_COMPRESSED_SM_F1     * NB_K_COEFF_PER_BIN ] = {0};  // 13 * 32 = 416
float k_coeff_intercalib_f1_sbm[  NB_BINS_COMPRESSED_SM_SBM_F1 * NB_K_COEFF_PER_BIN ] = {0};  // 26 * 32 = 832

//************
// RTEMS TASKS

rtems_task avf1_task( rtems_task_argument lfrRequestedMode )
{
    int i;

    rtems_event_set event_out;
    rtems_status_code status;
    rtems_id queue_id_prc1;
    asm_msg msgForPRC;
    ring_node *nodeForAveraging;
    ring_node *ring_node_tab[NB_SM_BEFORE_AVF0_F1];
    ring_node_asm *current_ring_node_asm_burst_sbm_f1;
    ring_node_asm *current_ring_node_asm_norm_f1;

    unsigned int nb_norm_bp1;
    unsigned int nb_norm_bp2;
    unsigned int nb_norm_asm;
    unsigned int nb_sbm_bp1;
    unsigned int nb_sbm_bp2;

    event_out = EVENT_SETS_NONE_PENDING;
    queue_id_prc1 = RTEMS_ID_NONE;

    nb_norm_bp1 = 0;
    nb_norm_bp2 = 0;
    nb_norm_asm = 0;
    nb_sbm_bp1  = 0;
    nb_sbm_bp2  = 0;

    reset_nb_sm_f1( lfrRequestedMode );   // reset the sm counters that drive the BP and ASM computations / transmissions
    ASM_generic_init_ring( asm_ring_norm_f1, NB_RING_NODES_ASM_NORM_F1 );
    ASM_generic_init_ring( asm_ring_burst_sbm_f1, NB_RING_NODES_ASM_BURST_SBM_F1 );
    current_ring_node_asm_norm_f1      = asm_ring_norm_f1;
    current_ring_node_asm_burst_sbm_f1 = asm_ring_burst_sbm_f1;

    BOOT_PRINTF1("in AVF1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)

    status = get_message_queue_id_prc1( &queue_id_prc1 );
    if (status != RTEMS_SUCCESSFUL)
    {
        PRINTF1("in AVF1 *** ERR get_message_queue_id_prc1 %d\n", status)
    }

    while(1){
        rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0

        //****************************************
        // initialize the mesage for the MATR task
        msgForPRC.norm       = current_ring_node_asm_norm_f1;
        msgForPRC.burst_sbm  = current_ring_node_asm_burst_sbm_f1;
        msgForPRC.event      = EVENT_SETS_NONE_PENDING;  // this composite event will be sent to the PRC1 task
        //
        //****************************************

        nodeForAveraging = getRingNodeForAveraging( 1 );

        ring_node_tab[NB_SM_BEFORE_AVF0_F1-1] = nodeForAveraging;
        for ( i = 1; i < (NB_SM_BEFORE_AVF0_F1); i++ )
        {
            nodeForAveraging = nodeForAveraging->previous;
            ring_node_tab[NB_SM_BEFORE_AVF0_F1 - i - 1] = nodeForAveraging;
        }

        // compute the average and store it in the averaged_sm_f1 buffer
        SM_average( current_ring_node_asm_norm_f1->matrix,
                    current_ring_node_asm_burst_sbm_f1->matrix,
                    ring_node_tab,
                    nb_norm_bp1, nb_sbm_bp1,
                    &msgForPRC, 1 );    // 1 => frequency channel 1

        // update nb_average
        nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0_F1;
        nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0_F1;
        nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0_F1;
        nb_sbm_bp1  = nb_sbm_bp1  + NB_SM_BEFORE_AVF0_F1;
        nb_sbm_bp2  = nb_sbm_bp2  + NB_SM_BEFORE_AVF0_F1;

        if (nb_sbm_bp1 == nb_sm_before_f1.burst_sbm_bp1)
        {
            nb_sbm_bp1 = 0;
            // set another ring for the ASM storage
            current_ring_node_asm_burst_sbm_f1 = current_ring_node_asm_burst_sbm_f1->next;
            if ( lfrCurrentMode == LFR_MODE_BURST )
            {
                msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP1_F1;
            }
            else if ( lfrCurrentMode == LFR_MODE_SBM2 )
            {
                msgForPRC.event = msgForPRC.event | RTEMS_EVENT_SBM_BP1_F1;
            }
        }

        if (nb_sbm_bp2 == nb_sm_before_f1.burst_sbm_bp2)
        {
            nb_sbm_bp2 = 0;
            if ( lfrCurrentMode == LFR_MODE_BURST )
            {
                msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP2_F1;
            }
            else if ( lfrCurrentMode == LFR_MODE_SBM2 )
            {
                msgForPRC.event = msgForPRC.event | RTEMS_EVENT_SBM_BP2_F1;
            }
        }

        if (nb_norm_bp1 == nb_sm_before_f1.norm_bp1)
        {
            nb_norm_bp1 = 0;
            // set another ring for the ASM storage
            current_ring_node_asm_norm_f1 = current_ring_node_asm_norm_f1->next;
            if ( (lfrCurrentMode == LFR_MODE_NORMAL)
                 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
            {
                msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP1_F1;
            }
        }

        if (nb_norm_bp2 == nb_sm_before_f1.norm_bp2)
        {
            nb_norm_bp2 = 0;
            if ( (lfrCurrentMode == LFR_MODE_NORMAL)
                 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
            {
                msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP2_F1;
            }
        }

        if (nb_norm_asm == nb_sm_before_f1.norm_asm)
        {
            nb_norm_asm = 0;
            if ( (lfrCurrentMode == LFR_MODE_NORMAL)
                 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
            {
                msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_ASM_F1;
            }
        }

        //*************************
        // send the message to PRC
        if (msgForPRC.event != EVENT_SETS_NONE_PENDING)
        {
            status =  rtems_message_queue_send( queue_id_prc1, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC1);
        }

        if (status != RTEMS_SUCCESSFUL) {
            PRINTF1("in AVF1 *** Error sending message to PRC1, code %d\n", status)
        }
    }
}

rtems_task prc1_task( rtems_task_argument lfrRequestedMode )
{
    char incomingData[MSG_QUEUE_SIZE_SEND];  // incoming data buffer
    size_t size;                             // size of the incoming TC packet
    asm_msg *incomingMsg;
    //
    unsigned char sid;
    rtems_status_code status;
    rtems_id queue_id_send;
    rtems_id queue_id_q_p1;
    bp_packet_with_spare    __attribute__((aligned(4))) packet_norm_bp1;
    bp_packet               __attribute__((aligned(4))) packet_norm_bp2;
    bp_packet               __attribute__((aligned(4))) packet_sbm_bp1;
    bp_packet               __attribute__((aligned(4))) packet_sbm_bp2;
    ring_node               *current_ring_node_to_send_asm_f1;
    float nbSMInASMNORM;
    float nbSMInASMSBM;

    size = 0;
    queue_id_send = RTEMS_ID_NONE;
    queue_id_q_p1 = RTEMS_ID_NONE;
    memset( &packet_norm_bp1,   0, sizeof(bp_packet_with_spare) );
    memset( &packet_norm_bp2,   0, sizeof(bp_packet) );
    memset( &packet_sbm_bp1,    0, sizeof(bp_packet) );
    memset( &packet_sbm_bp2,    0, sizeof(bp_packet) );

    // init the ring of the averaged spectral matrices which will be transmitted to the DPU
    init_ring( ring_to_send_asm_f1, NB_RING_NODES_ASM_F1, (volatile int*) buffer_asm_f1, TOTAL_SIZE_SM );
    current_ring_node_to_send_asm_f1 = ring_to_send_asm_f1;

    //*************
    // NORM headers
    BP_init_header_with_spare( &packet_norm_bp1,
                               APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F1,
                               PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1, NB_BINS_COMPRESSED_SM_F1 );
    BP_init_header( &packet_norm_bp2,
                    APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F1,
                    PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1, NB_BINS_COMPRESSED_SM_F1);

    //***********************
    // BURST and SBM2 headers
    if ( lfrRequestedMode == LFR_MODE_BURST )
    {
        BP_init_header( &packet_sbm_bp1,
                        APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F1,
                        PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
        BP_init_header( &packet_sbm_bp2,
                        APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F1,
                        PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
    }
    else if ( lfrRequestedMode == LFR_MODE_SBM2 )
    {
        BP_init_header( &packet_sbm_bp1,
                        APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F1,
                        PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
        BP_init_header( &packet_sbm_bp2,
                        APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F1,
                        PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
    }
    else
    {
        PRINTF1("in PRC1 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
    }

    status = get_message_queue_id_send( &queue_id_send );
    if (status != RTEMS_SUCCESSFUL)
    {
        PRINTF1("in PRC1 *** ERR get_message_queue_id_send %d\n", status)
    }
    status = get_message_queue_id_prc1( &queue_id_q_p1);
    if (status != RTEMS_SUCCESSFUL)
    {
        PRINTF1("in PRC1 *** ERR get_message_queue_id_prc1 %d\n", status)
    }

    BOOT_PRINTF1("in PRC1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)

    while(1){
        status = rtems_message_queue_receive( queue_id_q_p1, incomingData, &size, //************************************
                                             RTEMS_WAIT, RTEMS_NO_TIMEOUT );      // wait for a message coming from AVF0

        incomingMsg = (asm_msg*) incomingData;

        ASM_patch( incomingMsg->norm->matrix,      asm_f1_patched_norm      );
        ASM_patch( incomingMsg->burst_sbm->matrix, asm_f1_patched_burst_sbm );

        nbSMInASMNORM = incomingMsg->numberOfSMInASMNORM;
        nbSMInASMSBM = incomingMsg->numberOfSMInASMSBM;

        //***********
        //***********
        // BURST SBM2
        //***********
        //***********
        if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F1) )
        {
            sid = getSID( incomingMsg->event );
            // 1)  compress the matrix for Basic Parameters calculation
            ASM_compress_reorganize_and_divide_mask( asm_f1_patched_burst_sbm, compressed_sm_sbm_f1,
                                         nbSMInASMSBM,
                                         NB_BINS_COMPRESSED_SM_SBM_F1, NB_BINS_TO_AVERAGE_ASM_SBM_F1,
                                         ASM_F1_INDICE_START, CHANNELF1);
            // 2) compute the BP1 set
            BP1_set( compressed_sm_sbm_f1, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp1.data );
            // 3) send the BP1 set
            set_time( packet_sbm_bp1.time,            (unsigned char *) &incomingMsg->coarseTimeSBM );
            set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
            packet_sbm_bp1.pa_bia_status_info = pa_bia_status_info;
            packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
            BP_send_s1_s2( (char *) &packet_sbm_bp1, queue_id_send,
                     PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 + PACKET_LENGTH_DELTA,
                     sid );
            // 4) compute the BP2 set if needed
            if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F1) )
            {
                // 1) compute the BP2 set
                BP2_set( compressed_sm_sbm_f1, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp2.data );
                // 2) send the BP2 set
                set_time( packet_sbm_bp2.time,            (unsigned char *) &incomingMsg->coarseTimeSBM );
                set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
                packet_sbm_bp2.pa_bia_status_info = pa_bia_status_info;
                packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
                BP_send_s1_s2( (char *) &packet_sbm_bp2, queue_id_send,
                         PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1 + PACKET_LENGTH_DELTA,
                         sid );
            }
        }

        //*****
        //*****
        // NORM
        //*****
        //*****
        if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F1)
        {
            // 1)  compress the matrix for Basic Parameters calculation
            ASM_compress_reorganize_and_divide_mask( asm_f1_patched_norm, compressed_sm_norm_f1,
                                         nbSMInASMNORM,
                                         NB_BINS_COMPRESSED_SM_F1, NB_BINS_TO_AVERAGE_ASM_F1,
                                         ASM_F1_INDICE_START, CHANNELF1 );
            // 2) compute the BP1 set
            BP1_set( compressed_sm_norm_f1, k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp1.data );
            // 3) send the BP1 set
            set_time( packet_norm_bp1.time,            (unsigned char *) &incomingMsg->coarseTimeNORM );
            set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
            packet_norm_bp1.pa_bia_status_info = pa_bia_status_info;
            packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
            BP_send( (char *) &packet_norm_bp1, queue_id_send,
                     PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 + PACKET_LENGTH_DELTA,
                     SID_NORM_BP1_F1 );
            if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F1)
            {
                // 1) compute the BP2 set
                BP2_set( compressed_sm_norm_f1, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp2.data );
                // 2) send the BP2 set
                set_time( packet_norm_bp2.time,            (unsigned char *) &incomingMsg->coarseTimeNORM );
                set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
                packet_norm_bp2.pa_bia_status_info = pa_bia_status_info;
                packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
                BP_send( (char *) &packet_norm_bp2, queue_id_send,
                         PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 + PACKET_LENGTH_DELTA,
                         SID_NORM_BP2_F1 );
            }
        }

        if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F1)
        {
            // 1) reorganize the ASM and divide
            ASM_reorganize_and_divide( asm_f1_patched_norm,
                                       (float*) current_ring_node_to_send_asm_f1->buffer_address,
                                       nbSMInASMNORM );
            current_ring_node_to_send_asm_f1->coarseTime    = incomingMsg->coarseTimeNORM;
            current_ring_node_to_send_asm_f1->fineTime      = incomingMsg->fineTimeNORM;
            current_ring_node_to_send_asm_f1->sid           = SID_NORM_ASM_F1;

            // 3) send the spectral matrix packets
            status =  rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f1, sizeof( ring_node* ) );

            // change asm ring node
            current_ring_node_to_send_asm_f1 = current_ring_node_to_send_asm_f1->next;
        }

        update_queue_max_count( queue_id_q_p1, &hk_lfr_q_p1_fifo_size_max );

    }
}

//**********
// FUNCTIONS

void reset_nb_sm_f1( unsigned char lfrMode )
{
    nb_sm_before_f1.norm_bp1  = parameter_dump_packet.sy_lfr_n_bp_p0 * NB_SM_PER_S_F1;
    nb_sm_before_f1.norm_bp2  = parameter_dump_packet.sy_lfr_n_bp_p1 * NB_SM_PER_S_F1;
    nb_sm_before_f1.norm_asm  =
            ( (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256) + parameter_dump_packet.sy_lfr_n_asm_p[1]) * NB_SM_PER_S_F1;
    nb_sm_before_f1.sbm2_bp1  =  parameter_dump_packet.sy_lfr_s2_bp_p0 * NB_SM_PER_S_F1;
    nb_sm_before_f1.sbm2_bp2  =  parameter_dump_packet.sy_lfr_s2_bp_p1 * NB_SM_PER_S_F1;
    nb_sm_before_f1.burst_bp1 =  parameter_dump_packet.sy_lfr_b_bp_p0 * NB_SM_PER_S_F1;
    nb_sm_before_f1.burst_bp2 =  parameter_dump_packet.sy_lfr_b_bp_p1 * NB_SM_PER_S_F1;

    if (lfrMode == LFR_MODE_SBM2)
    {
        nb_sm_before_f1.burst_sbm_bp1 =  nb_sm_before_f1.sbm2_bp1;
        nb_sm_before_f1.burst_sbm_bp2 =  nb_sm_before_f1.sbm2_bp2;
    }
    else if (lfrMode == LFR_MODE_BURST)
    {
        nb_sm_before_f1.burst_sbm_bp1 =  nb_sm_before_f1.burst_bp1;
        nb_sm_before_f1.burst_sbm_bp2 =  nb_sm_before_f1.burst_bp2;
    }
    else
    {
        nb_sm_before_f1.burst_sbm_bp1 =  nb_sm_before_f1.burst_bp1;
        nb_sm_before_f1.burst_sbm_bp2 =  nb_sm_before_f1.burst_bp2;
    }
}

void init_k_coefficients_prc1( void )
{
    init_k_coefficients( k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1 );

    init_kcoeff_sbm_from_kcoeff_norm( k_coeff_intercalib_f1_norm, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_F1);
}


Generated by: GCOVR (Version 4.1)

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 "avf1_prc1.h"
35
36			nb_sm_before_bp_asm_f1 nb_sm_before_f1 = {0};
37
38			//***
39			// F1
40			ring_node_asm asm_ring_norm_f1 [ NB_RING_NODES_ASM_NORM_F1 ] = {0};
41			ring_node_asm asm_ring_burst_sbm_f1 [ NB_RING_NODES_ASM_BURST_SBM_F1 ] = {0};
42
43			ring_node ring_to_send_asm_f1 [ NB_RING_NODES_ASM_F1 ] = {0};
44			int buffer_asm_f1 [ NB_RING_NODES_ASM_F1 * TOTAL_SIZE_SM ] = {0};
45
46			float asm_f1_patched_norm [ TOTAL_SIZE_SM ] = {0};
47			float asm_f1_patched_burst_sbm [ TOTAL_SIZE_SM ] = {0};
48			float asm_f1_reorganized [ TOTAL_SIZE_SM ] = {0};
49
50			float compressed_sm_norm_f1[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F1] = {0};
51			float compressed_sm_sbm_f1 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 ] = {0};
52
53			float k_coeff_intercalib_f1_norm[ NB_BINS_COMPRESSED_SM_F1 * NB_K_COEFF_PER_BIN ] = {0}; // 13 * 32 = 416
54			float k_coeff_intercalib_f1_sbm[ NB_BINS_COMPRESSED_SM_SBM_F1 * NB_K_COEFF_PER_BIN ] = {0}; // 26 * 32 = 832
55
56			//************
57			// RTEMS TASKS
58
59		1606	rtems_task avf1_task( rtems_task_argument lfrRequestedMode )
60			{
61			int i;
62
63			rtems_event_set event_out;
64			rtems_status_code status;
65			rtems_id queue_id_prc1;
66			asm_msg msgForPRC;
67			ring_node *nodeForAveraging;
68			ring_node *ring_node_tab[NB_SM_BEFORE_AVF0_F1];
69			ring_node_asm *current_ring_node_asm_burst_sbm_f1;
70			ring_node_asm *current_ring_node_asm_norm_f1;
71
72			unsigned int nb_norm_bp1;
73			unsigned int nb_norm_bp2;
74			unsigned int nb_norm_asm;
75			unsigned int nb_sbm_bp1;
76			unsigned int nb_sbm_bp2;
77
78		1606	event_out = EVENT_SETS_NONE_PENDING;
79		1606	queue_id_prc1 = RTEMS_ID_NONE;
80
81		1606	nb_norm_bp1 = 0;
82		1606	nb_norm_bp2 = 0;
83		1606	nb_norm_asm = 0;
84		1606	nb_sbm_bp1 = 0;
85		1606	nb_sbm_bp2 = 0;
86
87		1606	reset_nb_sm_f1( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
88		1606	ASM_generic_init_ring( asm_ring_norm_f1, NB_RING_NODES_ASM_NORM_F1 );
89		1606	ASM_generic_init_ring( asm_ring_burst_sbm_f1, NB_RING_NODES_ASM_BURST_SBM_F1 );
90		1606	current_ring_node_asm_norm_f1 = asm_ring_norm_f1;
91		1606	current_ring_node_asm_burst_sbm_f1 = asm_ring_burst_sbm_f1;
92
93			BOOT_PRINTF1("in AVF1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
94
95		1606	status = get_message_queue_id_prc1( &queue_id_prc1 );
96			if (status != RTEMS_SUCCESSFUL)
97			{
98			PRINTF1("in AVF1 *** ERR get_message_queue_id_prc1 %d\n", status)
99			}
100
101			while(1){
102		216611	rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
103
104			//****************************************
105			// initialize the mesage for the MATR task
106		215184	msgForPRC.norm = current_ring_node_asm_norm_f1;
107		215184	msgForPRC.burst_sbm = current_ring_node_asm_burst_sbm_f1;
108		215184	msgForPRC.event = EVENT_SETS_NONE_PENDING; // this composite event will be sent to the PRC1 task
109			//
110			//****************************************
111
112		215184	nodeForAveraging = getRingNodeForAveraging( 1 );
113
114		215184	ring_node_tab[NB_SM_BEFORE_AVF0_F1-1] = nodeForAveraging;
115	✓✓	1721472	for ( i = 1; i < (NB_SM_BEFORE_AVF0_F1); i++ )
116			{
117		1506288	nodeForAveraging = nodeForAveraging->previous;
118		1506288	ring_node_tab[NB_SM_BEFORE_AVF0_F1 - i - 1] = nodeForAveraging;
119			}
120
121			// compute the average and store it in the averaged_sm_f1 buffer
122		215184	SM_average( current_ring_node_asm_norm_f1->matrix,
123			current_ring_node_asm_burst_sbm_f1->matrix,
124			ring_node_tab,
125			nb_norm_bp1, nb_sbm_bp1,
126			&msgForPRC, 1 ); // 1 => frequency channel 1
127
128			// update nb_average
129		215005	nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0_F1;
130		215005	nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0_F1;
131		215005	nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0_F1;
132		215005	nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0_F1;
133		215005	nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0_F1;
134
135	✓✓	215005	if (nb_sbm_bp1 == nb_sm_before_f1.burst_sbm_bp1)
136			{
137		106569	nb_sbm_bp1 = 0;
138			// set another ring for the ASM storage
139		106569	current_ring_node_asm_burst_sbm_f1 = current_ring_node_asm_burst_sbm_f1->next;
140	✓✓	106569	if ( lfrCurrentMode == LFR_MODE_BURST )
141			{
142		10506	msgForPRC.event = msgForPRC.event \| RTEMS_EVENT_BURST_BP1_F1;
143			}
144	✓✓	96063	else if ( lfrCurrentMode == LFR_MODE_SBM2 )
145			{
146		23776	msgForPRC.event = msgForPRC.event \| RTEMS_EVENT_SBM_BP1_F1;
147			}
148			}
149
150	✓✓	215005	if (nb_sbm_bp2 == nb_sm_before_f1.burst_sbm_bp2)
151			{
152		21079	nb_sbm_bp2 = 0;
153	✓✓	21079	if ( lfrCurrentMode == LFR_MODE_BURST )
154			{
155		2040	msgForPRC.event = msgForPRC.event \| RTEMS_EVENT_BURST_BP2_F1;
156			}
157	✓✓	19039	else if ( lfrCurrentMode == LFR_MODE_SBM2 )
158			{
159		4702	msgForPRC.event = msgForPRC.event \| RTEMS_EVENT_SBM_BP2_F1;
160			}
161			}
162
163	✓✓	215005	if (nb_norm_bp1 == nb_sm_before_f1.norm_bp1)
164			{
165		26547	nb_norm_bp1 = 0;
166			// set another ring for the ASM storage
167		26547	current_ring_node_asm_norm_f1 = current_ring_node_asm_norm_f1->next;
168	✓✓✓✓	35202	if ( (lfrCurrentMode == LFR_MODE_NORMAL)
169		35202	\|\| (lfrCurrentMode == LFR_MODE_SBM1) \|\| (lfrCurrentMode == LFR_MODE_SBM2) )
170			{
171		23971	msgForPRC.event = msgForPRC.event \| RTEMS_EVENT_NORM_BP1_F1;
172			}
173			}
174
175	✓✓	215005	if (nb_norm_bp2 == nb_sm_before_f1.norm_bp2)
176			{
177		5183	nb_norm_bp2 = 0;
178	✓✓✓✓	6857	if ( (lfrCurrentMode == LFR_MODE_NORMAL)
179		6857	\|\| (lfrCurrentMode == LFR_MODE_SBM1) \|\| (lfrCurrentMode == LFR_MODE_SBM2) )
180			{
181		4694	msgForPRC.event = msgForPRC.event \| RTEMS_EVENT_NORM_BP2_F1;
182			}
183			}
184
185	✓✓	215005	if (nb_norm_asm == nb_sm_before_f1.norm_asm)
186			{
187		17256	nb_norm_asm = 0;
188	✓✓✓✓	22712	if ( (lfrCurrentMode == LFR_MODE_NORMAL)
189		22712	\|\| (lfrCurrentMode == LFR_MODE_SBM1) \|\| (lfrCurrentMode == LFR_MODE_SBM2) )
190			{
191		16114	msgForPRC.event = msgForPRC.event \| RTEMS_EVENT_NORM_ASM_F1;
192			}
193			}
194
195			//*************************
196			// send the message to PRC
197	✓✓	215005	if (msgForPRC.event != EVENT_SETS_NONE_PENDING)
198			{
199		52301	status = rtems_message_queue_send( queue_id_prc1, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC1);
200			}
201
202			if (status != RTEMS_SUCCESSFUL) {
203			PRINTF1("in AVF1 *** Error sending message to PRC1, code %d\n", status)
204			}
205			}
206			}
207
208		1606	rtems_task prc1_task( rtems_task_argument lfrRequestedMode )
209			{
210			char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
211			size_t size; // size of the incoming TC packet
212			asm_msg *incomingMsg;
213			//
214			unsigned char sid;
215			rtems_status_code status;
216			rtems_id queue_id_send;
217			rtems_id queue_id_q_p1;
218			bp_packet_with_spare __attribute__((aligned(4))) packet_norm_bp1;
219			bp_packet __attribute__((aligned(4))) packet_norm_bp2;
220			bp_packet __attribute__((aligned(4))) packet_sbm_bp1;
221			bp_packet __attribute__((aligned(4))) packet_sbm_bp2;
222			ring_node *current_ring_node_to_send_asm_f1;
223			float nbSMInASMNORM;
224			float nbSMInASMSBM;
225
226		1606	size = 0;
227		1606	queue_id_send = RTEMS_ID_NONE;
228		1606	queue_id_q_p1 = RTEMS_ID_NONE;
229		1606	memset( &packet_norm_bp1, 0, sizeof(bp_packet_with_spare) );
230		1606	memset( &packet_norm_bp2, 0, sizeof(bp_packet) );
231		1606	memset( &packet_sbm_bp1, 0, sizeof(bp_packet) );
232		1606	memset( &packet_sbm_bp2, 0, sizeof(bp_packet) );
233
234			// init the ring of the averaged spectral matrices which will be transmitted to the DPU
235		1606	init_ring( ring_to_send_asm_f1, NB_RING_NODES_ASM_F1, (volatile int*) buffer_asm_f1, TOTAL_SIZE_SM );
236		1606	current_ring_node_to_send_asm_f1 = ring_to_send_asm_f1;
237
238			//*************
239			// NORM headers
240		1606	BP_init_header_with_spare( &packet_norm_bp1,
241			APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F1,
242			PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1, NB_BINS_COMPRESSED_SM_F1 );
243		1606	BP_init_header( &packet_norm_bp2,
244			APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F1,
245			PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1, NB_BINS_COMPRESSED_SM_F1);
246
247			//***********************
248			// BURST and SBM2 headers
249	✓✓	1606	if ( lfrRequestedMode == LFR_MODE_BURST )
250			{
251		398	BP_init_header( &packet_sbm_bp1,
252			APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F1,
253			PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
254		398	BP_init_header( &packet_sbm_bp2,
255			APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F1,
256			PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
257			}
258	✓✓	1208	else if ( lfrRequestedMode == LFR_MODE_SBM2 )
259			{
260		413	BP_init_header( &packet_sbm_bp1,
261			APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F1,
262			PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
263		413	BP_init_header( &packet_sbm_bp2,
264			APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F1,
265			PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
266			}
267			else
268			{
269			PRINTF1("in PRC1 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
270			}
271
272		1606	status = get_message_queue_id_send( &queue_id_send );
273			if (status != RTEMS_SUCCESSFUL)
274			{
275			PRINTF1("in PRC1 *** ERR get_message_queue_id_send %d\n", status)
276			}
277		1606	status = get_message_queue_id_prc1( &queue_id_q_p1);
278			if (status != RTEMS_SUCCESSFUL)
279			{
280			PRINTF1("in PRC1 *** ERR get_message_queue_id_prc1 %d\n", status)
281			}
282
283			BOOT_PRINTF1("in PRC1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
284
285			while(1){
286		53887	status = rtems_message_queue_receive( queue_id_q_p1, incomingData, &size, //************************************
287			RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
288
289		52281	incomingMsg = (asm_msg*) incomingData;
290
291		52281	ASM_patch( incomingMsg->norm->matrix, asm_f1_patched_norm );
292		52281	ASM_patch( incomingMsg->burst_sbm->matrix, asm_f1_patched_burst_sbm );
293
294		52281	nbSMInASMNORM = incomingMsg->numberOfSMInASMNORM;
295		52281	nbSMInASMSBM = incomingMsg->numberOfSMInASMSBM;
296
297			//***********
298			//***********
299			// BURST SBM2
300			//***********
301			//***********
302	✓✓	52281	if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F1) \|\| (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F1) )
303			{
304		34267	sid = getSID( incomingMsg->event );
305			// 1) compress the matrix for Basic Parameters calculation
306		34267	ASM_compress_reorganize_and_divide_mask( asm_f1_patched_burst_sbm, compressed_sm_sbm_f1,
307			nbSMInASMSBM,
308			NB_BINS_COMPRESSED_SM_SBM_F1, NB_BINS_TO_AVERAGE_ASM_SBM_F1,
309			ASM_F1_INDICE_START, CHANNELF1);
310			// 2) compute the BP1 set
311		34267	BP1_set( compressed_sm_sbm_f1, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp1.data );
312			// 3) send the BP1 set
313		34267	set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
314		34267	set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
315		34267	packet_sbm_bp1.pa_bia_status_info = pa_bia_status_info;
316		34267	packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
317		34267	BP_send_s1_s2( (char *) &packet_sbm_bp1, queue_id_send,
318			PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 + PACKET_LENGTH_DELTA,
319			sid );
320			// 4) compute the BP2 set if needed
321	✓✓	34267	if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F1) \|\| (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F1) )
322			{
323			// 1) compute the BP2 set
324		6742	BP2_set( compressed_sm_sbm_f1, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp2.data );
325			// 2) send the BP2 set
326		6742	set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
327		6742	set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
328		6742	packet_sbm_bp2.pa_bia_status_info = pa_bia_status_info;
329		6742	packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
330		6742	BP_send_s1_s2( (char *) &packet_sbm_bp2, queue_id_send,
331			PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1 + PACKET_LENGTH_DELTA,
332			sid );
333			}
334			}
335
336			//*****
337			//*****
338			// NORM
339			//*****
340			//*****
341	✓✓	52281	if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F1)
342			{
343			// 1) compress the matrix for Basic Parameters calculation
344		23966	ASM_compress_reorganize_and_divide_mask( asm_f1_patched_norm, compressed_sm_norm_f1,
345			nbSMInASMNORM,
346			NB_BINS_COMPRESSED_SM_F1, NB_BINS_TO_AVERAGE_ASM_F1,
347			ASM_F1_INDICE_START, CHANNELF1 );
348			// 2) compute the BP1 set
349		23966	BP1_set( compressed_sm_norm_f1, k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp1.data );
350			// 3) send the BP1 set
351		23966	set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
352		23966	set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
353		23966	packet_norm_bp1.pa_bia_status_info = pa_bia_status_info;
354		23966	packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
355		23966	BP_send( (char *) &packet_norm_bp1, queue_id_send,
356			PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 + PACKET_LENGTH_DELTA,
357			SID_NORM_BP1_F1 );
358	✓✓	23966	if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F1)
359			{
360			// 1) compute the BP2 set
361		4692	BP2_set( compressed_sm_norm_f1, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp2.data );
362			// 2) send the BP2 set
363		4692	set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
364		4692	set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
365		4692	packet_norm_bp2.pa_bia_status_info = pa_bia_status_info;
366		4692	packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
367		4692	BP_send( (char *) &packet_norm_bp2, queue_id_send,
368			PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 + PACKET_LENGTH_DELTA,
369			SID_NORM_BP2_F1 );
370			}
371			}
372
373	✓✓	52281	if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F1)
374			{
375			// 1) reorganize the ASM and divide
376		16112	ASM_reorganize_and_divide( asm_f1_patched_norm,
377		16112	(float*) current_ring_node_to_send_asm_f1->buffer_address,
378			nbSMInASMNORM );
379		16112	current_ring_node_to_send_asm_f1->coarseTime = incomingMsg->coarseTimeNORM;
380		16112	current_ring_node_to_send_asm_f1->fineTime = incomingMsg->fineTimeNORM;
381		16112	current_ring_node_to_send_asm_f1->sid = SID_NORM_ASM_F1;
382
383			// 3) send the spectral matrix packets
384		16112	status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f1, sizeof( ring_node* ) );
385
386			// change asm ring node
387		16112	current_ring_node_to_send_asm_f1 = current_ring_node_to_send_asm_f1->next;
388			}
389
390		52281	update_queue_max_count( queue_id_q_p1, &hk_lfr_q_p1_fifo_size_max );
391
392		52281	}
393			}
394
395			//**********
396			// FUNCTIONS
397
398		1606	void reset_nb_sm_f1( unsigned char lfrMode )
399			{
400		1606	nb_sm_before_f1.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * NB_SM_PER_S_F1;
401		1606	nb_sm_before_f1.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * NB_SM_PER_S_F1;
402		1606	nb_sm_before_f1.norm_asm =
403		1606	( (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256) + parameter_dump_packet.sy_lfr_n_asm_p[1]) * NB_SM_PER_S_F1;
404		1606	nb_sm_before_f1.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * NB_SM_PER_S_F1;
405		1606	nb_sm_before_f1.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * NB_SM_PER_S_F1;
406		1606	nb_sm_before_f1.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * NB_SM_PER_S_F1;
407		1606	nb_sm_before_f1.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * NB_SM_PER_S_F1;
408
409	✓✓	1606	if (lfrMode == LFR_MODE_SBM2)
410			{
411		413	nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.sbm2_bp1;
412		413	nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.sbm2_bp2;
413			}
414	✓✓	1193	else if (lfrMode == LFR_MODE_BURST)
415			{
416		398	nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
417		398	nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
418			}
419			else
420			{
421		795	nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
422		795	nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
423			}
424		1606	}
425
426		87	void init_k_coefficients_prc1( void )
427			{
428		87	init_k_coefficients( k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1 );
429
430		87	init_kcoeff_sbm_from_kcoeff_norm( k_coeff_intercalib_f1_norm, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_F1);
431		87	}