diff --git a/FSW-qt/Makefile b/FSW-qt/Makefile --- a/FSW-qt/Makefile +++ b/FSW-qt/Makefile @@ -1,6 +1,6 @@ ############################################################################# # Makefile for building: bin/fsw -# Generated by qmake (2.01a) (Qt 4.8.5) on: Thu Apr 3 10:09:31 2014 +# Generated by qmake (2.01a) (Qt 4.8.5) on: Thu Apr 3 13:43:01 2014 # Project: fsw-qt.pro # Template: app # Command: /usr/bin/qmake-qt4 -spec /usr/lib64/qt4/mkspecs/linux-g++ -o Makefile fsw-qt.pro @@ -10,7 +10,7 @@ CC = sparc-rtems-gcc CXX = sparc-rtems-g++ -DEFINES = -DSW_VERSION_N1=1 -DSW_VERSION_N2=0 -DSW_VERSION_N3=0 -DSW_VERSION_N4=5 -DPRINT_MESSAGES_ON_CONSOLE +DEFINES = -DSW_VERSION_N1=1 -DSW_VERSION_N2=0 -DSW_VERSION_N3=0 -DSW_VERSION_N4=6 -DPRINT_MESSAGES_ON_CONSOLE -DPRINT_TASK_STATISTICS CFLAGS = -pipe -O3 -Wall $(DEFINES) CXXFLAGS = -pipe -O3 -Wall $(DEFINES) INCPATH = -I/usr/lib64/qt4/mkspecs/linux-g++ -I. -I../src -I../header -I../../LFR_basic-parameters diff --git a/FSW-qt/fsw-qt.pro b/FSW-qt/fsw-qt.pro --- a/FSW-qt/fsw-qt.pro +++ b/FSW-qt/fsw-qt.pro @@ -1,7 +1,7 @@ TEMPLATE = app # CONFIG += console v8 sim # CONFIG options = verbose *** boot_messages *** debug_messages *** cpu_usage_report *** stack_report *** vhdl_dev *** debug_tch -CONFIG += console verbose +CONFIG += console verbose cpu_usage_report CONFIG -= qt include(./sparc.pri) @@ -11,7 +11,7 @@ SWVERSION=-1-0 DEFINES += SW_VERSION_N1=1 # major DEFINES += SW_VERSION_N2=0 # minor DEFINES += SW_VERSION_N3=0 # patch -DEFINES += SW_VERSION_N4=5 # internal +DEFINES += SW_VERSION_N4=6 # internal contains( CONFIG, debug_tch ) { DEFINES += DEBUG_TCH diff --git a/FSW-qt/fsw-qt.pro.user b/FSW-qt/fsw-qt.pro.user --- a/FSW-qt/fsw-qt.pro.user +++ b/FSW-qt/fsw-qt.pro.user @@ -1,6 +1,6 @@ - + ProjectExplorer.Project.ActiveTarget diff --git a/header/ccsds_types.h b/header/ccsds_types.h --- a/header/ccsds_types.h +++ b/header/ccsds_types.h @@ -195,17 +195,19 @@ enum apid_destid{ #define TM_HEADER_LEN 16 #define HEADER_LENGTH_TM_LFR_SCIENCE_ASM 28 // PACKET_LENGTH -#define PACKET_LENGTH_TC_EXE_SUCCESS (20 - CCSDS_TC_TM_PACKET_OFFSET) -#define PACKET_LENGTH_TC_EXE_INCONSISTENT (26 - CCSDS_TC_TM_PACKET_OFFSET) -#define PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE (26 - CCSDS_TC_TM_PACKET_OFFSET) -#define PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED (24 - CCSDS_TC_TM_PACKET_OFFSET) -#define PACKET_LENGTH_TC_EXE_ERROR (24 - CCSDS_TC_TM_PACKET_OFFSET) -#define PACKET_LENGTH_TC_EXE_CORRUPTED (32 - CCSDS_TC_TM_PACKET_OFFSET) -#define PACKET_LENGTH_HK (124 - CCSDS_TC_TM_PACKET_OFFSET) -#define PACKET_LENGTH_PARAMETER_DUMP (36 - CCSDS_TC_TM_PACKET_OFFSET) -#define PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0 2221 // 44 * 25 * 2 + 28 - 7 -#define PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1 2621 // 52 * 25 * 2 + 28 - 7 -#define PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2 2421 // 48 * 25 * 2 + 28 - 7 +#define PACKET_LENGTH_TC_EXE_SUCCESS (20 - CCSDS_TC_TM_PACKET_OFFSET) +#define PACKET_LENGTH_TC_EXE_INCONSISTENT (26 - CCSDS_TC_TM_PACKET_OFFSET) +#define PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE (26 - CCSDS_TC_TM_PACKET_OFFSET) +#define PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED (24 - CCSDS_TC_TM_PACKET_OFFSET) +#define PACKET_LENGTH_TC_EXE_ERROR (24 - CCSDS_TC_TM_PACKET_OFFSET) +#define PACKET_LENGTH_TC_EXE_CORRUPTED (32 - CCSDS_TC_TM_PACKET_OFFSET) +#define PACKET_LENGTH_HK (124 - CCSDS_TC_TM_PACKET_OFFSET) +#define PACKET_LENGTH_PARAMETER_DUMP (36 - CCSDS_TC_TM_PACKET_OFFSET) +#define PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0 (2228 - CCSDS_TC_TM_PACKET_OFFSET) // 44 * 25 * 2 + 28 - 7 +#define PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1 (2628 - CCSDS_TC_TM_PACKET_OFFSET) // 52 * 25 * 2 + 28 - 7 +#define PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2 (2428 - CCSDS_TC_TM_PACKET_OFFSET) // 48 * 25 * 2 + 28 - 7 +#define PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0 (126 - CCSDS_TC_TM_PACKET_OFFSET) // 11 * 9 + 27 - 7 +#define PACKET_LENGTH_TM_LFR_SCIENCE_SBM1_BP1_F0 (224 - CCSDS_TC_TM_PACKET_OFFSET) // 22 * 9 + 26 - 7 #define SPARE1_PUSVERSION_SPARE2 0x10 @@ -466,7 +468,28 @@ typedef struct { unsigned char acquisitionTime[6]; unsigned char spare_source_data; unsigned char pa_lfr_bp_blk_nr[2]; -} Header_TM_LFR_SCIENCE_BP_t; +} Header_TM_LFR_SCIENCE_BP_NORM_t; + +typedef struct { + unsigned char targetLogicalAddress; + unsigned char protocolIdentifier; + unsigned char reserved; + unsigned char userApplication; + unsigned char packetID[2]; + unsigned char packetSequenceControl[2]; + unsigned char packetLength[2]; + // DATA FIELD HEADER + unsigned char spare1_pusVersion_spare2; + unsigned char serviceType; + unsigned char serviceSubType; + unsigned char destinationID; + unsigned char time[6]; + // AUXILIARY HEADER + unsigned char sid; + unsigned char biaStatusInfo; + unsigned char acquisitionTime[6]; + unsigned char pa_lfr_bp_blk_nr[2]; +} Header_TM_LFR_SCIENCE_BP_SBM_t; typedef struct { //targetLogicalAddress is removed by the grspw module diff --git a/header/fsw_params.h b/header/fsw_params.h --- a/header/fsw_params.h +++ b/header/fsw_params.h @@ -28,6 +28,26 @@ typedef struct ring_node_sm unsigned int fineTime; } ring_node_sm; +typedef struct ring_node_bp +{ + struct ring_node_bp *previous; + struct ring_node_bp *next; + unsigned int status; + unsigned int coarseTime; + unsigned int fineTime; + Header_TM_LFR_SCIENCE_BP_SBM_t header; + unsigned char data[ 9 * 22 ]; +} ring_node_bp; + +typedef struct +{ + unsigned int status; + unsigned int coarseTime; + unsigned int fineTime; + Header_TM_LFR_SCIENCE_BP_NORM_t header; + unsigned char data[ 9 * 22 ]; +} ring_node_norm_bp; + //************************ // flight software version // this parameters is handled by the Qt project options @@ -142,8 +162,9 @@ typedef struct ring_node_sm //********** // LPP CODES -#define LFR_SUCCESSFUL 0 -#define LFR_DEFAULT 1 +#define LFR_SUCCESSFUL 0 +#define LFR_DEFAULT 1 +#define LFR_EXE_ERROR 2 //****** // RTEMS @@ -184,7 +205,8 @@ typedef struct ring_node_sm #define ACTION_MSG_QUEUE_COUNT 10 #define ACTION_MSG_PKTS_COUNT 50 -#define ACTION_MSG_PKTS_MAX_SIZE (PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES) +//#define ACTION_MSG_PKTS_MAX_SIZE (PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES) +#define ACTION_MSG_PKTS_MAX_SIZE (PACKET_LENGTH_TM_LFR_SCIENCE_SBM1_BP1_F0 + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES) #define ACTION_MSG_SPW_IOCTL_SEND_SIZE 24 // hlen *hdr dlen *data sent options #define QUEUE_RECV 0 diff --git a/header/fsw_params_processing.h b/header/fsw_params_processing.h --- a/header/fsw_params_processing.h +++ b/header/fsw_params_processing.h @@ -4,13 +4,15 @@ #define NB_BINS_PER_SM 128 #define NB_VALUES_PER_SM 25 #define TOTAL_SIZE_SM 3200 // 25 * 128 -#define TOTAL_SIZE_BP1_F0 99 // 11 * 9 = 99 -#define TOTAL_SIZE_BP1_F1 117 // 13 * 9 = 117 -#define TOTAL_SIZE_BP1_F2 108 // 12 * 9 = 108 +#define TOTAL_SIZE_NORM_BP1_F0 99 // 11 * 9 = 99 +#define TOTAL_SIZE_NORM_BP1_F1 117 // 13 * 9 = 117 +#define TOTAL_SIZE_NORM_BP1_F2 108 // 12 * 9 = 108 +#define TOTAL_SIZE_SBM1_BP1_F0 198 // 22 * 9 = 198 // -#define NB_RING_NODES_ASM_F0 12 // AT LEAST 3 -#define NB_RING_NODES_ASM_F1 2 // AT LEAST 3 -#define NB_RING_NODES_ASM_F2 2 // AT LEAST 3 +#define NB_RING_NODES_ASM_F0 12 // AT LEAST 3 +#define NB_RING_NODES_ASM_F1 2 // AT LEAST 3 +#define NB_RING_NODES_ASM_F2 2 // AT LEAST 3 +#define NB_RING_NODES_BP1_SBM1 10 // AT LEAST 3 // #define NB_BINS_PER_ASM_F0 88 #define NB_BINS_PER_PKT_ASM_F0 44 @@ -30,30 +32,22 @@ #define ASM_F2_INDICE_START 7 // 96 bins #define ASM_F2_INDICE_STOP 102 // 2 packets of 48 bins // -#define NB_BINS_COMPRESSED_SM_F0 11 -#define NB_BINS_COMPRESSED_SM_F1 13 -#define NB_BINS_COMPRESSED_SM_F2 12 +#define NB_BINS_COMPRESSED_SM_F0 11 +#define NB_BINS_COMPRESSED_SM_F1 13 +#define NB_BINS_COMPRESSED_SM_F2 12 +#define NB_BINS_COMPRESSED_SM_SBM1_F0 22 // #define NB_BINS_TO_AVERAGE_ASM_F0 8 #define NB_BINS_TO_AVERAGE_ASM_F1 8 #define NB_BINS_TO_AVERAGE_ASM_F2 8 +#define NB_BINS_TO_AVERAGE_ASM_SBM1_F0 4 // #define TOTAL_SIZE_COMPRESSED_ASM_F0 275 // 11 * 25 WORDS #define TOTAL_SIZE_COMPRESSED_ASM_F1 325 // 13 * 25 WORDS #define TOTAL_SIZE_COMPRESSED_ASM_F2 300 // 12 * 25 WORDS #define TOTAL_SIZE_COMPRESSED_ASM_SBM1 550 // 22 * 25 WORDS -#define NB_AVERAGE_NORMAL_F0 384 // 96 * 4 -#define NB_AVERAGE_SBM1_F0 24 // 24 matrices at f0 = 0.25 second -#define NB_SM_TO_RECEIVE_BEFORE_AVF0 8 - -typedef struct { - volatile unsigned char PE[2]; - volatile unsigned char PB[2]; - volatile unsigned char V0; - volatile unsigned char V1; - volatile unsigned char V2_ELLIP_DOP; - volatile unsigned char SZ; - volatile unsigned char VPHI; -} BP1_t; +#define NB_AVERAGE_NORMAL_F0 384 // 96 * 4 +#define NB_AVERAGE_SBM1_F0 24 // 24 matrices at f0 = 0.25 second +#define NB_SM_TO_RECEIVE_BEFORE_AVF0 8 #endif // FSW_PARAMS_PROCESSING_H diff --git a/header/fsw_processing.h b/header/fsw_processing.h --- a/header/fsw_processing.h +++ b/header/fsw_processing.h @@ -38,20 +38,27 @@ rtems_task avf0_task(rtems_task_argument rtems_task smiq_task(rtems_task_argument argument); // added to test the spectral matrix simulator rtems_task matr_task(rtems_task_argument argument); -void BP1_set_old(float * compressed_spec_mat, unsigned char nb_bins_compressed_spec_mat, unsigned char * LFR_BP1); -void BP2_set_old(float * compressed_spec_mat, unsigned char nb_bins_compressed_spec_mat); -// -void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header); -void matrix_reset(volatile float *averaged_spec_mat); +//***************************** +// Spectral matrices processing + void ASM_average(float *averaged_spec_mat_f0, float *averaged_spec_mat_f1, ring_node_sm *ring_node_tab[], unsigned int firstTimeF0, unsigned int firstTimeF1 ); -void ASM_reorganize( float *averaged_spec_mat, float *averaged_spec_mat_reorganized ); -void ASM_compress( float *averaged_spec_mat, unsigned char fChannel, float *compressed_spec_mat ); +void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized, + float divider ); +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); void ASM_convert(volatile float *input_matrix, char *output_matrix); void ASM_send(Header_TM_LFR_SCIENCE_ASM_t *header, char *spectral_matrix, unsigned int sid, spw_ioctl_pkt_send *spw_ioctl_send, rtems_id queue_id); -void fill_averaged_spectral_matrix( void ); -void reset_spectral_matrix_regs(); + +void BP1_send( ring_node_bp *ring_node_to_send, unsigned int sid, rtems_id queue_id ); + +void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header); +void init_headers_bp_ring_sbm1(); +void init_header_bp( Header_TM_LFR_SCIENCE_BP_SBM_t *header); + +void reset_spectral_matrix_regs( void ); #endif // FSW_PROCESSING_H_INCLUDED diff --git a/src/fsw_processing.c b/src/fsw_processing.c --- a/src/fsw_processing.c +++ b/src/fsw_processing.c @@ -13,32 +13,35 @@ //************************ // spectral matrices rings -ring_node_sm sm_ring_f0[NB_RING_NODES_ASM_F0]; -ring_node_sm sm_ring_f1[NB_RING_NODES_ASM_F1]; -ring_node_sm sm_ring_f2[NB_RING_NODES_ASM_F2]; +ring_node_sm sm_ring_f0[ NB_RING_NODES_ASM_F0 ]; +ring_node_sm sm_ring_f1[ NB_RING_NODES_ASM_F1 ]; +ring_node_sm sm_ring_f2[ NB_RING_NODES_ASM_F2 ]; ring_node_sm *current_ring_node_sm_f0; ring_node_sm *ring_node_for_averaging_sm_f0; ring_node_sm *current_ring_node_sm_f1; ring_node_sm *current_ring_node_sm_f2; -BP1_t data_BP1[ NB_BINS_COMPRESSED_SM_F0 ]; +//********************** +// basic parameter rings +ring_node_bp *current_node_sbm1_bp1_f0; +ring_node_bp bp_ring_sbm1[ NB_RING_NODES_BP1_SBM1 ]; //***** // NORM // F0 -float averaged_sm_f0 [ TIME_OFFSET + TOTAL_SIZE_SM ]; -float averaged_sm_f0_reorganized[ TIME_OFFSET + TOTAL_SIZE_SM ]; -char averaged_sm_f0_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ]; -float compressed_sm_f0 [ TOTAL_SIZE_COMPRESSED_ASM_F0 ]; +float asm_f0 [ TIME_OFFSET + TOTAL_SIZE_SM ]; +float asm_f0_reorganized[ TIME_OFFSET + TOTAL_SIZE_SM ]; +char asm_f0_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ]; +float compressed_sm_f0 [ TIME_OFFSET + TOTAL_SIZE_COMPRESSED_ASM_F0 ]; //***** // SBM1 -float averaged_sm_sbm1 [ TIME_OFFSET + TOTAL_SIZE_SM ]; -float compressed_sm_sbm1 [ TOTAL_SIZE_COMPRESSED_ASM_SBM1 ]; +float averaged_sm_sbm1 [ TIME_OFFSET + TOTAL_SIZE_SM ]; +float compressed_sm_sbm1 [ TIME_OFFSET + TOTAL_SIZE_COMPRESSED_ASM_SBM1 ]; -unsigned char LFR_BP1_F0[ TIME_OFFSET_IN_BYTES + TOTAL_SIZE_BP1_F0 * 2 ]; -unsigned char LFR_BP1_F1[ TIME_OFFSET_IN_BYTES + TOTAL_SIZE_BP1_F1 ]; -unsigned char LFR_BP1_F2[ TIME_OFFSET_IN_BYTES + TOTAL_SIZE_BP1_F2 ]; +unsigned char LFR_BP1_F0[ TIME_OFFSET_IN_BYTES + TOTAL_SIZE_NORM_BP1_F0 * 2 ]; +unsigned char LFR_BP1_F1[ TIME_OFFSET_IN_BYTES + TOTAL_SIZE_NORM_BP1_F1 ]; +unsigned char LFR_BP1_F2[ TIME_OFFSET_IN_BYTES + TOTAL_SIZE_NORM_BP1_F2 ]; unsigned int nb_sm_f0; @@ -120,6 +123,11 @@ void reset_current_sm_ring_nodes( void ) ring_node_for_averaging_sm_f0 = sm_ring_f0; } +void reset_current_node_sbm1_bp1_f0( void ) +{ + current_node_sbm1_bp1_f0 = bp_ring_sbm1; +} + //*********************************************************** // Interrupt Service Routine for spectral matrices processing void reset_nb_sm_f0( void ) @@ -223,14 +231,14 @@ rtems_task avf0_task(rtems_task_argument ring_node_tab[NB_SM_TO_RECEIVE_BEFORE_AVF0-i] = ring_node_for_averaging_sm_f0; } - // copy time information in the averaged_sm_f0 buffer - averaged_sm_f0[0] = ring_node_tab[7]->coarseTime; - averaged_sm_f0[1] = ring_node_tab[7]->fineTime; + // copy time information in the asm_f0 buffer + asm_f0[0] = ring_node_tab[7]->coarseTime; + asm_f0[1] = ring_node_tab[7]->fineTime; averaged_sm_sbm1[0] = ring_node_tab[7]->coarseTime; averaged_sm_sbm1[1] = ring_node_tab[7]->fineTime; // compute the average and store it in the averaged_sm_f1 buffer - ASM_average( averaged_sm_f0, averaged_sm_sbm1, + ASM_average( asm_f0, averaged_sm_sbm1, ring_node_tab, nb_average_norm_f0, nb_average_sbm1_f0 ); @@ -271,8 +279,10 @@ rtems_task matr_task(rtems_task_argument rtems_status_code status; rtems_id queue_id; Header_TM_LFR_SCIENCE_ASM_t headerASM; + ring_node_norm_bp current_node_norm_bp1_f0; init_header_asm( &headerASM ); +// init_header_bp( ¤t_node_norm_bp1_f0.header ); status = get_message_queue_id_send( &queue_id ); if (status != RTEMS_SUCCESSFUL) @@ -282,33 +292,40 @@ rtems_task matr_task(rtems_task_argument BOOT_PRINTF("in MATR *** \n") - fill_averaged_spectral_matrix( ); - while(1){ rtems_event_receive( RTEMS_EVENT_MODE_NORMAL | RTEMS_EVENT_MODE_SBM1, RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); if (event_out==RTEMS_EVENT_MODE_NORMAL) { // 1) compress the matrix for Basic Parameters calculation - ASM_compress( averaged_sm_f0, 0, compressed_sm_f0 ); + ASM_compress_reorganize_and_divide( asm_f0, compressed_sm_f0, + NB_AVERAGE_NORMAL_F0, + NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0, + ASM_F0_INDICE_START ); // 2) compute the BP1 set // 3) send the BP1 set - - // 4) convert the float array in a char array - ASM_reorganize( averaged_sm_f0, averaged_sm_f0_reorganized ); - ASM_convert( averaged_sm_f0_reorganized, averaged_sm_f0_char); - // 5) send the spectral matrix packets - ASM_send( &headerASM, averaged_sm_f0_char, SID_NORM_ASM_F0, &spw_ioctl_send_ASM, queue_id); + BP1_send( ¤t_node_norm_bp1_f0, SID_NORM_BP1_F0, queue_id ); + // 4) reorganize the ASM and divide + ASM_reorganize_and_divide( asm_f0, asm_f0_reorganized, NB_AVERAGE_NORMAL_F0 ); + // 5) convert the float array in a char array + ASM_convert( asm_f0_reorganized, asm_f0_char); + // 6) send the spectral matrix packets + ASM_send( &headerASM, asm_f0_char, SID_NORM_ASM_F0, &spw_ioctl_send_ASM, queue_id); } else if (event_out==RTEMS_EVENT_MODE_SBM1) { // 1) compress the matrix for Basic Parameters calculation - ASM_compress( averaged_sm_sbm1, 0, compressed_sm_sbm1 ); + ASM_compress_reorganize_and_divide( averaged_sm_sbm1, compressed_sm_sbm1, + NB_AVERAGE_SBM1_F0, + NB_BINS_COMPRESSED_SM_SBM1_F0, NB_BINS_TO_AVERAGE_ASM_SBM1_F0, + ASM_F0_INDICE_START); // 2) compute the BP1 set // 3) send the basic parameters set 1 packet - + BP1_send( current_node_sbm1_bp1_f0, SID_SBM1_BP1_F0, queue_id ); + // 4) update current_node_sbm1_bp1_f0 + current_node_sbm1_bp1_f0 = current_node_sbm1_bp1_f0->next; } else { @@ -320,14 +337,6 @@ rtems_task matr_task(rtems_task_argument //***************************** // Spectral matrices processing -void matrix_reset(volatile float *averaged_spec_mat) -{ - int i; - for(i=0; i abs(sx_im) ) { - LFR_BP1[(i*9) + 7] = LFR_BP1[(i*9) + 1] | (0x80); // extract the sector of sx - } - else { - LFR_BP1[(i*9) + 7] = LFR_BP1[(i*9) + 1] & (0x7f); // extract the sector of sx - } - //====================================================================== - // BP1 phase velocity estimator == PAR_LFR_SC_BP1_VPHI_F0 == 8 bits (7+1) - ny = sin(Alpha_M)*NVEC_V1 + cos(Alpha_M)*NVEC_V2; - nz = NVEC_V0; - bx_bx_star = cos(Alpha_M) * cos(Alpha_M) * compressed_spec_mat[i*30+10] // re S22 - + sin(Alpha_M) * sin(Alpha_M) * compressed_spec_mat[i*30+18] // re S33 - - 2 * sin(Alpha_M) * cos(Alpha_M) * compressed_spec_mat[i*30+12]; // re S23 - nebx_re = ny * (compressed_spec_mat[(i*30) + 14] * K24_ny_re - +compressed_spec_mat[(i*30) + 16] * K25_ny_re - +compressed_spec_mat[(i*30) + 20] * K34_ny_re - +compressed_spec_mat[(i*30) + 22] * K35_ny_re) - + nz * (compressed_spec_mat[(i*30) + 14] * K24_nz_re - +compressed_spec_mat[(i*30) + 16] * K25_nz_re - +compressed_spec_mat[(i*30) + 20] * K34_nz_re - +compressed_spec_mat[(i*30) + 22] * K35_nz_re); - nebx_im = ny * (compressed_spec_mat[(i*30) + 15]*K24_ny_re - +compressed_spec_mat[(i*30) + 17] * K25_ny_re - +compressed_spec_mat[(i*30) + 21] * K34_ny_re - +compressed_spec_mat[(i*30) + 23] * K35_ny_re) - + nz * (compressed_spec_mat[(i*30) + 15] * K24_nz_im - +compressed_spec_mat[(i*30) + 17] * K25_nz_im - +compressed_spec_mat[(i*30) + 21] * K34_nz_im - +compressed_spec_mat[(i*30) + 23] * K35_nz_im); - tmp = nebx_re / bx_bx_star; - LFR_BP1[(i*9) + 8] = ((unsigned char) (tmp * 128)) & 0x7f; // cf DOC for the compression - if ( abs(nebx_re) > abs(nebx_im) ) { - LFR_BP1[(i*9) + 8] = LFR_BP1[(i*9) + 8] | (0x80); // extract the sector of nebx - } - else { - LFR_BP1[(i*9) + 8] = LFR_BP1[(i*9) + 8] & (0x7f); // extract the sector of nebx - } + // (1) BUILD THE DATA + switch(sid) + { + case SID_NORM_BP1_F0: + length = PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0; + ring_node_to_send->header.packetID[0] = (unsigned char) (TM_PACKET_ID_SCIENCE_NORMAL_BURST >> 8); + ring_node_to_send->header.packetID[1] = (unsigned char) (TM_PACKET_ID_SCIENCE_NORMAL_BURST); + ring_node_to_send->header.pa_lfr_bp_blk_nr[0] = (unsigned char) ( (NB_BINS_COMPRESSED_SM_F0) >> 8 ); // BLK_NR MSB + ring_node_to_send->header.pa_lfr_bp_blk_nr[1] = (unsigned char) (NB_BINS_COMPRESSED_SM_F0); // BLK_NR LSB + nbBytesTosend = PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0 + + CCSDS_TC_TM_PACKET_OFFSET + + CCSDS_PROTOCOLE_EXTRA_BYTES; + case SID_SBM1_BP1_F0: + length = PACKET_LENGTH_TM_LFR_SCIENCE_SBM1_BP1_F0; + ring_node_to_send->header.packetID[0] = (unsigned char) (TM_PACKET_ID_SCIENCE_SBM1_SBM2 >> 8); + ring_node_to_send->header.packetID[1] = (unsigned char) (TM_PACKET_ID_SCIENCE_SBM1_SBM2); + ring_node_to_send->header.pa_lfr_bp_blk_nr[0] = (unsigned char) ( (NB_BINS_COMPRESSED_SM_SBM1_F0) >> 8 ); // BLK_NR MSB + ring_node_to_send->header.pa_lfr_bp_blk_nr[1] = (unsigned char) (NB_BINS_COMPRESSED_SM_SBM1_F0); // BLK_NR LSB + nbBytesTosend = PACKET_LENGTH_TM_LFR_SCIENCE_SBM1_BP1_F0 + + CCSDS_TC_TM_PACKET_OFFSET + + CCSDS_PROTOCOLE_EXTRA_BYTES; + break; + default: + nbBytesTosend = 0; + PRINTF1("ERR *** in BP1_send *** unexpected sid %d\n", sid) + break; } -} - -void BP2_set_old(float * compressed_spec_mat, unsigned char nb_bins_compressed_spec_mat){ - // BP2 autocorrelation - int i; - int aux = 0; + // (2) BUILD THE HEADER + ring_node_to_send->header.packetLength[0] = (unsigned char) (length>>8); + ring_node_to_send->header.packetLength[1] = (unsigned char) (length); + ring_node_to_send->header.sid = sid; - for(i = 0; iheader.time[0] = (unsigned char) (ring_node_to_send->coarseTime>>24); + ring_node_to_send->header.time[1] = (unsigned char) (ring_node_to_send->coarseTime>>16); + ring_node_to_send->header.time[2] = (unsigned char) (ring_node_to_send->coarseTime>>8); + ring_node_to_send->header.time[3] = (unsigned char) (ring_node_to_send->coarseTime); + ring_node_to_send->header.time[4] = (unsigned char) (ring_node_to_send->fineTime>>8); + ring_node_to_send->header.time[5] = (unsigned char) (ring_node_to_send->fineTime); + // + ring_node_to_send->header.acquisitionTime[0] = (unsigned char) (ring_node_to_send->coarseTime>>24); + ring_node_to_send->header.acquisitionTime[1] = (unsigned char) (ring_node_to_send->coarseTime>>16); + ring_node_to_send->header.acquisitionTime[2] = (unsigned char) (ring_node_to_send->coarseTime>>8); + ring_node_to_send->header.acquisitionTime[3] = (unsigned char) (ring_node_to_send->coarseTime); + ring_node_to_send->header.acquisitionTime[4] = (unsigned char) (ring_node_to_send->fineTime>>8); + ring_node_to_send->header.acquisitionTime[5] = (unsigned char) (ring_node_to_send->fineTime); + + // (4) SEND PACKET + status = rtems_message_queue_send( queue_id, &ring_node_to_send->header, nbBytesTosend); + if (status != RTEMS_SUCCESSFUL) + { + printf("ERR *** in BP1_send *** ERR %d\n", (int) status); } } @@ -746,14 +615,41 @@ void init_header_asm( Header_TM_LFR_SCIE header->pa_lfr_asm_blk_nr[1] = 0x00; // BLK_NR LSB } -void init_header_bp( Header_TM_LFR_SCIENCE_BP_t *header) +void init_bp_ring_sbm1() +{ + unsigned int i; + + //******** + // F0 RING + bp_ring_sbm1[0].next = (ring_node_bp*) &bp_ring_sbm1[1]; + bp_ring_sbm1[0].previous = (ring_node_bp*) &bp_ring_sbm1[NB_RING_NODES_BP1_SBM1-1]; + + bp_ring_sbm1[NB_RING_NODES_BP1_SBM1-1].next = (ring_node_bp*) &bp_ring_sbm1[0]; + bp_ring_sbm1[NB_RING_NODES_BP1_SBM1-1].previous = (ring_node_bp*) &bp_ring_sbm1[NB_RING_NODES_ASM_F0-2]; + + for(i=1; itargetLogicalAddress = CCSDS_DESTINATION_ID; header->protocolIdentifier = CCSDS_PROTOCOLE_ID; header->reserved = 0x00; header->userApplication = CCSDS_USER_APP; -// header->packetID[0] = (unsigned char) (TM_PACKET_ID_SCIENCE_NORMAL_BURST >> 8); -// header->packetID[1] = (unsigned char) (TM_PACKET_ID_SCIENCE_NORMAL_BURST); + header->packetID[0] = (unsigned char) (TM_PACKET_ID_SCIENCE_SBM1_SBM2 >> 8); + header->packetID[1] = (unsigned char) (TM_PACKET_ID_SCIENCE_SBM1_SBM2); header->packetSequenceControl[0] = 0xc0; header->packetSequenceControl[1] = 0x00; header->packetLength[0] = 0x00; @@ -776,86 +672,7 @@ void init_header_bp( Header_TM_LFR_SCIEN header->pa_lfr_bp_blk_nr[1] = 0x00; // BLK_NR LSB } -void fill_averaged_spectral_matrix(void) -{ - /** This function fills spectral matrices related buffers with arbitrary data. - * - * This function is for testing purpose only. - * - */ - - float offset; - float coeff; - - offset = 10.; - coeff = 100000.; - averaged_sm_f0[ 0 + 25 * 0 ] = 0. + offset; - averaged_sm_f0[ 0 + 25 * 1 ] = 1. + offset; - averaged_sm_f0[ 0 + 25 * 2 ] = 2. + offset; - averaged_sm_f0[ 0 + 25 * 3 ] = 3. + offset; - averaged_sm_f0[ 0 + 25 * 4 ] = 4. + offset; - averaged_sm_f0[ 0 + 25 * 5 ] = 5. + offset; - averaged_sm_f0[ 0 + 25 * 6 ] = 6. + offset; - averaged_sm_f0[ 0 + 25 * 7 ] = 7. + offset; - averaged_sm_f0[ 0 + 25 * 8 ] = 8. + offset; - averaged_sm_f0[ 0 + 25 * 9 ] = 9. + offset; - averaged_sm_f0[ 0 + 25 * 10 ] = 10. + offset; - averaged_sm_f0[ 0 + 25 * 11 ] = 11. + offset; - averaged_sm_f0[ 0 + 25 * 12 ] = 12. + offset; - averaged_sm_f0[ 0 + 25 * 13 ] = 13. + offset; - averaged_sm_f0[ 0 + 25 * 14 ] = 14. + offset; - averaged_sm_f0[ 9 + 25 * 0 ] = -(0. + offset)* coeff; - averaged_sm_f0[ 9 + 25 * 1 ] = -(1. + offset)* coeff; - averaged_sm_f0[ 9 + 25 * 2 ] = -(2. + offset)* coeff; - averaged_sm_f0[ 9 + 25 * 3 ] = -(3. + offset)* coeff; - averaged_sm_f0[ 9 + 25 * 4 ] = -(4. + offset)* coeff; - averaged_sm_f0[ 9 + 25 * 5 ] = -(5. + offset)* coeff; - averaged_sm_f0[ 9 + 25 * 6 ] = -(6. + offset)* coeff; - averaged_sm_f0[ 9 + 25 * 7 ] = -(7. + offset)* coeff; - averaged_sm_f0[ 9 + 25 * 8 ] = -(8. + offset)* coeff; - averaged_sm_f0[ 9 + 25 * 9 ] = -(9. + offset)* coeff; - averaged_sm_f0[ 9 + 25 * 10 ] = -(10. + offset)* coeff; - averaged_sm_f0[ 9 + 25 * 11 ] = -(11. + offset)* coeff; - averaged_sm_f0[ 9 + 25 * 12 ] = -(12. + offset)* coeff; - averaged_sm_f0[ 9 + 25 * 13 ] = -(13. + offset)* coeff; - averaged_sm_f0[ 9 + 25 * 14 ] = -(14. + offset)* coeff; - - offset = 10000000; - averaged_sm_f0[ 16 + 25 * 0 ] = (0. + offset)* coeff; - averaged_sm_f0[ 16 + 25 * 1 ] = (1. + offset)* coeff; - averaged_sm_f0[ 16 + 25 * 2 ] = (2. + offset)* coeff; - averaged_sm_f0[ 16 + 25 * 3 ] = (3. + offset)* coeff; - averaged_sm_f0[ 16 + 25 * 4 ] = (4. + offset)* coeff; - averaged_sm_f0[ 16 + 25 * 5 ] = (5. + offset)* coeff; - averaged_sm_f0[ 16 + 25 * 6 ] = (6. + offset)* coeff; - averaged_sm_f0[ 16 + 25 * 7 ] = (7. + offset)* coeff; - averaged_sm_f0[ 16 + 25 * 8 ] = (8. + offset)* coeff; - averaged_sm_f0[ 16 + 25 * 9 ] = (9. + offset)* coeff; - averaged_sm_f0[ 16 + 25 * 10 ] = (10. + offset)* coeff; - averaged_sm_f0[ 16 + 25 * 11 ] = (11. + offset)* coeff; - averaged_sm_f0[ 16 + 25 * 12 ] = (12. + offset)* coeff; - averaged_sm_f0[ 16 + 25 * 13 ] = (13. + offset)* coeff; - averaged_sm_f0[ 16 + 25 * 14 ] = (14. + offset)* coeff; - - averaged_sm_f0[ (TOTAL_SIZE_SM/2) + 0 ] = averaged_sm_f0[ 0 ]; - averaged_sm_f0[ (TOTAL_SIZE_SM/2) + 1 ] = averaged_sm_f0[ 1 ]; - averaged_sm_f0[ (TOTAL_SIZE_SM/2) + 2 ] = averaged_sm_f0[ 2 ]; - averaged_sm_f0[ (TOTAL_SIZE_SM/2) + 3 ] = averaged_sm_f0[ 3 ]; - averaged_sm_f0[ (TOTAL_SIZE_SM/2) + 4 ] = averaged_sm_f0[ 4 ]; - averaged_sm_f0[ (TOTAL_SIZE_SM/2) + 5 ] = averaged_sm_f0[ 5 ]; - averaged_sm_f0[ (TOTAL_SIZE_SM/2) + 6 ] = averaged_sm_f0[ 6 ]; - averaged_sm_f0[ (TOTAL_SIZE_SM/2) + 7 ] = averaged_sm_f0[ 7 ]; - averaged_sm_f0[ (TOTAL_SIZE_SM/2) + 8 ] = averaged_sm_f0[ 8 ]; - averaged_sm_f0[ (TOTAL_SIZE_SM/2) + 9 ] = averaged_sm_f0[ 9 ]; - averaged_sm_f0[ (TOTAL_SIZE_SM/2) + 10 ] = averaged_sm_f0[ 10 ]; - averaged_sm_f0[ (TOTAL_SIZE_SM/2) + 11 ] = averaged_sm_f0[ 11 ]; - averaged_sm_f0[ (TOTAL_SIZE_SM/2) + 12 ] = averaged_sm_f0[ 12 ]; - averaged_sm_f0[ (TOTAL_SIZE_SM/2) + 13 ] = averaged_sm_f0[ 13 ]; - averaged_sm_f0[ (TOTAL_SIZE_SM/2) + 14 ] = averaged_sm_f0[ 14 ]; - averaged_sm_f0[ (TOTAL_SIZE_SM/2) + 15 ] = averaged_sm_f0[ 15 ]; -} - -void reset_spectral_matrix_regs() +void reset_spectral_matrix_regs( void ) { /** This function resets the spectral matrices module registers. * diff --git a/src/tc_handler.c b/src/tc_handler.c --- a/src/tc_handler.c +++ b/src/tc_handler.c @@ -532,7 +532,7 @@ int enter_mode( unsigned char mode, unsi #endif status = restart_science_tasks(); launch_waveform_picker( mode, transitionCoarseTime ); -// launch_spectral_matrix_simu( mode ); + launch_spectral_matrix_simu( mode ); } else if ( mode == LFR_MODE_STANDBY ) { @@ -842,7 +842,7 @@ void close_action(ccsdsTelecommandPacket updateLFRCurrentMode(); } } - else + else if (result == LFR_EXE_ERROR) { send_tm_lfr_tc_exe_error( TC, queue_id ); }