|
|
#ifndef FSW_PROCESSING_H_INCLUDED
|
|
|
#define FSW_PROCESSING_H_INCLUDED
|
|
|
|
|
|
#include <rtems.h>
|
|
|
#include <grspw.h>
|
|
|
#include <math.h>
|
|
|
#include <stdlib.h> // abs() is in the stdlib
|
|
|
#include <stdio.h>
|
|
|
#include <math.h>
|
|
|
#include <grlib_regs.h>
|
|
|
|
|
|
#include "fsw_params.h"
|
|
|
|
|
|
#define SBM_COEFF_PER_NORM_COEFF 2
|
|
|
#define MAX_SRC_DATA 780 // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1]
|
|
|
#define MAX_SRC_DATA_WITH_SPARE 143 // 13 bins * 11 Bytes
|
|
|
|
|
|
typedef struct ring_node_asm
|
|
|
{
|
|
|
struct ring_node_asm *next;
|
|
|
float matrix[ TOTAL_SIZE_SM ];
|
|
|
unsigned int status;
|
|
|
} ring_node_asm;
|
|
|
|
|
|
typedef struct
|
|
|
{
|
|
|
unsigned char targetLogicalAddress;
|
|
|
unsigned char protocolIdentifier;
|
|
|
unsigned char reserved;
|
|
|
unsigned char userApplication;
|
|
|
unsigned char packetID[BYTES_PER_PACKETID];
|
|
|
unsigned char packetSequenceControl[BYTES_PER_SEQ_CTRL];
|
|
|
unsigned char packetLength[BYTES_PER_PKT_LEN];
|
|
|
// DATA FIELD HEADER
|
|
|
unsigned char spare1_pusVersion_spare2;
|
|
|
unsigned char serviceType;
|
|
|
unsigned char serviceSubType;
|
|
|
unsigned char destinationID;
|
|
|
unsigned char time[BYTES_PER_TIME];
|
|
|
// AUXILIARY HEADER
|
|
|
unsigned char sid;
|
|
|
unsigned char pa_bia_status_info;
|
|
|
unsigned char sy_lfr_common_parameters_spare;
|
|
|
unsigned char sy_lfr_common_parameters;
|
|
|
unsigned char acquisitionTime[BYTES_PER_TIME];
|
|
|
unsigned char pa_lfr_bp_blk_nr[BYTES_PER_BLKNR];
|
|
|
// SOURCE DATA
|
|
|
unsigned char data[ MAX_SRC_DATA ]; // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1]
|
|
|
} bp_packet;
|
|
|
|
|
|
typedef struct
|
|
|
{
|
|
|
unsigned char targetLogicalAddress;
|
|
|
unsigned char protocolIdentifier;
|
|
|
unsigned char reserved;
|
|
|
unsigned char userApplication;
|
|
|
unsigned char packetID[BYTES_PER_PACKETID];
|
|
|
unsigned char packetSequenceControl[BYTES_PER_SEQ_CTRL];
|
|
|
unsigned char packetLength[BYTES_PER_PKT_LEN];
|
|
|
// DATA FIELD HEADER
|
|
|
unsigned char spare1_pusVersion_spare2;
|
|
|
unsigned char serviceType;
|
|
|
unsigned char serviceSubType;
|
|
|
unsigned char destinationID;
|
|
|
unsigned char time[BYTES_PER_TIME];
|
|
|
// AUXILIARY HEADER
|
|
|
unsigned char sid;
|
|
|
unsigned char pa_bia_status_info;
|
|
|
unsigned char sy_lfr_common_parameters_spare;
|
|
|
unsigned char sy_lfr_common_parameters;
|
|
|
unsigned char acquisitionTime[BYTES_PER_TIME];
|
|
|
unsigned char source_data_spare;
|
|
|
unsigned char pa_lfr_bp_blk_nr[BYTES_PER_BLKNR];
|
|
|
// SOURCE DATA
|
|
|
unsigned char data[ MAX_SRC_DATA_WITH_SPARE ]; // 13 bins * 11 Bytes
|
|
|
} bp_packet_with_spare; // only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1
|
|
|
|
|
|
typedef struct asm_msg
|
|
|
{
|
|
|
ring_node_asm *norm;
|
|
|
ring_node_asm *burst_sbm;
|
|
|
rtems_event_set event;
|
|
|
unsigned int coarseTimeNORM;
|
|
|
unsigned int fineTimeNORM;
|
|
|
unsigned int coarseTimeSBM;
|
|
|
unsigned int fineTimeSBM;
|
|
|
unsigned int numberOfSMInASMNORM;
|
|
|
unsigned int numberOfSMInASMSBM;
|
|
|
} asm_msg;
|
|
|
|
|
|
extern unsigned char thisIsAnASMRestart;
|
|
|
|
|
|
extern volatile int sm_f0[ ];
|
|
|
extern volatile int sm_f1[ ];
|
|
|
extern volatile int sm_f2[ ];
|
|
|
extern unsigned int acquisitionDurations[];
|
|
|
|
|
|
// parameters
|
|
|
extern struct param_local_str param_local;
|
|
|
extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
|
|
|
|
|
|
// registers
|
|
|
extern time_management_regs_t *time_management_regs;
|
|
|
extern volatile spectral_matrix_regs_t *spectral_matrix_regs;
|
|
|
|
|
|
extern rtems_name misc_name[];
|
|
|
extern rtems_id Task_id[]; /* array of task ids */
|
|
|
|
|
|
ring_node * getRingNodeForAveraging( unsigned char frequencyChannel);
|
|
|
// ISR
|
|
|
rtems_isr spectral_matrices_isr( rtems_vector_number vector );
|
|
|
|
|
|
//******************
|
|
|
// Spectral Matrices
|
|
|
void reset_nb_sm( void );
|
|
|
// SM
|
|
|
void SM_init_rings( void );
|
|
|
void SM_reset_current_ring_nodes( void );
|
|
|
// ASM
|
|
|
void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes );
|
|
|
|
|
|
//*****************
|
|
|
// Basic Parameters
|
|
|
|
|
|
void BP_reset_current_ring_nodes( void );
|
|
|
void BP_init_header(bp_packet *packet,
|
|
|
unsigned int apid, unsigned char sid,
|
|
|
unsigned int packetLength , unsigned char blkNr);
|
|
|
void BP_init_header_with_spare(bp_packet_with_spare *packet,
|
|
|
unsigned int apid, unsigned char sid,
|
|
|
unsigned int packetLength, unsigned char blkNr );
|
|
|
void BP_send( char *data,
|
|
|
rtems_id queue_id,
|
|
|
unsigned int nbBytesToSend , unsigned int sid );
|
|
|
void BP_send_s1_s2(char *data,
|
|
|
rtems_id queue_id,
|
|
|
unsigned int nbBytesToSend, unsigned int sid );
|
|
|
|
|
|
//******************
|
|
|
// general functions
|
|
|
void reset_sm_status( void );
|
|
|
void reset_spectral_matrix_regs( void );
|
|
|
void set_time(unsigned char *time, unsigned char *timeInBuffer );
|
|
|
unsigned long long int get_acquisition_time( unsigned char *timePtr );
|
|
|
unsigned char getSID( rtems_event_set event );
|
|
|
|
|
|
extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
|
|
|
extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
|
|
|
|
|
|
//***************************************
|
|
|
// DEFINITIONS OF STATIC INLINE FUNCTIONS
|
|
|
static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
|
|
|
ring_node *ring_node_tab[],
|
|
|
unsigned int nbAverageNORM, unsigned int nbAverageSBM,
|
|
|
asm_msg *msgForMATR , unsigned char channel);
|
|
|
|
|
|
void ASM_patch( float *inputASM, float *outputASM );
|
|
|
|
|
|
void extractReImVectors(float *inputASM, float *outputASM, unsigned int asmComponent );
|
|
|
|
|
|
static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized,
|
|
|
float divider );
|
|
|
|
|
|
static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat,
|
|
|
float divider,
|
|
|
unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
|
|
|
|
|
|
static inline void ASM_convert(volatile float *input_matrix, char *output_matrix);
|
|
|
|
|
|
unsigned char isPolluted( u_int64_t t0, u_int64_t t1, u_int64_t tbad0, u_int64_t tbad1 );
|
|
|
|
|
|
unsigned char acquisitionTimeIsValid(unsigned int coarseTime, unsigned int fineTime, unsigned char channel);
|
|
|
|
|
|
void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
|
|
|
ring_node *ring_node_tab[],
|
|
|
unsigned int nbAverageNORM, unsigned int nbAverageSBM,
|
|
|
asm_msg *msgForMATR, unsigned char channel )
|
|
|
{
|
|
|
float sum;
|
|
|
unsigned int i;
|
|
|
unsigned int k;
|
|
|
unsigned char incomingSMIsValid[NB_SM_BEFORE_AVF0_F1];
|
|
|
unsigned int numberOfValidSM;
|
|
|
unsigned char isValid;
|
|
|
|
|
|
//**************
|
|
|
// PAS FILTERING
|
|
|
// check acquisitionTime of the incoming data
|
|
|
numberOfValidSM = 0;
|
|
|
for (k=0; k<NB_SM_BEFORE_AVF0_F1; k++)
|
|
|
{
|
|
|
isValid = acquisitionTimeIsValid( ring_node_tab[k]->coarseTime, ring_node_tab[k]->fineTime, channel );
|
|
|
incomingSMIsValid[k] = isValid;
|
|
|
numberOfValidSM = numberOfValidSM + isValid;
|
|
|
}
|
|
|
|
|
|
//************************
|
|
|
// AVERAGE SPECTRAL MATRIX
|
|
|
for(i=0; i<TOTAL_SIZE_SM; i++)
|
|
|
{
|
|
|
sum = INIT_FLOAT;
|
|
|
for ( k = 0; k < NB_SM_BEFORE_AVF0_F1; k++ )
|
|
|
{
|
|
|
if (incomingSMIsValid[k] == MATRIX_IS_NOT_POLLUTED)
|
|
|
{
|
|
|
sum = sum + ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ] ;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
|
|
|
{
|
|
|
averaged_spec_mat_NORM[ i ] = sum;
|
|
|
averaged_spec_mat_SBM[ i ] = sum;
|
|
|
msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
|
|
|
msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
|
|
|
msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
|
|
|
msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
|
|
|
}
|
|
|
else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
|
|
|
{
|
|
|
averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
|
|
|
averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
|
|
|
}
|
|
|
else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
|
|
|
{
|
|
|
averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
|
|
|
averaged_spec_mat_SBM[ i ] = sum;
|
|
|
msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
|
|
|
msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
averaged_spec_mat_NORM[ i ] = sum;
|
|
|
averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
|
|
|
msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
|
|
|
msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
|
|
|
// PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
|
|
|
}
|
|
|
}
|
|
|
|
|
|
//*******************
|
|
|
// UPDATE SM COUNTERS
|
|
|
if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
|
|
|
{
|
|
|
msgForMATR->numberOfSMInASMNORM = numberOfValidSM;
|
|
|
msgForMATR->numberOfSMInASMSBM = numberOfValidSM;
|
|
|
}
|
|
|
else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
|
|
|
{
|
|
|
msgForMATR->numberOfSMInASMNORM = msgForMATR->numberOfSMInASMNORM + numberOfValidSM;
|
|
|
msgForMATR->numberOfSMInASMSBM = msgForMATR->numberOfSMInASMSBM + numberOfValidSM;
|
|
|
}
|
|
|
else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
|
|
|
{
|
|
|
msgForMATR->numberOfSMInASMNORM = msgForMATR->numberOfSMInASMNORM + numberOfValidSM;
|
|
|
msgForMATR->numberOfSMInASMSBM = numberOfValidSM;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
msgForMATR->numberOfSMInASMNORM = numberOfValidSM;
|
|
|
msgForMATR->numberOfSMInASMSBM = msgForMATR->numberOfSMInASMSBM + numberOfValidSM;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
|
|
|
{
|
|
|
int frequencyBin;
|
|
|
int asmComponent;
|
|
|
unsigned int offsetASM;
|
|
|
unsigned int offsetASMReorganized;
|
|
|
|
|
|
// BUILD DATA
|
|
|
for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
|
|
|
{
|
|
|
for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
|
|
|
{
|
|
|
offsetASMReorganized =
|
|
|
(frequencyBin * NB_VALUES_PER_SM)
|
|
|
+ asmComponent;
|
|
|
offsetASM =
|
|
|
(asmComponent * NB_BINS_PER_SM)
|
|
|
+ frequencyBin;
|
|
|
if ( divider != INIT_FLOAT )
|
|
|
{
|
|
|
averaged_spec_mat_reorganized[offsetASMReorganized ] = averaged_spec_mat[ offsetASM ] / divider;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
averaged_spec_mat_reorganized[offsetASMReorganized ] = INIT_FLOAT;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
|
|
|
unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
|
|
|
{
|
|
|
int frequencyBin;
|
|
|
int asmComponent;
|
|
|
int offsetASM;
|
|
|
int offsetCompressed;
|
|
|
int k;
|
|
|
|
|
|
// BUILD DATA
|
|
|
for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
|
|
|
{
|
|
|
for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
|
|
|
{
|
|
|
offsetCompressed = // NO TIME OFFSET
|
|
|
(frequencyBin * NB_VALUES_PER_SM)
|
|
|
+ asmComponent;
|
|
|
offsetASM = // NO TIME OFFSET
|
|
|
(asmComponent * NB_BINS_PER_SM)
|
|
|
+ ASMIndexStart
|
|
|
+ (frequencyBin * nbBinsToAverage);
|
|
|
compressed_spec_mat[ offsetCompressed ] = 0;
|
|
|
for ( k = 0; k < nbBinsToAverage; k++ )
|
|
|
{
|
|
|
compressed_spec_mat[offsetCompressed ] =
|
|
|
( compressed_spec_mat[ offsetCompressed ]
|
|
|
+ averaged_spec_mat[ offsetASM + k ] );
|
|
|
}
|
|
|
compressed_spec_mat[ offsetCompressed ] =
|
|
|
compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
void ASM_convert( volatile float *input_matrix, char *output_matrix)
|
|
|
{
|
|
|
unsigned int frequencyBin;
|
|
|
unsigned int asmComponent;
|
|
|
char * pt_char_input;
|
|
|
char * pt_char_output;
|
|
|
unsigned int offsetInput;
|
|
|
unsigned int offsetOutput;
|
|
|
|
|
|
pt_char_input = (char*) &input_matrix;
|
|
|
pt_char_output = (char*) &output_matrix;
|
|
|
|
|
|
// convert all other data
|
|
|
for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++)
|
|
|
{
|
|
|
for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++)
|
|
|
{
|
|
|
offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ;
|
|
|
offsetOutput = SM_BYTES_PER_VAL * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ;
|
|
|
pt_char_input = (char*) &input_matrix [ offsetInput ];
|
|
|
pt_char_output = (char*) &output_matrix[ offsetOutput ];
|
|
|
pt_char_output[0] = pt_char_input[0]; // bits 31 downto 24 of the float
|
|
|
pt_char_output[1] = pt_char_input[1]; // bits 23 downto 16 of the float
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat,
|
|
|
float divider,
|
|
|
unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart, unsigned char channel);
|
|
|
|
|
|
int getFBinMask(int k, unsigned char channel);
|
|
|
|
|
|
void init_kcoeff_sbm_from_kcoeff_norm( float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm);
|
|
|
|
|
|
#endif // FSW_PROCESSING_H_INCLUDED
|
|
|
|
