fsw_processing.h
371 lines
| 13.9 KiB
| text/x-c
|
CLexer
paul
|
r126 | #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 | ||||
|
paul
|
r227 | #include <stdio.h> | ||
|
paul
|
r126 | #include <math.h> | ||
|
paul
|
r175 | #include <grlib_regs.h> | ||
|
paul
|
r126 | |||
| #include "fsw_params.h" | ||||
|
paul
|
r318 | #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 | ||||
|
paul
|
r324 | #define NODE_0 0 | ||
| #define NODE_1 1 | ||||
| #define NODE_2 2 | ||||
| #define NODE_3 3 | ||||
| #define NODE_4 4 | ||||
| #define NODE_5 5 | ||||
| #define NODE_6 6 | ||||
| #define NODE_7 7 | ||||
|
paul
|
r126 | typedef struct ring_node_asm | ||
| { | ||||
| struct ring_node_asm *next; | ||||
| float matrix[ TOTAL_SIZE_SM ]; | ||||
| unsigned int status; | ||||
| } ring_node_asm; | ||||
|
paul
|
r133 | typedef struct | ||
|
paul
|
r126 | { | ||
|
paul
|
r179 | unsigned char targetLogicalAddress; | ||
| unsigned char protocolIdentifier; | ||||
| unsigned char reserved; | ||||
| unsigned char userApplication; | ||||
|
paul
|
r318 | unsigned char packetID[BYTES_PER_PACKETID]; | ||
| unsigned char packetSequenceControl[BYTES_PER_SEQ_CTRL]; | ||||
| unsigned char packetLength[BYTES_PER_PKT_LEN]; | ||||
|
paul
|
r179 | // DATA FIELD HEADER | ||
| unsigned char spare1_pusVersion_spare2; | ||||
| unsigned char serviceType; | ||||
| unsigned char serviceSubType; | ||||
| unsigned char destinationID; | ||||
|
paul
|
r318 | unsigned char time[BYTES_PER_TIME]; | ||
|
paul
|
r179 | // AUXILIARY HEADER | ||
| unsigned char sid; | ||||
|
paul
|
r283 | unsigned char pa_bia_status_info; | ||
|
paul
|
r192 | unsigned char sy_lfr_common_parameters_spare; | ||
| unsigned char sy_lfr_common_parameters; | ||||
|
paul
|
r318 | unsigned char acquisitionTime[BYTES_PER_TIME]; | ||
| unsigned char pa_lfr_bp_blk_nr[BYTES_PER_BLKNR]; | ||||
|
paul
|
r179 | // SOURCE DATA | ||
|
paul
|
r318 | unsigned char data[ MAX_SRC_DATA ]; // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1] | ||
|
paul
|
r126 | } bp_packet; | ||
|
paul
|
r133 | typedef struct | ||
|
paul
|
r126 | { | ||
|
paul
|
r181 | unsigned char targetLogicalAddress; | ||
| unsigned char protocolIdentifier; | ||||
| unsigned char reserved; | ||||
| unsigned char userApplication; | ||||
|
paul
|
r318 | unsigned char packetID[BYTES_PER_PACKETID]; | ||
| unsigned char packetSequenceControl[BYTES_PER_SEQ_CTRL]; | ||||
| unsigned char packetLength[BYTES_PER_PKT_LEN]; | ||||
|
paul
|
r181 | // DATA FIELD HEADER | ||
| unsigned char spare1_pusVersion_spare2; | ||||
| unsigned char serviceType; | ||||
| unsigned char serviceSubType; | ||||
| unsigned char destinationID; | ||||
|
paul
|
r318 | unsigned char time[BYTES_PER_TIME]; | ||
|
paul
|
r181 | // AUXILIARY HEADER | ||
| unsigned char sid; | ||||
|
paul
|
r283 | unsigned char pa_bia_status_info; | ||
|
paul
|
r192 | unsigned char sy_lfr_common_parameters_spare; | ||
| unsigned char sy_lfr_common_parameters; | ||||
|
paul
|
r318 | unsigned char acquisitionTime[BYTES_PER_TIME]; | ||
|
paul
|
r181 | unsigned char source_data_spare; | ||
|
paul
|
r318 | unsigned char pa_lfr_bp_blk_nr[BYTES_PER_BLKNR]; | ||
|
paul
|
r181 | // SOURCE DATA | ||
|
paul
|
r318 | unsigned char data[ MAX_SRC_DATA_WITH_SPARE ]; // 13 bins * 11 Bytes | ||
|
paul
|
r216 | } bp_packet_with_spare; // only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1 | ||
|
paul
|
r126 | |||
|
paul
|
r197 | typedef struct asm_msg | ||
|
paul
|
r126 | { | ||
| ring_node_asm *norm; | ||||
| ring_node_asm *burst_sbm; | ||||
| rtems_event_set event; | ||||
|
paul
|
r179 | unsigned int coarseTimeNORM; | ||
| unsigned int fineTimeNORM; | ||||
| unsigned int coarseTimeSBM; | ||||
| unsigned int fineTimeSBM; | ||||
|
paul
|
r289 | unsigned int numberOfSMInASMNORM; | ||
| unsigned int numberOfSMInASMSBM; | ||||
|
paul
|
r126 | } asm_msg; | ||
|
paul
|
r259 | extern unsigned char thisIsAnASMRestart; | ||
|
paul
|
r126 | extern volatile int sm_f0[ ]; | ||
| extern volatile int sm_f1[ ]; | ||||
| extern volatile int sm_f2[ ]; | ||||
|
paul
|
r289 | extern unsigned int acquisitionDurations[]; | ||
|
paul
|
r126 | |||
| // parameters | ||||
| extern struct param_local_str param_local; | ||||
|
paul
|
r195 | extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet; | ||
|
paul
|
r126 | |||
| // registers | ||||
| extern time_management_regs_t *time_management_regs; | ||||
|
paul
|
r179 | extern volatile spectral_matrix_regs_t *spectral_matrix_regs; | ||
|
paul
|
r126 | |||
|
paul
|
r318 | extern rtems_name misc_name[]; | ||
| extern rtems_id Task_id[]; /* array of task ids */ | ||||
|
paul
|
r126 | |||
|
paul
|
r179 | ring_node * getRingNodeForAveraging( unsigned char frequencyChannel); | ||
|
paul
|
r126 | // 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 ); | ||||
|
paul
|
r181 | void BP_init_header(bp_packet *packet, | ||
|
paul
|
r243 | unsigned int apid, unsigned char sid, | ||
| unsigned int packetLength , unsigned char blkNr); | ||||
|
paul
|
r181 | void BP_init_header_with_spare(bp_packet_with_spare *packet, | ||
|
paul
|
r243 | unsigned int apid, unsigned char sid, | ||
| unsigned int packetLength, unsigned char blkNr ); | ||||
|
paul
|
r133 | void BP_send( char *data, | ||
|
paul
|
r243 | rtems_id queue_id, | ||
|
paul
|
r133 | unsigned int nbBytesToSend , unsigned int sid ); | ||
|
paul
|
r243 | void BP_send_s1_s2(char *data, | ||
| rtems_id queue_id, | ||||
| unsigned int nbBytesToSend, unsigned int sid ); | ||||
|
paul
|
r126 | |||
| //****************** | ||||
| // general functions | ||||
|
paul
|
r171 | void reset_sm_status( void ); | ||
|
paul
|
r126 | void reset_spectral_matrix_regs( void ); | ||
| void set_time(unsigned char *time, unsigned char *timeInBuffer ); | ||||
|
paul
|
r139 | unsigned long long int get_acquisition_time( unsigned char *timePtr ); | ||
|
paul
|
r149 | unsigned char getSID( rtems_event_set event ); | ||
|
paul
|
r126 | |||
| 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 | ||||
|
paul
|
r172 | static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM, | ||
|
paul
|
r179 | ring_node *ring_node_tab[], | ||
| unsigned int nbAverageNORM, unsigned int nbAverageSBM, | ||||
|
paul
|
r289 | asm_msg *msgForMATR , unsigned char channel); | ||
|
paul
|
r182 | |||
| void ASM_patch( float *inputASM, float *outputASM ); | ||||
|
paul
|
r195 | |||
|
paul
|
r182 | void extractReImVectors(float *inputASM, float *outputASM, unsigned int asmComponent ); | ||
|
paul
|
r126 | static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized, | ||
|
paul
|
r243 | float divider ); | ||
|
paul
|
r195 | |||
|
paul
|
r126 | static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat, | ||
|
paul
|
r243 | float divider, | ||
| unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart); | ||||
|
paul
|
r195 | |||
|
paul
|
r126 | static inline void ASM_convert(volatile float *input_matrix, char *output_matrix); | ||
|
paul
|
r289 | unsigned char acquisitionTimeIsValid(unsigned int coarseTime, unsigned int fineTime, unsigned char channel); | ||
|
paul
|
r173 | void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM, | ||
|
paul
|
r179 | ring_node *ring_node_tab[], | ||
| unsigned int nbAverageNORM, unsigned int nbAverageSBM, | ||||
|
paul
|
r289 | asm_msg *msgForMATR, unsigned char channel ) | ||
|
paul
|
r126 | { | ||
| float sum; | ||||
| unsigned int i; | ||||
|
paul
|
r289 | unsigned int k; | ||
|
paul
|
r318 | unsigned char incomingSMIsValid[NB_SM_BEFORE_AVF0_F1]; | ||
|
paul
|
r289 | unsigned int numberOfValidSM; | ||
| unsigned char isValid; | ||||
|
paul
|
r126 | |||
|
paul
|
r289 | //************** | ||
| // PAS FILTERING | ||||
| // check acquisitionTime of the incoming data | ||||
| numberOfValidSM = 0; | ||||
|
paul
|
r318 | for (k=0; k<NB_SM_BEFORE_AVF0_F1; k++) | ||
|
paul
|
r289 | { | ||
| isValid = acquisitionTimeIsValid( ring_node_tab[k]->coarseTime, ring_node_tab[k]->fineTime, channel ); | ||||
| incomingSMIsValid[k] = isValid; | ||||
| numberOfValidSM = numberOfValidSM + isValid; | ||||
| } | ||||
| //************************ | ||||
| // AVERAGE SPECTRAL MATRIX | ||||
|
paul
|
r126 | for(i=0; i<TOTAL_SIZE_SM; i++) | ||
| { | ||||
|
paul
|
r325 | sum = INIT_FLOAT; | ||
| for ( k = 0; k < NB_SM_BEFORE_AVF0_F1; k++ ) | ||||
| { | ||||
| if (incomingSMIsValid[k] == 1) | ||||
| { | ||||
| sum = sum + ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ] ; | ||||
| } | ||||
| } | ||||
|
paul
|
r126 | |||
|
paul
|
r150 | if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) ) | ||
|
paul
|
r126 | { | ||
|
paul
|
r150 | averaged_spec_mat_NORM[ i ] = sum; | ||
| averaged_spec_mat_SBM[ i ] = sum; | ||||
|
paul
|
r179 | 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; | ||||
|
paul
|
r126 | } | ||
|
paul
|
r150 | else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) ) | ||
|
paul
|
r126 | { | ||
|
paul
|
r150 | averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum ); | ||
| averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum ); | ||||
|
paul
|
r126 | } | ||
|
paul
|
r150 | else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) ) | ||
|
paul
|
r126 | { | ||
|
paul
|
r150 | averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum ); | ||
| averaged_spec_mat_SBM[ i ] = sum; | ||||
|
paul
|
r179 | msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime; | ||
| msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime; | ||||
|
paul
|
r126 | } | ||
| else | ||||
| { | ||||
|
paul
|
r223 | averaged_spec_mat_NORM[ i ] = sum; | ||
| averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum ); | ||||
|
paul
|
r231 | msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime; | ||
| msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime; | ||||
|
paul
|
r243 | // PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM) | ||
|
paul
|
r126 | } | ||
| } | ||||
|
paul
|
r289 | //******************* | ||
| // UPDATE SM COUNTERS | ||||
| if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) ) | ||||
| { | ||||
| msgForMATR->numberOfSMInASMNORM = numberOfValidSM; | ||||
| msgForMATR->numberOfSMInASMSBM = numberOfValidSM; | ||||
| } | ||||
| else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) ) | ||||
|
paul
|
r172 | { | ||
|
paul
|
r289 | 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; | ||||
|
paul
|
r172 | } | ||
| } | ||||
|
paul
|
r126 | void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider ) | ||
| { | ||||
| int frequencyBin; | ||||
| int asmComponent; | ||||
|
paul
|
r179 | unsigned int offsetASM; | ||
| unsigned int offsetASMReorganized; | ||||
|
paul
|
r126 | |||
|
paul
|
r179 | // BUILD DATA | ||
|
paul
|
r126 | for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++) | ||
| { | ||||
| for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ ) | ||||
| { | ||||
|
paul
|
r179 | offsetASMReorganized = | ||
|
paul
|
r318 | (frequencyBin * NB_VALUES_PER_SM) | ||
|
paul
|
r126 | + asmComponent; | ||
|
paul
|
r179 | offsetASM = | ||
|
paul
|
r318 | (asmComponent * NB_BINS_PER_SM) | ||
|
paul
|
r126 | + frequencyBin; | ||
|
paul
|
r318 | if ( divider != INIT_FLOAT ) | ||
| { | ||||
| averaged_spec_mat_reorganized[offsetASMReorganized ] = averaged_spec_mat[ offsetASM ] / divider; | ||||
| } | ||||
| else | ||||
| { | ||||
| averaged_spec_mat_reorganized[offsetASMReorganized ] = INIT_FLOAT; | ||||
| } | ||||
|
paul
|
r126 | } | ||
| } | ||||
| } | ||||
| void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider, | ||||
|
paul
|
r243 | unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart ) | ||
|
paul
|
r126 | { | ||
| int frequencyBin; | ||||
| int asmComponent; | ||||
| int offsetASM; | ||||
| int offsetCompressed; | ||||
| int k; | ||||
|
paul
|
r179 | // BUILD DATA | ||
|
paul
|
r126 | for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++) | ||
| { | ||||
| for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ ) | ||||
| { | ||||
| offsetCompressed = // NO TIME OFFSET | ||||
|
paul
|
r318 | (frequencyBin * NB_VALUES_PER_SM) | ||
|
paul
|
r126 | + asmComponent; | ||
| offsetASM = // NO TIME OFFSET | ||||
|
paul
|
r318 | (asmComponent * NB_BINS_PER_SM) | ||
|
paul
|
r126 | + ASMIndexStart | ||
|
paul
|
r318 | + (frequencyBin * nbBinsToAverage); | ||
|
paul
|
r126 | compressed_spec_mat[ offsetCompressed ] = 0; | ||
| for ( k = 0; k < nbBinsToAverage; k++ ) | ||||
| { | ||||
| compressed_spec_mat[offsetCompressed ] = | ||||
| ( compressed_spec_mat[ offsetCompressed ] | ||||
|
paul
|
r243 | + averaged_spec_mat[ offsetASM + k ] ); | ||
|
paul
|
r126 | } | ||
|
paul
|
r179 | compressed_spec_mat[ offsetCompressed ] = | ||
| compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage); | ||||
|
paul
|
r126 | } | ||
| } | ||||
| } | ||||
| 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++) | ||||
| { | ||||
|
paul
|
r318 | offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ; | ||
| offsetOutput = SM_BYTES_PER_VAL * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ; | ||||
|
paul
|
r126 | 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 | ||||
| } | ||||
| } | ||||
| } | ||||
|
paul
|
r195 | void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat, | ||
|
paul
|
r243 | float divider, | ||
| unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart, unsigned char channel); | ||||
|
paul
|
r195 | |||
|
paul
|
r236 | int getFBinMask(int k, unsigned char channel); | ||
|
paul
|
r195 | |||
|
paul
|
r214 | void init_kcoeff_sbm_from_kcoeff_norm( float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm); | ||
|
paul
|
r126 | #endif // FSW_PROCESSING_H_INCLUDED | ||