# HG changeset patch # User paul # Date 2014-04-27 13:12:37 # Node ID 9657f56b7e43765f656d1f65cc8492ecfb4009c6 # Parent 1bf6d1691be71cfa9dd28c93f44283639bd80310 avf2 prc2 tasks added sources reorganized 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 24 22:14:00 2014 +# Generated by qmake (2.01a) (Qt 4.8.5) on: Sun Apr 27 14:59:58 2014 # Project: fsw-qt.pro # Template: app # Command: /usr/bin/qmake-qt4 -spec /usr/lib64/qt4/mkspecs/linux-g++ -o Makefile fsw-qt.pro @@ -13,7 +13,7 @@ CXX = sparc-rtems-g++ DEFINES = -DSW_VERSION_N1=1 -DSW_VERSION_N2=0 -DSW_VERSION_N3=0 -DSW_VERSION_N4=6 -DPRINT_MESSAGES_ON_CONSOLE -DPRINT_TASK_STATISTICS -DBOOT_MESSAGES CFLAGS = -pipe -O3 -Wall $(DEFINES) CXXFLAGS = -pipe -O3 -Wall $(DEFINES) -INCPATH = -I/usr/lib64/qt4/mkspecs/linux-g++ -I. -I../src -I../header -I../src/basic_parameters +INCPATH = -I/usr/lib64/qt4/mkspecs/linux-g++ -I. -I../src -I../header -I../src/basic_parameters -I../src/avf_prc LINK = sparc-rtems-g++ LFLAGS = LIBS = $(SUBLIBS) @@ -45,7 +45,6 @@ OBJECTS_DIR = obj/ SOURCES = ../src/wf_handler.c \ ../src/tc_handler.c \ - ../src/fsw_processing.c \ ../src/fsw_misc.c \ ../src/fsw_init.c \ ../src/fsw_globals.c \ @@ -53,10 +52,13 @@ SOURCES = ../src/wf_handler.c \ ../src/tc_load_dump_parameters.c \ ../src/tm_lfr_tc_exe.c \ ../src/tc_acceptance.c \ - ../src/basic_parameters/basic_parameters.c + ../src/basic_parameters/basic_parameters.c \ + ../src/avf_prc/fsw_processing.c \ + ../src/avf_prc/avf0_prc0.c \ + ../src/avf_prc/avf1_prc1.c \ + ../src/avf_prc/avf2_prc2.c OBJECTS = obj/wf_handler.o \ obj/tc_handler.o \ - obj/fsw_processing.o \ obj/fsw_misc.o \ obj/fsw_init.o \ obj/fsw_globals.o \ @@ -64,7 +66,11 @@ OBJECTS = obj/wf_handler.o \ obj/tc_load_dump_parameters.o \ obj/tm_lfr_tc_exe.o \ obj/tc_acceptance.o \ - obj/basic_parameters.o + obj/basic_parameters.o \ + obj/fsw_processing.o \ + obj/avf0_prc0.o \ + obj/avf1_prc1.o \ + obj/avf2_prc2.o DIST = /usr/lib64/qt4/mkspecs/common/unix.conf \ /usr/lib64/qt4/mkspecs/common/linux.conf \ /usr/lib64/qt4/mkspecs/common/gcc-base.conf \ @@ -216,9 +222,6 @@ obj/wf_handler.o: ../src/wf_handler.c obj/tc_handler.o: ../src/tc_handler.c $(CC) -c $(CFLAGS) $(INCPATH) -o obj/tc_handler.o ../src/tc_handler.c -obj/fsw_processing.o: ../src/fsw_processing.c ../src/fsw_processing_globals.c - $(CC) -c $(CFLAGS) $(INCPATH) -o obj/fsw_processing.o ../src/fsw_processing.c - obj/fsw_misc.o: ../src/fsw_misc.c $(CC) -c $(CFLAGS) $(INCPATH) -o obj/fsw_misc.o ../src/fsw_misc.c @@ -243,6 +246,21 @@ obj/tc_acceptance.o: ../src/tc_acceptanc obj/basic_parameters.o: ../src/basic_parameters/basic_parameters.c ../src/basic_parameters/basic_parameters.h $(CC) -c $(CFLAGS) $(INCPATH) -o obj/basic_parameters.o ../src/basic_parameters/basic_parameters.c +obj/fsw_processing.o: ../src/avf_prc/fsw_processing.c ../src/avf_prc/fsw_processing.h + $(CC) -c $(CFLAGS) $(INCPATH) -o obj/fsw_processing.o ../src/avf_prc/fsw_processing.c + +obj/avf0_prc0.o: ../src/avf_prc/avf0_prc0.c ../src/avf_prc/avf0_prc0.h \ + ../src/avf_prc/fsw_processing.h + $(CC) -c $(CFLAGS) $(INCPATH) -o obj/avf0_prc0.o ../src/avf_prc/avf0_prc0.c + +obj/avf1_prc1.o: ../src/avf_prc/avf1_prc1.c ../src/avf_prc/avf1_prc1.h \ + ../src/avf_prc/fsw_processing.h + $(CC) -c $(CFLAGS) $(INCPATH) -o obj/avf1_prc1.o ../src/avf_prc/avf1_prc1.c + +obj/avf2_prc2.o: ../src/avf_prc/avf2_prc2.c ../src/avf_prc/avf2_prc2.h \ + ../src/avf_prc/fsw_processing.h + $(CC) -c $(CFLAGS) $(INCPATH) -o obj/avf2_prc2.o ../src/avf_prc/avf2_prc2.c + ####### Install install: FORCE 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 @@ -50,12 +50,12 @@ TARGET = fsw INCLUDEPATH += \ ../src \ ../header \ - ../src/basic_parameters + ../src/basic_parameters \ + ../src/avf_prc SOURCES += \ ../src/wf_handler.c \ ../src/tc_handler.c \ - ../src/fsw_processing.c \ ../src/fsw_misc.c \ ../src/fsw_init.c \ ../src/fsw_globals.c \ @@ -63,14 +63,17 @@ SOURCES += \ ../src/tc_load_dump_parameters.c \ ../src/tm_lfr_tc_exe.c \ ../src/tc_acceptance.c \ - ../src/basic_parameters/basic_parameters.c + ../src/basic_parameters/basic_parameters.c \ + ../src/avf_prc/fsw_processing.c \ + ../src/avf_prc/avf0_prc0.c \ + ../src/avf_prc/avf1_prc1.c \ + ../src/avf_prc/avf2_prc2.c HEADERS += \ ../header/wf_handler.h \ ../header/tc_handler.h \ ../header/grlib_regs.h \ - ../header/fsw_processing.h \ ../header/fsw_params.h \ ../header/fsw_misc.h \ ../header/fsw_init.h \ @@ -81,5 +84,9 @@ HEADERS += \ ../header/tm_lfr_tc_exe.h \ ../header/tc_acceptance.h \ ../header/fsw_params_nb_bytes.h \ - ../src/basic_parameters/basic_parameters.h + ../src/basic_parameters/basic_parameters.h \ + ../src/avf_prc/fsw_processing.h \ + ../src/avf_prc/avf0_prc0.h \ + ../src/avf_prc/avf1_prc1.h \ + ../src/avf_prc/avf2_prc2.h 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 @@ -210,6 +210,8 @@ enum apid_destid{ #define PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0 (356 - CCSDS_TC_TM_PACKET_OFFSET) // 11 * 30 + 26 #define PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 (144 - CCSDS_TC_TM_PACKET_OFFSET) // 13 * 9 + 27 #define PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 (416 - CCSDS_TC_TM_PACKET_OFFSET) // 13 * 30 + 26 +#define PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2 (134 - CCSDS_TC_TM_PACKET_OFFSET) // 12 * 9 + 26 +#define PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2 (386 - CCSDS_TC_TM_PACKET_OFFSET) // 12 * 30 + 26 #define PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0 (224 - CCSDS_TC_TM_PACKET_OFFSET) // 22 * 9 + 26 #define PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0 (686 - CCSDS_TC_TM_PACKET_OFFSET) // 22 * 30 + 26 #define PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 (260 - CCSDS_TC_TM_PACKET_OFFSET) // 26 * 9 + 26 diff --git a/header/fsw_init.h b/header/fsw_init.h --- a/header/fsw_init.h +++ b/header/fsw_init.h @@ -7,9 +7,12 @@ #include "fsw_params.h" #include "fsw_misc.h" #include "fsw_processing.h" +#include "avf0_prc0.h" +#include "avf1_prc1.h" +#include "avf2_prc2.h" + #include "tc_handler.h" #include "wf_handler.h" - #include "fsw_spacewire.h" extern rtems_name Task_name[20]; /* array of task names */ @@ -28,6 +31,7 @@ rtems_status_code get_message_queue_id_s rtems_status_code get_message_queue_id_recv( rtems_id *queue_id ); rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id ); rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id ); +rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id ); // int start_recv_send_tasks( void ); // diff --git a/header/fsw_params.h b/header/fsw_params.h --- a/header/fsw_params.h +++ b/header/fsw_params.h @@ -18,44 +18,6 @@ typedef struct ring_node unsigned int status; } ring_node; -typedef struct { - unsigned int norm_bp1; - unsigned int norm_bp2; - unsigned int norm_asm; - unsigned int burst_sbm_bp1; - unsigned int burst_sbm_bp2; - unsigned int burst_bp1; - unsigned int burst_bp2; - unsigned int sbm1_bp1; - unsigned int sbm1_bp2; - unsigned int sbm2_bp1; - unsigned int sbm2_bp2; -} nb_sm_before_bp_asm_f0; - -typedef struct { - unsigned int norm_bp1; - unsigned int norm_bp2; - unsigned int norm_asm; - unsigned int burst_sbm_bp1; - unsigned int burst_sbm_bp2; - unsigned int burst_bp1; - unsigned int burst_bp2; - unsigned int sbm2_bp1; - unsigned int sbm2_bp2; -} nb_sm_before_bp_asm_f1; - -typedef struct { - unsigned int norm_bp1_f2; - unsigned int norm_bp2_f2; - unsigned int norm_asm_f2; - unsigned int burst_sbm_bp1_f2; - unsigned int burst_sbm_bp2_f2; - unsigned int burst_bp1_f2; - unsigned int burst_bp2_f2; - unsigned int sbm2_bp1_f2; - unsigned int sbm2_bp2_f2; -} nb_sm_before_bp_asm_f2; - //************************ // flight software version // this parameters is handled by the Qt project options @@ -101,19 +63,17 @@ typedef struct { #define RTEMS_EVENT_MODE_SBM2_WFRM RTEMS_EVENT_5 #define RTEMS_EVENT_NORM_BP1_F0 RTEMS_EVENT_6 #define RTEMS_EVENT_NORM_BP2_F0 RTEMS_EVENT_7 -#define RTEMS_EVENT_NORM_ASM_F0 RTEMS_EVENT_8 +#define RTEMS_EVENT_NORM_ASM_F0 RTEMS_EVENT_8 // ASM only in NORM mode #define RTEMS_EVENT_NORM_BP1_F1 RTEMS_EVENT_9 #define RTEMS_EVENT_NORM_BP2_F1 RTEMS_EVENT_10 -#define RTEMS_EVENT_NORM_ASM_F1 RTEMS_EVENT_11 +#define RTEMS_EVENT_NORM_ASM_F1 RTEMS_EVENT_11 // ASM only in NORM mode #define RTEMS_EVENT_NORM_BP1_F2 RTEMS_EVENT_12 #define RTEMS_EVENT_NORM_BP2_F2 RTEMS_EVENT_13 -#define RTEMS_EVENT_NORM_ASM_F2 RTEMS_EVENT_14 +#define RTEMS_EVENT_NORM_ASM_F2 RTEMS_EVENT_14 // ASM only in NORM mode #define RTEMS_EVENT_BURST_SBM_BP1_F0 RTEMS_EVENT_15 #define RTEMS_EVENT_BURST_SBM_BP2_F0 RTEMS_EVENT_16 #define RTEMS_EVENT_BURST_SBM_BP1_F1 RTEMS_EVENT_17 #define RTEMS_EVENT_BURST_SBM_BP2_F1 RTEMS_EVENT_18 -#define RTEMS_EVENT_BURST_SBM_BP1_F2 RTEMS_EVENT_19 -#define RTEMS_EVENT_BURST_SBM_BP2_F2 RTEMS_EVENT_20 //**************************** // LFR DEFAULT MODE PARAMETERS @@ -205,6 +165,8 @@ typedef struct { #define TASKID_WTDG 15 #define TASKID_AVF1 16 #define TASKID_PRC1 17 +#define TASKID_AVF2 18 +#define TASKID_PRC2 19 #define TASK_PRIORITY_SPIQ 5 #define TASK_PRIORITY_WTDG 20 @@ -221,6 +183,8 @@ typedef struct { #define TASK_PRIORITY_AVF1 70 #define TASK_PRIORITY_PRC0 100 #define TASK_PRIORITY_PRC1 100 +#define TASK_PRIORITY_AVF2 110 +#define TASK_PRIORITY_PRC2 110 #define TASK_PRIORITY_STAT 200 #define TASK_PRIORITY_DUMB 200 @@ -228,15 +192,18 @@ typedef struct { #define MSG_QUEUE_COUNT_SEND 50 #define MSG_QUEUE_COUNT_PRC0 10 #define MSG_QUEUE_COUNT_PRC1 10 +#define MSG_QUEUE_COUNT_PRC2 5 #define MSG_QUEUE_SIZE_SEND 810 // 806 + 4 => TM_LFR_SCIENCE_BURST_BP2_F1 #define ACTION_MSG_SPW_IOCTL_SEND_SIZE 24 // hlen *hdr dlen *data sent options #define MSG_QUEUE_SIZE_PRC0 20 // two pointers and one rtems_event + 2 integers #define MSG_QUEUE_SIZE_PRC1 20 // two pointers and one rtems_event + 2 integers +#define MSG_QUEUE_SIZE_PRC2 20 // two pointers and one rtems_event + 2 integers #define QUEUE_RECV 0 #define QUEUE_SEND 1 #define QUEUE_PRC0 2 #define QUEUE_PRC1 3 +#define QUEUE_PRC2 4 //******* // MACROS 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 @@ -14,8 +14,10 @@ #define NB_RING_NODES_ASM_NORM_F0 10 // AT LEAST 3 #define NB_RING_NODES_SM_F1 3 // AT LEAST 3 #define NB_RING_NODES_ASM_BURST_SBM_F1 5 // AT LEAST 3 -#define NB_RING_NODES_ASM_NORM_F1 5 // AT LEAST 3 +#define NB_RING_NODES_ASM_NORM_F1 5 // AT LEAST 3 #define NB_RING_NODES_SM_F2 3 // AT LEAST 3 +#define NB_RING_NODES_ASM_BURST_SBM_F2 3 // AT LEAST 3 +#define NB_RING_NODES_ASM_NORM_F2 3 // AT LEAST 3 // #define NB_BINS_PER_ASM_F0 88 #define NB_BINS_PER_PKT_ASM_F0 44 @@ -49,15 +51,15 @@ #define NB_BINS_TO_AVERAGE_ASM_SBM_F1 4 #define NB_BINS_TO_AVERAGE_ASM_SBM_F2 4 // -#define TOTAL_SIZE_COMPRESSED_ASM_NORM_F0 275 // 11 * 25 WORDS -#define TOTAL_SIZE_COMPRESSED_ASM_NORM_F1 325 // 13 * 25 WORDS -#define TOTAL_SIZE_COMPRESSED_ASM_F2 300 // 12 * 25 WORDS -#define TOTAL_SIZE_COMPRESSED_ASM_SBM_F0 550 // 22 * 25 WORDS -#define TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 650 // 26 * 25 WORDS -// NORM -#define NB_SM_BEFORE_NORM_BP1_F0 384 // 96 * 4 +#define TOTAL_SIZE_COMPRESSED_ASM_NORM_F0 275 // 11 * 25 WORDS +#define TOTAL_SIZE_COMPRESSED_ASM_NORM_F1 325 // 13 * 25 WORDS +#define TOTAL_SIZE_COMPRESSED_ASM_NORM_F2 300 // 12 * 25 WORDS +#define TOTAL_SIZE_COMPRESSED_ASM_SBM_F0 550 // 22 * 25 WORDS +#define TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 650 // 26 * 25 WORDS +#define TOTAL_SIZE_COMPRESSED_ASM_SBM_F2 600 // 24 * 25 WORDS // GENERAL -#define NB_SM_BEFORE_AVF0 8 -#define NB_SM_BEFORE_AVF1 8 +#define NB_SM_BEFORE_AVF0 8 // must be 8 due to the SM_average() function +#define NB_SM_BEFORE_AVF1 8 // must be 8 due to the SM_average() function +#define NB_SM_BEFORE_AVF2 1 // must be 1 due to the SM_average_f2() function #endif // FSW_PARAMS_PROCESSING_H diff --git a/header/fsw_processing.h b/header/fsw_processing.h deleted file mode 100644 --- a/header/fsw_processing.h +++ /dev/null @@ -1,123 +0,0 @@ -#ifndef FSW_PROCESSING_H_INCLUDED -#define FSW_PROCESSING_H_INCLUDED - -#include -#include -#include -#include // abs() is in the stdlib -#include // printf() -#include - -#include "fsw_params.h" -#include "fsw_spacewire.h" - -typedef struct ring_node_sm -{ - struct ring_node_sm *previous; - struct ring_node_sm *next; - int buffer_address; - unsigned int status; - unsigned int coarseTime; - unsigned int fineTime; -} ring_node_sm; - -typedef struct ring_node_asm -{ - struct ring_node_asm *next; - float matrix[ TOTAL_SIZE_SM ]; - unsigned int status; -} ring_node_asm; - -typedef struct bp_packet -{ - Header_TM_LFR_SCIENCE_BP_t header; - unsigned char data[ 30 * 22 ]; // MAX size is 22 * 30 [TM_LFR_SCIENCE_BURST_BP2_F1] -} bp_packet; - -typedef struct bp_packet_with_spare -{ - Header_TM_LFR_SCIENCE_BP_with_spare_t header; - unsigned char data[ 9 * 13 ]; // only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1 -} bp_packet_with_spare; - -typedef struct asm_msg -{ - ring_node_asm *norm; - ring_node_asm *burst_sbm; - rtems_event_set event; - unsigned int coarseTime; - unsigned int fineTime; -} asm_msg; - -extern nb_sm_before_bp_asm_f0 nb_sm_before_f0; -extern nb_sm_before_bp_asm_f1 nb_sm_before_f1; -extern nb_sm_before_bp_asm_f2 nb_sm_before_f2; - -extern volatile int sm_f0[ ]; -extern volatile int sm_f1[ ]; -extern volatile int sm_f2[ ]; - -// parameters -extern struct param_local_str param_local; - -// registers -extern time_management_regs_t *time_management_regs; -extern spectral_matrix_regs_t *spectral_matrix_regs; - -extern rtems_name misc_name[5]; -extern rtems_id Task_id[20]; /* array of task ids */ - -// ISR -void reset_nb_sm_f0( unsigned char lfrMode ); -rtems_isr spectral_matrices_isr( rtems_vector_number vector ); -rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector ); - -// RTEMS TASKS -rtems_task avf0_task( rtems_task_argument lfrRequestedMode ); -rtems_task prc0_task( rtems_task_argument lfrRequestedMode ); -rtems_task avf1_task( rtems_task_argument lfrRequestedMode ); -rtems_task prc1_task( rtems_task_argument lfrRequestedMode ); - -//****************** -// Spectral Matrices -void SM_init_rings( void ); -void ASM_init_rings( void ); -void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes ); -void SM_reset_current_ring_nodes( void ); -void ASM_reset_current_ring_nodes( void ); -void ASM_init_header( Header_TM_LFR_SCIENCE_ASM_t *header); -void SM_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_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); - -//***************** -// Basic Parameters - -void BP_reset_current_ring_nodes( void ); -void BP_init_header(Header_TM_LFR_SCIENCE_BP_t *header, - unsigned int apid, unsigned char sid, - unsigned int packetLength , unsigned char blkNr); -void BP_init_header_with_spare(Header_TM_LFR_SCIENCE_BP_with_spare_t *header, - unsigned int apid, unsigned char sid, - unsigned int packetLength, unsigned char blkNr ); -void BP_send(char *data, - rtems_id queue_id , - unsigned int nbBytesToSend ); - -//****************** -// general functions -void reset_spectral_matrix_regs( void ); -void set_time(unsigned char *time, unsigned char *timeInBuffer ); - -extern rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id ); -extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id ); - -#endif // FSW_PROCESSING_H_INCLUDED diff --git a/header/tc_load_dump_parameters.h b/header/tc_load_dump_parameters.h --- a/header/tc_load_dump_parameters.h +++ b/header/tc_load_dump_parameters.h @@ -9,10 +9,6 @@ #include "tm_lfr_tc_exe.h" #include "fsw_misc.h" -extern nb_sm_before_bp_asm_f0 nb_sm_before_f0; -extern nb_sm_before_bp_asm_f1 nb_sm_before_f1; -extern nb_sm_before_bp_asm_f2 nb_sm_before_f2; - int action_load_common_par( ccsdsTelecommandPacket_t *TC ); int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time); int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time); diff --git a/src/avf_prc/avf0_prc0.c b/src/avf_prc/avf0_prc0.c new file mode 100644 --- /dev/null +++ b/src/avf_prc/avf0_prc0.c @@ -0,0 +1,358 @@ +/** 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 "avf0_prc0.h" + +ring_node_asm asm_ring_norm_f0 [ NB_RING_NODES_ASM_NORM_F0 ]; +ring_node_asm asm_ring_burst_sbm_f0[ NB_RING_NODES_ASM_BURST_SBM_F0 ]; +ring_node_asm *current_ring_node_asm_burst_sbm_f0; +ring_node_asm *current_ring_node_asm_norm_f0; + +float asm_f0_reorganized [ TOTAL_SIZE_SM ]; +char asm_f0_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ]; +float compressed_sm_norm_f0[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F0]; +float compressed_sm_sbm_f0 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F0 ]; + +nb_sm_before_bp_asm_f0 nb_sm_before_f0; + +void reset_nb_sm_f0( unsigned char lfrMode ) +{ + nb_sm_before_f0.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 96; + nb_sm_before_f0.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 96; + nb_sm_before_f0.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1]) * 96; + nb_sm_before_f0.sbm1_bp1 = parameter_dump_packet.sy_lfr_s1_bp_p0 * 24; + nb_sm_before_f0.sbm1_bp2 = parameter_dump_packet.sy_lfr_s1_bp_p1 * 96; + nb_sm_before_f0.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 96; + nb_sm_before_f0.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 96; + nb_sm_before_f0.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 96; + nb_sm_before_f0.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 96; + + if (lfrMode == LFR_MODE_SBM1) + { + nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm1_bp1; + nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm1_bp2; + } + else if (lfrMode == LFR_MODE_SBM2) + { + nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm2_bp1; + nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm2_bp2; + } + else if (lfrMode == LFR_MODE_BURST) + { + nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1; + nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2; + } + else + { + nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1; + nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2; + } +} + +//************ +// RTEMS TASKS + +rtems_task avf0_task( rtems_task_argument lfrRequestedMode ) +{ + int i; + + rtems_event_set event_out; + rtems_status_code status; + rtems_id queue_id_prc0; + asm_msg msgForMATR; + ring_node_sm *ring_node_tab[8]; + + 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; + + nb_norm_bp1 = 0; + nb_norm_bp2 = 0; + nb_norm_asm = 0; + nb_sbm_bp1 = 0; + nb_sbm_bp2 = 0; + + reset_nb_sm_f0( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions + ASM_generic_init_ring( asm_ring_norm_f0, NB_RING_NODES_ASM_NORM_F0 ); + ASM_generic_init_ring( asm_ring_burst_sbm_f0, NB_RING_NODES_ASM_BURST_SBM_F0 ); + current_ring_node_asm_norm_f0 = asm_ring_norm_f0; + current_ring_node_asm_burst_sbm_f0 = asm_ring_burst_sbm_f0; + + BOOT_PRINTF1("in AVFO *** lfrRequestedMode = %d\n", (int) lfrRequestedMode) + + status = get_message_queue_id_prc0( &queue_id_prc0 ); + if (status != RTEMS_SUCCESSFUL) + { + PRINTF1("in MATR *** ERR get_message_queue_id_prc0 %d\n", status) + } + + while(1){ + rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0 + ring_node_tab[NB_SM_BEFORE_AVF0-1] = ring_node_for_averaging_sm_f0; + for ( i = 2; i < (NB_SM_BEFORE_AVF0+1); i++ ) + { + ring_node_for_averaging_sm_f0 = ring_node_for_averaging_sm_f0->previous; + ring_node_tab[NB_SM_BEFORE_AVF0-i] = ring_node_for_averaging_sm_f0; + } + + // compute the average and store it in the averaged_sm_f1 buffer + SM_average( current_ring_node_asm_norm_f0->matrix, + current_ring_node_asm_burst_sbm_f0->matrix, + ring_node_tab, + nb_norm_bp1, nb_sbm_bp1 ); + + // update nb_average + nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0; + nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0; + nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0; + nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0; + nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0; + + //**************************************** + // initialize the mesage for the MATR task + msgForMATR.event = 0x00; // this composite event will be sent to the MATR task + msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f0; + msgForMATR.norm = current_ring_node_asm_norm_f0; +// msgForMATR.coarseTime = ( (unsigned int *) (ring_node_tab[0]->buffer_address) )[0]; +// msgForMATR.fineTime = ( (unsigned int *) (ring_node_tab[0]->buffer_address) )[1]; + msgForMATR.coarseTime = time_management_regs->coarse_time; + msgForMATR.fineTime = time_management_regs->fine_time; + + if (nb_sbm_bp1 == nb_sm_before_f0.burst_sbm_bp1) + { + nb_sbm_bp1 = 0; + // set another ring for the ASM storage + current_ring_node_asm_burst_sbm_f0 = current_ring_node_asm_burst_sbm_f0->next; + if ( (lfrCurrentMode == LFR_MODE_BURST) + || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) + { + msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_SBM_BP1_F0; + } + } + + if (nb_sbm_bp2 == nb_sm_before_f0.burst_sbm_bp2) + { + nb_sbm_bp2 = 0; + if ( (lfrCurrentMode == LFR_MODE_BURST) + || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) + { + msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_SBM_BP2_F0; + } + } + + if (nb_norm_bp1 == nb_sm_before_f0.norm_bp1) + { + nb_norm_bp1 = 0; + // set another ring for the ASM storage + current_ring_node_asm_norm_f0 = current_ring_node_asm_norm_f0->next; + if ( (lfrCurrentMode == LFR_MODE_NORMAL) + || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) + { + msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F0; + } + } + + if (nb_norm_bp2 == nb_sm_before_f0.norm_bp2) + { + nb_norm_bp2 = 0; + if ( (lfrCurrentMode == LFR_MODE_NORMAL) + || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) + { + msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F0; + } + } + + if (nb_norm_asm == nb_sm_before_f0.norm_asm) + { + nb_norm_asm = 0; + if ( (lfrCurrentMode == LFR_MODE_NORMAL) + || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) + { +// PRINTF1("%lld\n", localTime) + msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F0; + } + } + + //************************* + // send the message to MATR + if (msgForMATR.event != 0x00) + { + status = rtems_message_queue_send( queue_id_prc0, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC0); + } + + if (status != RTEMS_SUCCESSFUL) { + printf("in AVF0 *** Error sending message to MATR, code %d\n", status); + } + } +} + +rtems_task prc0_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; + // + spw_ioctl_pkt_send spw_ioctl_send_ASM; + rtems_status_code status; + rtems_id queue_id; + rtems_id queue_id_q_p0; + Header_TM_LFR_SCIENCE_ASM_t headerASM; + bp_packet_with_spare packet_norm_bp1_f0; + bp_packet packet_norm_bp2_f0; + bp_packet packet_sbm_bp1_f0; + bp_packet packet_sbm_bp2_f0; + + unsigned long long int localTime; + + ASM_init_header( &headerASM ); + + //************* + // NORM headers + BP_init_header_with_spare( &packet_norm_bp1_f0.header, + APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F0, + PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0, NB_BINS_COMPRESSED_SM_F0 ); + BP_init_header( &packet_norm_bp2_f0.header, + APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F0, + PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0, NB_BINS_COMPRESSED_SM_F0); + + //**************************** + // BURST SBM1 and SBM2 headers + if ( lfrRequestedMode == LFR_MODE_BURST ) + { + BP_init_header( &packet_sbm_bp1_f0.header, + APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F0, + PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0); + BP_init_header( &packet_sbm_bp2_f0.header, + APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F0, + PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0); + } + else if ( lfrRequestedMode == LFR_MODE_SBM1 ) + { + BP_init_header( &packet_sbm_bp1_f0.header, + APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP1_F0, + PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0); + BP_init_header( &packet_sbm_bp2_f0.header, + APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP2_F0, + PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0); + } + else if ( lfrRequestedMode == LFR_MODE_SBM2 ) + { + BP_init_header( &packet_sbm_bp1_f0.header, + APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F0, + PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0); + BP_init_header( &packet_sbm_bp2_f0.header, + APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F0, + PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0); + } + else + { + PRINTF1("in PRC0 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode) + } + + status = get_message_queue_id_send( &queue_id ); + if (status != RTEMS_SUCCESSFUL) + { + PRINTF1("in PRC0 *** ERR get_message_queue_id_send %d\n", status) + } + status = get_message_queue_id_prc0( &queue_id_q_p0); + if (status != RTEMS_SUCCESSFUL) + { + PRINTF1("in PRC0 *** ERR get_message_queue_id_prc0 %d\n", status) + } + + BOOT_PRINTF1("in PRC0 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode) + + while(1){ + status = rtems_message_queue_receive( queue_id_q_p0, incomingData, &size, //************************************ + RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0 + + incomingMsg = (asm_msg*) incomingData; + + localTime = getTimeAsUnsignedLongLongInt( ); + //**************** + //**************** + // BURST SBM1 SBM2 + //**************** + //**************** + if (incomingMsg->event & RTEMS_EVENT_BURST_SBM_BP1_F0 ) + { + // 1) compress the matrix for Basic Parameters calculation + ASM_compress_reorganize_and_divide( incomingMsg->burst_sbm->matrix, compressed_sm_sbm_f0, + nb_sm_before_f0.burst_sbm_bp1, + NB_BINS_COMPRESSED_SM_SBM_F0, NB_BINS_TO_AVERAGE_ASM_SBM_F0, + ASM_F0_INDICE_START); + // 2) compute the BP1 set + + // 3) send the BP1 set + set_time( packet_sbm_bp1_f0.header.time, (unsigned char *) &incomingMsg->coarseTime ); + set_time( packet_sbm_bp1_f0.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); + BP_send( (char *) &packet_sbm_bp1_f0.header, queue_id, + PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0 + PACKET_LENGTH_DELTA); + // 4) compute the BP2 set if needed + if ( incomingMsg->event & RTEMS_EVENT_BURST_SBM_BP2_F0 ) + { + // 1) compute the BP2 set + + // 2) send the BP2 set + set_time( packet_sbm_bp2_f0.header.time, (unsigned char *) &incomingMsg->coarseTime ); + set_time( packet_sbm_bp2_f0.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); + BP_send( (char *) &packet_sbm_bp2_f0.header, queue_id, + PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0 + PACKET_LENGTH_DELTA); + } + } + + //***** + //***** + // NORM + //***** + //***** + if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F0) + { + // 1) compress the matrix for Basic Parameters calculation + ASM_compress_reorganize_and_divide( incomingMsg->norm->matrix, compressed_sm_norm_f0, + nb_sm_before_f0.norm_bp1, + 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 + set_time( packet_norm_bp1_f0.header.time, (unsigned char *) &incomingMsg->coarseTime ); + set_time( packet_norm_bp1_f0.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); + BP_send( (char *) &packet_norm_bp1_f0.header, queue_id, + PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0 + PACKET_LENGTH_DELTA); + if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F0) + { + // 1) compute the BP2 set using the same ASM as the one used for BP1 + + // 2) send the BP2 set + set_time( packet_norm_bp2_f0.header.time, (unsigned char *) &incomingMsg->coarseTime ); + set_time( packet_norm_bp2_f0.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); + BP_send( (char *) &packet_norm_bp2_f0.header, queue_id, + PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0 + PACKET_LENGTH_DELTA); + } + } + + if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F0) + { + // 1) reorganize the ASM and divide + ASM_reorganize_and_divide( incomingMsg->norm->matrix, + asm_f0_reorganized, + nb_sm_before_f0.norm_bp1 ); + // 2) convert the float array in a char array + ASM_convert( asm_f0_reorganized, asm_f0_char); + // 3) send the spectral matrix packets + set_time( headerASM.time , (unsigned char *) &incomingMsg->coarseTime ); + set_time( headerASM.acquisitionTime, (unsigned char *) &incomingMsg->coarseTime ); + ASM_send( &headerASM, asm_f0_char, SID_NORM_ASM_F0, &spw_ioctl_send_ASM, queue_id); + } + + } +} diff --git a/src/avf_prc/avf0_prc0.h b/src/avf_prc/avf0_prc0.h new file mode 100644 --- /dev/null +++ b/src/avf_prc/avf0_prc0.h @@ -0,0 +1,30 @@ +#ifndef AVF0_PRC0_H +#define AVF0_PRC0_H + +#include "fsw_processing.h" + +typedef struct { + unsigned int norm_bp1; + unsigned int norm_bp2; + unsigned int norm_asm; + unsigned int burst_sbm_bp1; + unsigned int burst_sbm_bp2; + unsigned int burst_bp1; + unsigned int burst_bp2; + unsigned int sbm1_bp1; + unsigned int sbm1_bp2; + unsigned int sbm2_bp1; + unsigned int sbm2_bp2; +} nb_sm_before_bp_asm_f0; + +extern struct ring_node_sm *current_ring_node_sm_f0; +extern struct ring_node_sm *ring_node_for_averaging_sm_f0; + +extern rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id ); + +void reset_nb_sm_f0( unsigned char lfrMode ); + +rtems_task avf0_task( rtems_task_argument lfrRequestedMode ); +rtems_task prc0_task( rtems_task_argument lfrRequestedMode ); + +#endif // AVF0_PRC0_H diff --git a/src/avf_prc/avf1_prc1.c b/src/avf_prc/avf1_prc1.c new file mode 100644 --- /dev/null +++ b/src/avf_prc/avf1_prc1.c @@ -0,0 +1,339 @@ +/** 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" + +ring_node_asm asm_ring_norm_f1 [ NB_RING_NODES_ASM_NORM_F1 ]; +ring_node_asm asm_ring_burst_sbm_f1[ NB_RING_NODES_ASM_BURST_SBM_F1 ]; +ring_node_asm *current_ring_node_asm_burst_sbm_f1; +ring_node_asm *current_ring_node_asm_norm_f1; + +float asm_f1_reorganized [ TOTAL_SIZE_SM ]; +char asm_f1_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ]; +float compressed_sm_norm_f1[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F1]; +float compressed_sm_sbm_f1 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 ]; + +nb_sm_before_bp_asm_f1 nb_sm_before_f1; + +void reset_nb_sm_f1( unsigned char lfrMode ) +{ + nb_sm_before_f1.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 16; + nb_sm_before_f1.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 16; + 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]) * 16; + nb_sm_before_f1.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 16; + nb_sm_before_f1.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 16; + nb_sm_before_f1.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 16; + nb_sm_before_f1.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 16; + + 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; + } +} + +//************ +// 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 msgForMATR; + ring_node_sm *ring_node_tab[8]; + + 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; + + 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 + ring_node_tab[NB_SM_BEFORE_AVF1-1] = ring_node_for_averaging_sm_f1; + for ( i = 2; i < (NB_SM_BEFORE_AVF1+1); i++ ) + { + ring_node_for_averaging_sm_f1 = ring_node_for_averaging_sm_f1->previous; + ring_node_tab[NB_SM_BEFORE_AVF1-i] = ring_node_for_averaging_sm_f1; + } + + // 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 ); + + // update nb_average + nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF1; + nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF1; + nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF1; + nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF1; + nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF1; + + //**************************************** + // initialize the mesage for the MATR task + msgForMATR.event = 0x00; // this composite event will be sent to the PRC1 task + msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f1; + msgForMATR.norm = current_ring_node_asm_norm_f1; +// msgForMATR.coarseTime = ( (unsigned int *) (ring_node_tab[0]->buffer_address) )[0]; +// msgForMATR.fineTime = ( (unsigned int *) (ring_node_tab[0]->buffer_address) )[1]; + msgForMATR.coarseTime = time_management_regs->coarse_time; + msgForMATR.fineTime = time_management_regs->fine_time; + + 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) || (lfrCurrentMode == LFR_MODE_SBM2) ) + { + msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_SBM_BP1_F1; + } + } + + if (nb_sbm_bp2 == nb_sm_before_f1.burst_sbm_bp2) + { + nb_sbm_bp2 = 0; + if ( (lfrCurrentMode == LFR_MODE_BURST) || (lfrCurrentMode == LFR_MODE_SBM2) ) + { + msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_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) ) + { + msgForMATR.event = msgForMATR.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) ) + { + msgForMATR.event = msgForMATR.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) ) + { + msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F1; + } + } + + //************************* + // send the message to MATR + if (msgForMATR.event != 0x00) + { + status = rtems_message_queue_send( queue_id_prc1, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC1); + } + + if (status != RTEMS_SUCCESSFUL) { + printf("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; + // + spw_ioctl_pkt_send spw_ioctl_send_ASM; + rtems_status_code status; + rtems_id queue_id_send; + rtems_id queue_id_q_p1; + Header_TM_LFR_SCIENCE_ASM_t headerASM; + bp_packet_with_spare packet_norm_bp1; + bp_packet packet_norm_bp2; + bp_packet packet_sbm_bp1; + bp_packet packet_sbm_bp2; + + unsigned long long int localTime; + + ASM_init_header( &headerASM ); + + //************* + // NORM headers + BP_init_header_with_spare( &packet_norm_bp1.header, + 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.header, + 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.header, + APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F1, + PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F1); + BP_init_header( &packet_sbm_bp2.header, + APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F1, + PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F1); + } + else if ( lfrRequestedMode == LFR_MODE_SBM2 ) + { + BP_init_header( &packet_sbm_bp1.header, + 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.header, + 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; + + localTime = getTimeAsUnsignedLongLongInt( ); + //*********** + //*********** + // BURST SBM2 + //*********** + //*********** + if (incomingMsg->event & RTEMS_EVENT_BURST_SBM_BP1_F1 ) + { + // 1) compress the matrix for Basic Parameters calculation + ASM_compress_reorganize_and_divide( incomingMsg->burst_sbm->matrix, compressed_sm_sbm_f1, + nb_sm_before_f1.burst_sbm_bp1, + NB_BINS_COMPRESSED_SM_SBM_F1, NB_BINS_TO_AVERAGE_ASM_SBM_F1, + ASM_F1_INDICE_START); + // 2) compute the BP1 set + + // 3) send the BP1 set + set_time( packet_sbm_bp1.header.time, (unsigned char *) &incomingMsg->coarseTime ); + set_time( packet_sbm_bp1.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); + BP_send( (char *) &packet_sbm_bp1.header, queue_id_send, + PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 + PACKET_LENGTH_DELTA); + // 4) compute the BP2 set if needed + if ( incomingMsg->event & RTEMS_EVENT_BURST_SBM_BP2_F1 ) + { + // 1) compute the BP2 set + + // 2) send the BP2 set + set_time( packet_sbm_bp2.header.time, (unsigned char *) &incomingMsg->coarseTime ); + set_time( packet_sbm_bp2.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); + BP_send( (char *) &packet_sbm_bp2.header, queue_id_send, + PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1 + PACKET_LENGTH_DELTA); + } + } + + //***** + //***** + // NORM + //***** + //***** + if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F1) + { + // 1) compress the matrix for Basic Parameters calculation + ASM_compress_reorganize_and_divide( incomingMsg->norm->matrix, compressed_sm_norm_f1, + nb_sm_before_f1.norm_bp1, + 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 + set_time( packet_norm_bp1.header.time, (unsigned char *) &incomingMsg->coarseTime ); + set_time( packet_norm_bp1.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); + BP_send( (char *) &packet_norm_bp1.header, queue_id_send, + PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 + PACKET_LENGTH_DELTA); + if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F1) + { + // 1) compute the BP2 set + + // 2) send the BP2 set + set_time( packet_norm_bp2.header.time, (unsigned char *) &incomingMsg->coarseTime ); + set_time( packet_norm_bp2.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); + BP_send( (char *) &packet_norm_bp2.header, queue_id_send, + PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 + PACKET_LENGTH_DELTA); + } + } + + if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F1) + { + // 1) reorganize the ASM and divide + ASM_reorganize_and_divide( incomingMsg->norm->matrix, + asm_f1_reorganized, + nb_sm_before_f1.norm_bp1 ); + // 2) convert the float array in a char array + ASM_convert( asm_f1_reorganized, asm_f1_char); + // 3) send the spectral matrix packets + set_time( headerASM.time , (unsigned char *) &incomingMsg->coarseTime ); + set_time( headerASM.acquisitionTime, (unsigned char *) &incomingMsg->coarseTime ); + ASM_send( &headerASM, asm_f1_char, SID_NORM_ASM_F1, &spw_ioctl_send_ASM, queue_id_send); + } + + } +} diff --git a/src/avf_prc/avf1_prc1.h b/src/avf_prc/avf1_prc1.h new file mode 100644 --- /dev/null +++ b/src/avf_prc/avf1_prc1.h @@ -0,0 +1,28 @@ +#ifndef AVF1_PRC1_H +#define AVF1_PRC1_H + +#include "fsw_processing.h" + +typedef struct { + unsigned int norm_bp1; + unsigned int norm_bp2; + unsigned int norm_asm; + unsigned int burst_sbm_bp1; + unsigned int burst_sbm_bp2; + unsigned int burst_bp1; + unsigned int burst_bp2; + unsigned int sbm2_bp1; + unsigned int sbm2_bp2; +} nb_sm_before_bp_asm_f1; + +extern struct ring_node_sm *current_ring_node_sm_f1; +extern struct ring_node_sm *ring_node_for_averaging_sm_f1; + +extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id ); + +void reset_nb_sm_f1( unsigned char lfrMode ); + +rtems_task avf1_task( rtems_task_argument lfrRequestedMode ); +rtems_task prc1_task( rtems_task_argument lfrRequestedMode ); + +#endif // AVF1_PRC1_H diff --git a/src/avf_prc/avf2_prc2.c b/src/avf_prc/avf2_prc2.c new file mode 100644 --- /dev/null +++ b/src/avf_prc/avf2_prc2.c @@ -0,0 +1,240 @@ +/** Functions related to data processing. + * + * @file + * @author P. LEROY + * + * These function are related to data processing at f2 = 256 Hz, i.e. spectral matrices averaging and basic parameters computation. + * + */ + +#include "avf2_prc2.h" + +ring_node_asm asm_ring_norm_f2 [ NB_RING_NODES_ASM_NORM_F2 ]; +ring_node_asm asm_ring_burst_sbm_f2[ NB_RING_NODES_ASM_BURST_SBM_F2 ]; +ring_node_asm *current_ring_node_asm_burst_sbm_f2; +ring_node_asm *current_ring_node_asm_norm_f2; + +float asm_f2_reorganized [ TOTAL_SIZE_SM ]; +char asm_f2_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ]; +float compressed_sm_norm_f2[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F2]; +float compressed_sm_sbm_f2 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F2 ]; + +nb_sm_before_bp_asm_f2 nb_sm_before_f2; + +void reset_nb_sm_f2( void ) +{ + nb_sm_before_f2.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0; + nb_sm_before_f2.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1; + nb_sm_before_f2.norm_asm = parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1]; +} + +void SM_average_f2( float *averaged_spec_mat_f2, + ring_node_sm *ring_node, + unsigned int nbAverageNormF2 ) +{ + float sum; + unsigned int i; + + for(i=0; ibuffer_address) ) [ i ]; + if ( (nbAverageNormF2 == 0) ) + { + averaged_spec_mat_f2[ i ] = sum; + } + else + { + averaged_spec_mat_f2[ i ] = ( averaged_spec_mat_f2[ i ] + sum ); + } + } +} + +//************ +// RTEMS TASKS + +rtems_task avf2_task( rtems_task_argument lfrRequestedMode ) +{ + rtems_event_set event_out; + rtems_status_code status; + rtems_id queue_id_prc2; + asm_msg msgForMATR; + + unsigned int nb_norm_bp1; + unsigned int nb_norm_bp2; + unsigned int nb_norm_asm; + + nb_norm_bp1 = 0; + nb_norm_bp2 = 0; + nb_norm_asm = 0; + + reset_nb_sm_f2( ); // reset the sm counters that drive the BP and ASM computations / transmissions + + BOOT_PRINTF1("in AVF2 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode) + + status = get_message_queue_id_prc2( &queue_id_prc2 ); + if (status != RTEMS_SUCCESSFUL) + { + PRINTF1("in AVF2 *** ERR get_message_queue_id_prc2 %d\n", status) + } + + while(1){ + rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0 + + // compute the average and store it in the averaged_sm_f2 buffer + SM_average_f2( current_ring_node_asm_norm_f2->matrix, + ring_node_for_averaging_sm_f2, + nb_norm_bp1 ); + + // update nb_average + nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF2; + nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF2; + nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF2; + + //**************************************** + // initialize the mesage for the MATR task + msgForMATR.event = 0x00; // this composite event will be sent to the MATR task + msgForMATR.burst_sbm = NULL; + msgForMATR.norm = current_ring_node_asm_norm_f2; +// msgForMATR.coarseTime = ( (unsigned int *) (ring_node_tab[0]->buffer_address) )[0]; +// msgForMATR.fineTime = ( (unsigned int *) (ring_node_tab[0]->buffer_address) )[1]; + msgForMATR.coarseTime = time_management_regs->coarse_time; + msgForMATR.fineTime = time_management_regs->fine_time; + + if (nb_norm_bp1 == nb_sm_before_f2.norm_bp1) + { + nb_norm_bp1 = 0; + // set another ring for the ASM storage + current_ring_node_asm_norm_f2 = current_ring_node_asm_norm_f2->next; + if ( lfrCurrentMode == LFR_MODE_NORMAL ) + { + msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F0; + } + } + + if (nb_norm_bp2 == nb_sm_before_f2.norm_bp2) + { + nb_norm_bp2 = 0; + if ( lfrCurrentMode == LFR_MODE_NORMAL ) + { + msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F2; + } + } + + if (nb_norm_asm == nb_sm_before_f2.norm_asm) + { + nb_norm_asm = 0; + if ( lfrCurrentMode == LFR_MODE_NORMAL ) + { +// PRINTF1("%lld\n", localTime) + msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F2; + } + } + + //************************* + // send the message to MATR + if (msgForMATR.event != 0x00) + { + status = rtems_message_queue_send( queue_id_prc2, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC0); + } + + if (status != RTEMS_SUCCESSFUL) { + printf("in AVF2 *** Error sending message to MATR, code %d\n", status); + } + } +} + +rtems_task prc2_task( rtems_task_argument argument ) +{ + char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer + size_t size; // size of the incoming TC packet + asm_msg *incomingMsg; + // + spw_ioctl_pkt_send spw_ioctl_send_ASM; + rtems_status_code status; + rtems_id queue_id; + rtems_id queue_id_q_p2; + Header_TM_LFR_SCIENCE_ASM_t headerASM; + bp_packet packet_norm_bp1_f2; + bp_packet packet_norm_bp2_f2; + + unsigned long long int localTime; + + ASM_init_header( &headerASM ); + + //************* + // NORM headers + BP_init_header( &packet_norm_bp1_f2.header, + APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F2, + PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2, NB_BINS_COMPRESSED_SM_F2 ); + BP_init_header( &packet_norm_bp2_f2.header, + APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F2, + PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2, NB_BINS_COMPRESSED_SM_F2 ); + + status = get_message_queue_id_send( &queue_id ); + if (status != RTEMS_SUCCESSFUL) + { + PRINTF1("in PRC2 *** ERR get_message_queue_id_send %d\n", status) + } + status = get_message_queue_id_prc2( &queue_id_q_p2); + if (status != RTEMS_SUCCESSFUL) + { + PRINTF1("in PRC2 *** ERR get_message_queue_id_prc2 %d\n", status) + } + + BOOT_PRINTF("in PRC2 ***\n") + + while(1){ + status = rtems_message_queue_receive( queue_id_q_p2, incomingData, &size, //************************************ + RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0 + + incomingMsg = (asm_msg*) incomingData; + + localTime = getTimeAsUnsignedLongLongInt( ); + + //***** + //***** + // NORM + //***** + //***** + if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F2) + { + // 1) compress the matrix for Basic Parameters calculation + ASM_compress_reorganize_and_divide( incomingMsg->norm->matrix, compressed_sm_norm_f2, + nb_sm_before_f2.norm_bp1, + NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2, + ASM_F2_INDICE_START ); + // 2) compute the BP1 set + + // 3) send the BP1 set + set_time( packet_norm_bp1_f2.header.time, (unsigned char *) &incomingMsg->coarseTime ); + set_time( packet_norm_bp1_f2.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); + BP_send( (char *) &packet_norm_bp1_f2.header, queue_id, + PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2 + PACKET_LENGTH_DELTA); + if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F2) + { + // 1) compute the BP2 set using the same ASM as the one used for BP1 + + // 2) send the BP2 set + set_time( packet_norm_bp2_f2.header.time, (unsigned char *) &incomingMsg->coarseTime ); + set_time( packet_norm_bp2_f2.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); + BP_send( (char *) &packet_norm_bp2_f2.header, queue_id, + PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2 + PACKET_LENGTH_DELTA); + } + } + + if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F2) + { + // 1) reorganize the ASM and divide + ASM_reorganize_and_divide( incomingMsg->norm->matrix, + asm_f2_reorganized, + nb_sm_before_f2.norm_bp1 ); + // 2) convert the float array in a char array + ASM_convert( asm_f2_reorganized, asm_f2_char); + // 3) send the spectral matrix packets + set_time( headerASM.time , (unsigned char *) &incomingMsg->coarseTime ); + set_time( headerASM.acquisitionTime, (unsigned char *) &incomingMsg->coarseTime ); + ASM_send( &headerASM, asm_f2_char, SID_NORM_ASM_F2, &spw_ioctl_send_ASM, queue_id); + } + + } +} diff --git a/src/avf_prc/avf2_prc2.h b/src/avf_prc/avf2_prc2.h new file mode 100644 --- /dev/null +++ b/src/avf_prc/avf2_prc2.h @@ -0,0 +1,24 @@ +#ifndef AVF2_PRC2_H +#define AVF2_PRC2_H + +#include "fsw_processing.h" + +typedef struct { + unsigned int norm_bp1; + unsigned int norm_bp2; + unsigned int norm_asm; +} nb_sm_before_bp_asm_f2; + +extern struct ring_node_sm *current_ring_node_sm_f2; +extern struct ring_node_sm *ring_node_for_averaging_sm_f2; + +extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id ); + +void reset_nb_sm_f2(void ); +void SM_average_f2( float *averaged_spec_mat_f2, ring_node_sm *ring_node, unsigned int nbAverageNormF2 ); + +// RTEMS TASKS +rtems_task avf2_task( rtems_task_argument lfrRequestedMode ); +rtems_task prc2_task(rtems_task_argument argument ); + +#endif // AVF2_PRC2_H diff --git a/src/avf_prc/fsw_processing.c b/src/avf_prc/fsw_processing.c new file mode 100644 --- /dev/null +++ b/src/avf_prc/fsw_processing.c @@ -0,0 +1,570 @@ +/** 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 "fsw_processing.h" +#include "fsw_processing_globals.c" + +unsigned int nb_sm_f0; +unsigned int nb_sm_f0_aux_f1; +unsigned int nb_sm_f1; +unsigned int nb_sm_f0_aux_f2; + +//************************ +// spectral matrices rings +ring_node_sm sm_ring_f0[ NB_RING_NODES_SM_F0 ]; +ring_node_sm sm_ring_f1[ NB_RING_NODES_SM_F1 ]; +ring_node_sm sm_ring_f2[ NB_RING_NODES_SM_F2 ]; +ring_node_sm *current_ring_node_sm_f0; +ring_node_sm *current_ring_node_sm_f1; +ring_node_sm *current_ring_node_sm_f2; +ring_node_sm *ring_node_for_averaging_sm_f0; +ring_node_sm *ring_node_for_averaging_sm_f1; +ring_node_sm *ring_node_for_averaging_sm_f2; + +//*********************************************************** +// Interrupt Service Routine for spectral matrices processing + +rtems_isr spectral_matrices_isr( rtems_vector_number vector ) +{ + ring_node_sm *previous_ring_node_sm_f0; + +// rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 ); + + previous_ring_node_sm_f0 = current_ring_node_sm_f0; + + if ( (spectral_matrix_regs->status & 0x2) == 0x02) // check ready matrix bit f0_1 + { + current_ring_node_sm_f0 = current_ring_node_sm_f0->next; + spectral_matrix_regs->matrixF0_Address0 = current_ring_node_sm_f0->buffer_address; + spectral_matrix_regs->status = spectral_matrix_regs->status & 0xfffffffd; // 1101 + nb_sm_f0 = nb_sm_f0 + 1; + } + + //************************ + // reset status error bits + if ( (spectral_matrix_regs->status & 0x30) != 0x00) + { + rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 ); + spectral_matrix_regs->status = spectral_matrix_regs->status & 0xffffffcf; // 1100 1111 + } + + //************************************** + // reset ready matrix bits for f0_0, f1 and f2 + spectral_matrix_regs->status = spectral_matrix_regs->status & 0xfffffff2; // 0010 + + if (nb_sm_f0 == NB_SM_BEFORE_AVF0) + { + ring_node_for_averaging_sm_f0 = previous_ring_node_sm_f0; + if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) + { + rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); + } + nb_sm_f0 = 0; + } + +} + +rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector ) +{ + //*** + // F0 + nb_sm_f0 = nb_sm_f0 + 1; + if (nb_sm_f0 == NB_SM_BEFORE_AVF0 ) + { + ring_node_for_averaging_sm_f0 = current_ring_node_sm_f0; + if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) + { + rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); + } + nb_sm_f0 = 0; + } + + //*** + // F1 + nb_sm_f0_aux_f1 = nb_sm_f0_aux_f1 + 1; + if (nb_sm_f0_aux_f1 == 6) + { + nb_sm_f0_aux_f1 = 0; + nb_sm_f1 = nb_sm_f1 + 1; + } + if (nb_sm_f1 == NB_SM_BEFORE_AVF1 ) + { + ring_node_for_averaging_sm_f1 = current_ring_node_sm_f1; + if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) + { + rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); + } + nb_sm_f1 = 0; + } + + //*** + // F2 + nb_sm_f0_aux_f2 = nb_sm_f0_aux_f2 + 1; + if (nb_sm_f0_aux_f2 == 96) + { + nb_sm_f0_aux_f2 = 0; + ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2; + if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) + { + rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); + } + } +} + +//****************** +// Spectral Matrices + +void reset_nb_sm( void ) +{ + nb_sm_f0 = 0; + nb_sm_f0_aux_f1 = 0; + nb_sm_f0_aux_f2 = 0; + + nb_sm_f1 = 0; +} + +void SM_init_rings( void ) +{ + unsigned char i; + + // F0 RING + sm_ring_f0[0].next = (ring_node_sm*) &sm_ring_f0[1]; + sm_ring_f0[0].previous = (ring_node_sm*) &sm_ring_f0[NB_RING_NODES_SM_F0-1]; + sm_ring_f0[0].buffer_address = + (int) &sm_f0[ 0 ]; + + sm_ring_f0[NB_RING_NODES_SM_F0-1].next = (ring_node_sm*) &sm_ring_f0[0]; + sm_ring_f0[NB_RING_NODES_SM_F0-1].previous = (ring_node_sm*) &sm_ring_f0[NB_RING_NODES_SM_F0-2]; + sm_ring_f0[NB_RING_NODES_SM_F0-1].buffer_address = + (int) &sm_f0[ (NB_RING_NODES_SM_F0-1) * TOTAL_SIZE_SM ]; + + for(i=1; imatrixF0_Address0 = sm_ring_f0[0].buffer_address; + DEBUG_PRINTF1("spectral_matrix_regs->matrixF0_Address0 @%x\n", spectral_matrix_regs->matrixF0_Address0) +} + +void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes ) +{ + unsigned char i; + + ring[ nbNodes - 1 ].next + = (ring_node_asm*) &ring[ 0 ]; + + for(i=0; itargetLogicalAddress = CCSDS_DESTINATION_ID; + header->protocolIdentifier = CCSDS_PROTOCOLE_ID; + header->reserved = 0x00; + header->userApplication = CCSDS_USER_APP; + header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8); + header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); + header->packetSequenceControl[0] = 0xc0; + header->packetSequenceControl[1] = 0x00; + header->packetLength[0] = 0x00; + header->packetLength[1] = 0x00; + // DATA FIELD HEADER + header->spare1_pusVersion_spare2 = 0x10; + header->serviceType = TM_TYPE_LFR_SCIENCE; // service type + header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype + header->destinationID = TM_DESTINATION_ID_GROUND; + // AUXILIARY DATA HEADER + header->sid = 0x00; + header->biaStatusInfo = 0x00; + header->pa_lfr_pkt_cnt_asm = 0x00; + header->pa_lfr_pkt_nr_asm = 0x00; + header->time[0] = 0x00; + header->time[0] = 0x00; + header->time[0] = 0x00; + header->time[0] = 0x00; + header->time[0] = 0x00; + header->time[0] = 0x00; + header->pa_lfr_asm_blk_nr[0] = 0x00; // BLK_NR MSB + header->pa_lfr_asm_blk_nr[1] = 0x00; // BLK_NR LSB +} + +void SM_average( float *averaged_spec_mat_f0, float *averaged_spec_mat_f1, + ring_node_sm *ring_node_tab[], + unsigned int nbAverageNormF0, unsigned int nbAverageSBM1F0 ) +{ + float sum; + unsigned int i; + + for(i=0; ibuffer_address) ) [ i ] + + ( (int *) (ring_node_tab[1]->buffer_address) ) [ i ] + + ( (int *) (ring_node_tab[2]->buffer_address) ) [ i ] + + ( (int *) (ring_node_tab[3]->buffer_address) ) [ i ] + + ( (int *) (ring_node_tab[4]->buffer_address) ) [ i ] + + ( (int *) (ring_node_tab[5]->buffer_address) ) [ i ] + + ( (int *) (ring_node_tab[6]->buffer_address) ) [ i ] + + ( (int *) (ring_node_tab[7]->buffer_address) ) [ i ]; + + if ( (nbAverageNormF0 == 0) && (nbAverageSBM1F0 == 0) ) + { + averaged_spec_mat_f0[ i ] = sum; + averaged_spec_mat_f1[ i ] = sum; + } + else if ( (nbAverageNormF0 != 0) && (nbAverageSBM1F0 != 0) ) + { + averaged_spec_mat_f0[ i ] = ( averaged_spec_mat_f0[ i ] + sum ); + averaged_spec_mat_f1[ i ] = ( averaged_spec_mat_f1[ i ] + sum ); + } + else if ( (nbAverageNormF0 != 0) && (nbAverageSBM1F0 == 0) ) + { + averaged_spec_mat_f0[ i ] = ( averaged_spec_mat_f0[ i ] + sum ); + averaged_spec_mat_f1[ i ] = sum; + } + else + { + PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNormF0, nbAverageSBM1F0) + } + } +} + +void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider ) +{ + int frequencyBin; + int asmComponent; + unsigned int offsetAveragedSpecMatReorganized; + unsigned int offsetAveragedSpecMat; + + for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++) + { + for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ ) + { + offsetAveragedSpecMatReorganized = + frequencyBin * NB_VALUES_PER_SM + + asmComponent; + offsetAveragedSpecMat = + asmComponent * NB_BINS_PER_SM + + frequencyBin; + averaged_spec_mat_reorganized[offsetAveragedSpecMatReorganized ] = + averaged_spec_mat[ offsetAveragedSpecMat ] / 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 ) +{ + 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 ] ) / (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; frequencyBindlen = TOTAL_SIZE_ASM_F0_IN_BYTES / 2; // 2 packets will be sent + spw_ioctl_send->data = &spectral_matrix[ + ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0) ) * NB_VALUES_PER_SM ) * 2 + ]; + length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0; + header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0) >> 8 ); // BLK_NR MSB + header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0); // BLK_NR LSB + break; + case SID_NORM_ASM_F1: + spw_ioctl_send->dlen = TOTAL_SIZE_ASM_F1_IN_BYTES / 2; // 2 packets will be sent + spw_ioctl_send->data = &spectral_matrix[ + ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1) ) * NB_VALUES_PER_SM ) * 2 + ]; + length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1; + header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1) >> 8 ); // BLK_NR MSB + header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1); // BLK_NR LSB + break; + case SID_NORM_ASM_F2: + break; + default: + PRINTF1("ERR *** in ASM_send *** unexpected sid %d\n", sid) + break; + } + spw_ioctl_send->hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM + CCSDS_PROTOCOLE_EXTRA_BYTES; + spw_ioctl_send->hdr = (char *) header; + spw_ioctl_send->options = 0; + + // (2) BUILD THE HEADER + header->packetLength[0] = (unsigned char) (length>>8); + header->packetLength[1] = (unsigned char) (length); + header->sid = (unsigned char) sid; // SID + header->pa_lfr_pkt_cnt_asm = 2; + header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1); + + // (3) SET PACKET TIME + header->time[0] = (unsigned char) (time_management_regs->coarse_time>>24); + header->time[1] = (unsigned char) (time_management_regs->coarse_time>>16); + header->time[2] = (unsigned char) (time_management_regs->coarse_time>>8); + header->time[3] = (unsigned char) (time_management_regs->coarse_time); + header->time[4] = (unsigned char) (time_management_regs->fine_time>>8); + header->time[5] = (unsigned char) (time_management_regs->fine_time); + // + header->acquisitionTime[0] = (unsigned char) (time_management_regs->coarse_time>>24); + header->acquisitionTime[1] = (unsigned char) (time_management_regs->coarse_time>>16); + header->acquisitionTime[2] = (unsigned char) (time_management_regs->coarse_time>>8); + header->acquisitionTime[3] = (unsigned char) (time_management_regs->coarse_time); + header->acquisitionTime[4] = (unsigned char) (time_management_regs->fine_time>>8); + header->acquisitionTime[5] = (unsigned char) (time_management_regs->fine_time); + + // (4) SEND PACKET + status = rtems_message_queue_send( queue_id, spw_ioctl_send, ACTION_MSG_SPW_IOCTL_SEND_SIZE); + if (status != RTEMS_SUCCESSFUL) { + printf("in ASM_send *** ERR %d\n", (int) status); + } + } +} + +//***************** +// Basic Parameters + +void BP_init_header( Header_TM_LFR_SCIENCE_BP_t *header, + unsigned int apid, unsigned char sid, + unsigned int packetLength, unsigned char blkNr ) +{ + header->targetLogicalAddress = CCSDS_DESTINATION_ID; + header->protocolIdentifier = CCSDS_PROTOCOLE_ID; + header->reserved = 0x00; + header->userApplication = CCSDS_USER_APP; + header->packetID[0] = (unsigned char) (apid >> 8); + header->packetID[1] = (unsigned char) (apid); + header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; + header->packetSequenceControl[1] = 0x00; + header->packetLength[0] = (unsigned char) (packetLength >> 8); + header->packetLength[1] = (unsigned char) (packetLength); + // DATA FIELD HEADER + header->spare1_pusVersion_spare2 = 0x10; + header->serviceType = TM_TYPE_LFR_SCIENCE; // service type + header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype + header->destinationID = TM_DESTINATION_ID_GROUND; + // AUXILIARY DATA HEADER + header->sid = sid; + header->biaStatusInfo = 0x00; + header->time[0] = 0x00; + header->time[0] = 0x00; + header->time[0] = 0x00; + header->time[0] = 0x00; + header->time[0] = 0x00; + header->time[0] = 0x00; + header->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB + header->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB +} + +void BP_init_header_with_spare(Header_TM_LFR_SCIENCE_BP_with_spare_t *header, + unsigned int apid, unsigned char sid, + unsigned int packetLength , unsigned char blkNr) +{ + header->targetLogicalAddress = CCSDS_DESTINATION_ID; + header->protocolIdentifier = CCSDS_PROTOCOLE_ID; + header->reserved = 0x00; + header->userApplication = CCSDS_USER_APP; + header->packetID[0] = (unsigned char) (apid >> 8); + header->packetID[1] = (unsigned char) (apid); + header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; + header->packetSequenceControl[1] = 0x00; + header->packetLength[0] = (unsigned char) (packetLength >> 8); + header->packetLength[1] = (unsigned char) (packetLength); + // DATA FIELD HEADER + header->spare1_pusVersion_spare2 = 0x10; + header->serviceType = TM_TYPE_LFR_SCIENCE; // service type + header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype + header->destinationID = TM_DESTINATION_ID_GROUND; + // AUXILIARY DATA HEADER + header->sid = sid; + header->biaStatusInfo = 0x00; + header->time[0] = 0x00; + header->time[0] = 0x00; + header->time[0] = 0x00; + header->time[0] = 0x00; + header->time[0] = 0x00; + header->time[0] = 0x00; + header->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB + header->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB +} + +void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend ) +{ + rtems_status_code status; + + // SEND PACKET + status = rtems_message_queue_send( queue_id, data, nbBytesToSend); + if (status != RTEMS_SUCCESSFUL) + { + printf("ERR *** in BP_send *** ERR %d\n", (int) status); + } +} + +//****************** +// general functions + +void reset_spectral_matrix_regs( void ) +{ + /** This function resets the spectral matrices module registers. + * + * The registers affected by this function are located at the following offset addresses: + * + * - 0x00 config + * - 0x04 status + * - 0x08 matrixF0_Address0 + * - 0x10 matrixFO_Address1 + * - 0x14 matrixF1_Address + * - 0x18 matrixF2_Address + * + */ + + spectral_matrix_regs->config = 0x00; + spectral_matrix_regs->status = 0x00; + + spectral_matrix_regs->matrixF0_Address0 = current_ring_node_sm_f0->buffer_address; + spectral_matrix_regs->matrixFO_Address1 = current_ring_node_sm_f0->buffer_address; + spectral_matrix_regs->matrixF1_Address = current_ring_node_sm_f1->buffer_address; + spectral_matrix_regs->matrixF2_Address = current_ring_node_sm_f2->buffer_address; +} + +void set_time( unsigned char *time, unsigned char * timeInBuffer ) +{ +// time[0] = timeInBuffer[2]; +// time[1] = timeInBuffer[3]; +// time[2] = timeInBuffer[0]; +// time[3] = timeInBuffer[1]; +// time[4] = timeInBuffer[6]; +// time[5] = timeInBuffer[7]; + + time[0] = timeInBuffer[0]; + time[1] = timeInBuffer[1]; + time[2] = timeInBuffer[2]; + time[3] = timeInBuffer[3]; + time[4] = timeInBuffer[6]; + time[5] = timeInBuffer[7]; +} + + diff --git a/src/avf_prc/fsw_processing.h b/src/avf_prc/fsw_processing.h new file mode 100644 --- /dev/null +++ b/src/avf_prc/fsw_processing.h @@ -0,0 +1,108 @@ +#ifndef FSW_PROCESSING_H_INCLUDED +#define FSW_PROCESSING_H_INCLUDED + +#include +#include +#include +#include // abs() is in the stdlib +#include // printf() +#include + +#include "fsw_params.h" +#include "fsw_spacewire.h" + +typedef struct ring_node_sm +{ + struct ring_node_sm *previous; + struct ring_node_sm *next; + int buffer_address; + unsigned int status; + unsigned int coarseTime; + unsigned int fineTime; +} ring_node_sm; + +typedef struct ring_node_asm +{ + struct ring_node_asm *next; + float matrix[ TOTAL_SIZE_SM ]; + unsigned int status; +} ring_node_asm; + +typedef struct bp_packet +{ + Header_TM_LFR_SCIENCE_BP_t header; + unsigned char data[ 30 * 22 ]; // MAX size is 22 * 30 [TM_LFR_SCIENCE_BURST_BP2_F1] +} bp_packet; + +typedef struct bp_packet_with_spare +{ + Header_TM_LFR_SCIENCE_BP_with_spare_t header; + unsigned char data[ 9 * 13 ]; // only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1 +} bp_packet_with_spare; + +typedef struct asm_msg +{ + ring_node_asm *norm; + ring_node_asm *burst_sbm; + rtems_event_set event; + unsigned int coarseTime; + unsigned int fineTime; +} asm_msg; + +extern volatile int sm_f0[ ]; +extern volatile int sm_f1[ ]; +extern volatile int sm_f2[ ]; + +// parameters +extern struct param_local_str param_local; + +// registers +extern time_management_regs_t *time_management_regs; +extern spectral_matrix_regs_t *spectral_matrix_regs; + +extern rtems_name misc_name[5]; +extern rtems_id Task_id[20]; /* array of task ids */ + +// ISR +rtems_isr spectral_matrices_isr( rtems_vector_number vector ); +rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector ); + +//****************** +// Spectral Matrices +void reset_nb_sm( void ); +void SM_init_rings( void ); +void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes ); +void SM_reset_current_ring_nodes( void ); +void ASM_init_header( Header_TM_LFR_SCIENCE_ASM_t *header); +void SM_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_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); + +//***************** +// Basic Parameters + +void BP_reset_current_ring_nodes( void ); +void BP_init_header(Header_TM_LFR_SCIENCE_BP_t *header, + unsigned int apid, unsigned char sid, + unsigned int packetLength , unsigned char blkNr); +void BP_init_header_with_spare(Header_TM_LFR_SCIENCE_BP_with_spare_t *header, + unsigned int apid, unsigned char sid, + unsigned int packetLength, unsigned char blkNr ); +void BP_send(char *data, + rtems_id queue_id , + unsigned int nbBytesToSend ); + +//****************** +// general functions +void reset_spectral_matrix_regs( void ); +void set_time(unsigned char *time, unsigned char *timeInBuffer ); + +#endif // FSW_PROCESSING_H_INCLUDED diff --git a/src/fsw_globals.c b/src/fsw_globals.c --- a/src/fsw_globals.c +++ b/src/fsw_globals.c @@ -48,10 +48,6 @@ char wf_cont_f3_light[ (NB_SAMPL //*********************************** // SPECTRAL MATRICES GLOBAL VARIABLES -nb_sm_before_bp_asm_f0 nb_sm_before_f0; -nb_sm_before_bp_asm_f1 nb_sm_before_f1; -nb_sm_before_bp_asm_f2 nb_sm_before_f2; - // alignment constraints for the spectral matrices buffers => the first data after the time (8 bytes) shall be aligned on 0x00 volatile int sm_f0[ NB_RING_NODES_SM_F0 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))); volatile int sm_f1[ NB_RING_NODES_SM_F1 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))); diff --git a/src/fsw_init.c b/src/fsw_init.c --- a/src/fsw_init.c +++ b/src/fsw_init.c @@ -35,7 +35,7 @@ #define CONFIGURE_MAXIMUM_DRIVERS 16 #define CONFIGURE_MAXIMUM_PERIODS 5 #define CONFIGURE_MAXIMUM_TIMERS 5 // STAT (1s), send SWF (0.3s), send CWF3 (1s) -#define CONFIGURE_MAXIMUM_MESSAGE_QUEUES 4 +#define CONFIGURE_MAXIMUM_MESSAGE_QUEUES 5 #ifdef PRINT_STACK_REPORT #define CONFIGURE_STACK_CHECKER_ENABLED #endif @@ -98,7 +98,6 @@ rtems_task Init( rtems_task_argument ign init_waveform_rings(); // initialize the waveform rings SM_init_rings(); // initialize spectral matrices rings - ASM_init_rings(); // initialize the average spectral matrix ring (just for burst, sbm1 and sbm2 asm @ f0 storage) reset_wfp_burst_enable(); reset_wfp_status(); @@ -119,7 +118,7 @@ rtems_task Init( rtems_task_argument ign status = create_all_tasks(); // create all tasks if (status != RTEMS_SUCCESSFUL) { - PRINTF1("in INIT *** ERR in create_all_tasks, code %d", status) + PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status) } // ************************** @@ -263,6 +262,8 @@ void create_names( void ) // create all Task_name[TASKID_WTDG] = rtems_build_name( 'W', 'T', 'D', 'G' ); Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' ); Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' ); + Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' ); + Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' ); // rate monotonic period names name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' ); @@ -271,6 +272,7 @@ void create_names( void ) // create all misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' ); misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' ); misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' ); + misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' ); } int create_all_tasks( void ) // create all tasks which run in the software @@ -366,6 +368,22 @@ int create_all_tasks( void ) // create a RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1] ); } + if (status == RTEMS_SUCCESSFUL) // AVF2 + { + status = rtems_task_create( + Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE, + RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT, + RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2] + ); + } + if (status == RTEMS_SUCCESSFUL) // PRC2 + { + status = rtems_task_create( + Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * 2, + RTEMS_DEFAULT_MODES, + RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2] + ); + } //**************** // WAVEFORM PICKER @@ -529,6 +547,20 @@ int start_all_tasks( void ) // start all BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n") } } + if (status == RTEMS_SUCCESSFUL) // AVF2 + { + status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 ); + if (status!=RTEMS_SUCCESSFUL) { + BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n") + } + } + if (status == RTEMS_SUCCESSFUL) // PRC2 + { + status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 ); + if (status!=RTEMS_SUCCESSFUL) { + BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n") + } + } //**************** // WAVEFORM PICKER @@ -601,6 +633,7 @@ rtems_status_code create_message_queues( rtems_status_code status_send; rtems_status_code status_q_p0; rtems_status_code status_q_p1; + rtems_status_code status_q_p2; rtems_status_code ret; rtems_id queue_id; @@ -640,6 +673,15 @@ rtems_status_code create_message_queues( PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1) } + //***************************************************************************** + // create the queue for handling averaged spectral matrices for processing @ f2 + status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2], + MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2, + RTEMS_FIFO | RTEMS_LOCAL, &queue_id ); + if ( status_q_p2 != RTEMS_SUCCESSFUL ) { + PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2) + } + if ( status_recv != RTEMS_SUCCESSFUL ) { ret = status_recv; @@ -652,9 +694,13 @@ rtems_status_code create_message_queues( { ret = status_q_p0; } + else if( status_q_p1 != RTEMS_SUCCESSFUL ) + { + ret = status_q_p1; + } else { - ret = status_q_p1; + ret = status_q_p2; } return ret; @@ -707,3 +753,15 @@ rtems_status_code get_message_queue_id_p return status; } + +rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id ) +{ + rtems_status_code status; + rtems_name queue_name; + + queue_name = rtems_build_name( 'Q', '_', 'P', '2' ); + + status = rtems_message_queue_ident( queue_name, 0, queue_id ); + + return status; +} diff --git a/src/fsw_processing.c b/src/fsw_processing.c deleted file mode 100644 --- a/src/fsw_processing.c +++ /dev/null @@ -1,1273 +0,0 @@ -/** 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 - -#include "fsw_processing_globals.c" - -unsigned int nb_sm_f0; -unsigned int nb_sm_f0_aux; -unsigned int nb_sm_f1; - -//************************ -// spectral matrices rings -ring_node_sm sm_ring_f0[ NB_RING_NODES_SM_F0 ]; -ring_node_sm sm_ring_f1[ NB_RING_NODES_SM_F1 ]; -ring_node_sm sm_ring_f2[ NB_RING_NODES_SM_F2 ]; -ring_node_sm *current_ring_node_sm_f0; -ring_node_sm *current_ring_node_sm_f1; -ring_node_sm *current_ring_node_sm_f2; -ring_node_sm *ring_node_for_averaging_sm_f0; -ring_node_sm *ring_node_for_averaging_sm_f1; - -ring_node_asm asm_ring_norm_f0 [ NB_RING_NODES_ASM_NORM_F0 ]; -ring_node_asm asm_ring_burst_sbm_f0[ NB_RING_NODES_ASM_BURST_SBM_F0 ]; -ring_node_asm asm_ring_norm_f1 [ NB_RING_NODES_ASM_NORM_F1 ]; -ring_node_asm asm_ring_burst_sbm_f1[ NB_RING_NODES_ASM_BURST_SBM_F1 ]; -ring_node_asm *current_ring_node_asm_burst_sbm_f0; -ring_node_asm *current_ring_node_asm_norm_f0; -ring_node_asm *current_ring_node_asm_burst_sbm_f1; -ring_node_asm *current_ring_node_asm_norm_f1; - -float asm_f0_reorganized [ TOTAL_SIZE_SM ]; -char asm_f0_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ]; -float compressed_sm_norm_f0[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F0]; -float compressed_sm_sbm_f0 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F0 ]; - -float asm_f1_reorganized [ TOTAL_SIZE_SM ]; -char asm_f1_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ]; -float compressed_sm_norm_f1[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F1]; -float compressed_sm_sbm_f1 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 ]; - -//*********************************************************** -// Interrupt Service Routine for spectral matrices processing - -void reset_nb_sm_f0( unsigned char lfrMode ) -{ - nb_sm_f0 = 0; - nb_sm_f0_aux = 0; - - nb_sm_before_f0.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 96; - nb_sm_before_f0.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 96; - nb_sm_before_f0.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1]) * 96; - nb_sm_before_f0.sbm1_bp1 = parameter_dump_packet.sy_lfr_s1_bp_p0 * 24; - nb_sm_before_f0.sbm1_bp2 = parameter_dump_packet.sy_lfr_s1_bp_p1 * 96; - nb_sm_before_f0.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 96; - nb_sm_before_f0.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 96; - nb_sm_before_f0.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 96; - nb_sm_before_f0.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 96; - - if (lfrMode == LFR_MODE_SBM1) - { - nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm1_bp1; - nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm1_bp2; - } - else if (lfrMode == LFR_MODE_SBM2) - { - nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm2_bp1; - nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm2_bp2; - } - else if (lfrMode == LFR_MODE_BURST) - { - nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1; - nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2; - } - else - { - nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1; - nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2; - } -} - -void reset_nb_sm_f1( unsigned char lfrMode ) -{ - nb_sm_f1 = 0; - - nb_sm_before_f1.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 16; - nb_sm_before_f1.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 16; - 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]) * 16; - nb_sm_before_f1.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 16; - nb_sm_before_f1.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 16; - nb_sm_before_f1.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 16; - nb_sm_before_f1.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 16; - - 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 reset_nb_sm_f2( unsigned char lfrMode ) -{ - nb_sm_before_f2.norm_bp1_f2 = parameter_dump_packet.sy_lfr_n_bp_p0; - nb_sm_before_f2.norm_bp2_f2 = parameter_dump_packet.sy_lfr_n_bp_p1; - nb_sm_before_f2.norm_asm_f2 = parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1]; - nb_sm_before_f2.sbm2_bp1_f2 = parameter_dump_packet.sy_lfr_s2_bp_p0; - nb_sm_before_f2.sbm2_bp2_f2 = parameter_dump_packet.sy_lfr_s2_bp_p1; - nb_sm_before_f2.burst_bp1_f2 = parameter_dump_packet.sy_lfr_b_bp_p0; - nb_sm_before_f2.burst_bp2_f2 = parameter_dump_packet.sy_lfr_b_bp_p1; - - if (lfrMode == LFR_MODE_SBM2) - { - nb_sm_before_f2.burst_sbm_bp1_f2 = nb_sm_before_f2.sbm2_bp1_f2; - nb_sm_before_f2.burst_sbm_bp2_f2 = nb_sm_before_f2.sbm2_bp2_f2; - } - else if (lfrMode == LFR_MODE_BURST) - { - nb_sm_before_f2.burst_sbm_bp1_f2 = nb_sm_before_f2.burst_bp1_f2; - nb_sm_before_f2.burst_sbm_bp2_f2 = nb_sm_before_f2.burst_bp2_f2; - } - else - { - nb_sm_before_f2.burst_sbm_bp1_f2 = nb_sm_before_f2.burst_bp1_f2; - nb_sm_before_f2.burst_sbm_bp2_f2 = nb_sm_before_f2.burst_bp2_f2; - } -} - -rtems_isr spectral_matrices_isr( rtems_vector_number vector ) -{ - ring_node_sm *previous_ring_node_sm_f0; - -// rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 ); - - previous_ring_node_sm_f0 = current_ring_node_sm_f0; - - if ( (spectral_matrix_regs->status & 0x2) == 0x02) // check ready matrix bit f0_1 - { - current_ring_node_sm_f0 = current_ring_node_sm_f0->next; - spectral_matrix_regs->matrixF0_Address0 = current_ring_node_sm_f0->buffer_address; - spectral_matrix_regs->status = spectral_matrix_regs->status & 0xfffffffd; // 1101 - nb_sm_f0 = nb_sm_f0 + 1; - } - - //************************ - // reset status error bits - if ( (spectral_matrix_regs->status & 0x30) != 0x00) - { - rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 ); - spectral_matrix_regs->status = spectral_matrix_regs->status & 0xffffffcf; // 1100 1111 - } - - //************************************** - // reset ready matrix bits for f0_0, f1 and f2 - spectral_matrix_regs->status = spectral_matrix_regs->status & 0xfffffff2; // 0010 - - if (nb_sm_f0 == NB_SM_BEFORE_AVF0) - { - ring_node_for_averaging_sm_f0 = previous_ring_node_sm_f0; - if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) - { - rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); - } - nb_sm_f0 = 0; - } - -} - -rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector ) -{ - //*** - // F0 - nb_sm_f0 = nb_sm_f0 + 1; - if (nb_sm_f0 == NB_SM_BEFORE_AVF0 ) - { - ring_node_for_averaging_sm_f0 = current_ring_node_sm_f0; - if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) - { - rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); - } - nb_sm_f0 = 0; - } - - //*** - // F1 - nb_sm_f0_aux = nb_sm_f0_aux + 1; - if (nb_sm_f0_aux == 6) - { - nb_sm_f0_aux = 0; - nb_sm_f1 = nb_sm_f1 + 1; - } - if (nb_sm_f1 == NB_SM_BEFORE_AVF1 ) - { - ring_node_for_averaging_sm_f1 = current_ring_node_sm_f1; - if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) - { - rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); - } - nb_sm_f1 = 0; - } -} - -//************ -// RTEMS TASKS - -//***************** -// PROCESSING AT F0 - -rtems_task avf0_task( rtems_task_argument lfrRequestedMode ) -{ - int i; - - rtems_event_set event_out; - rtems_status_code status; - rtems_id queue_id_prc0; - asm_msg msgForMATR; - ring_node_sm *ring_node_tab[8]; - - 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; - - nb_norm_bp1 = 0; - nb_norm_bp2 = 0; - nb_norm_asm = 0; - nb_sbm_bp1 = 0; - nb_sbm_bp2 = 0; - - reset_nb_sm_f0( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions - - BOOT_PRINTF1("in AVFO *** lfrRequestedMode = %d\n", (int) lfrRequestedMode) - - status = get_message_queue_id_prc0( &queue_id_prc0 ); - if (status != RTEMS_SUCCESSFUL) - { - PRINTF1("in MATR *** ERR get_message_queue_id_prc0 %d\n", status) - } - - while(1){ - rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0 - ring_node_tab[NB_SM_BEFORE_AVF0-1] = ring_node_for_averaging_sm_f0; - for ( i = 2; i < (NB_SM_BEFORE_AVF0+1); i++ ) - { - ring_node_for_averaging_sm_f0 = ring_node_for_averaging_sm_f0->previous; - ring_node_tab[NB_SM_BEFORE_AVF0-i] = ring_node_for_averaging_sm_f0; - } - - // compute the average and store it in the averaged_sm_f1 buffer - SM_average( current_ring_node_asm_norm_f0->matrix, - current_ring_node_asm_burst_sbm_f0->matrix, - ring_node_tab, - nb_norm_bp1, nb_sbm_bp1 ); - - // update nb_average - nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0; - nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0; - nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0; - nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0; - nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0; - - //**************************************** - // initialize the mesage for the MATR task - msgForMATR.event = 0x00; // this composite event will be sent to the MATR task - msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f0; - msgForMATR.norm = current_ring_node_asm_norm_f0; -// msgForMATR.coarseTime = ( (unsigned int *) (ring_node_tab[0]->buffer_address) )[0]; -// msgForMATR.fineTime = ( (unsigned int *) (ring_node_tab[0]->buffer_address) )[1]; - msgForMATR.coarseTime = time_management_regs->coarse_time; - msgForMATR.fineTime = time_management_regs->fine_time; - - if (nb_sbm_bp1 == nb_sm_before_f0.burst_sbm_bp1) - { - nb_sbm_bp1 = 0; - // set another ring for the ASM storage - current_ring_node_asm_burst_sbm_f0 = current_ring_node_asm_burst_sbm_f0->next; - if ( (lfrCurrentMode == LFR_MODE_BURST) - || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) - { - msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_SBM_BP1_F0; - } - } - - if (nb_sbm_bp2 == nb_sm_before_f0.burst_sbm_bp2) - { - nb_sbm_bp2 = 0; - if ( (lfrCurrentMode == LFR_MODE_BURST) - || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) - { - msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_SBM_BP2_F0; - } - } - - if (nb_norm_bp1 == nb_sm_before_f0.norm_bp1) - { - nb_norm_bp1 = 0; - // set another ring for the ASM storage - current_ring_node_asm_norm_f0 = current_ring_node_asm_norm_f0->next; - if ( (lfrCurrentMode == LFR_MODE_NORMAL) - || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) - { - msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F0; - } - } - - if (nb_norm_bp2 == nb_sm_before_f0.norm_bp2) - { - nb_norm_bp2 = 0; - if ( (lfrCurrentMode == LFR_MODE_NORMAL) - || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) - { - msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F0; - } - } - - if (nb_norm_asm == nb_sm_before_f0.norm_asm) - { - nb_norm_asm = 0; - if ( (lfrCurrentMode == LFR_MODE_NORMAL) - || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) - { -// PRINTF1("%lld\n", localTime) - msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F0; - } - } - - //************************* - // send the message to MATR - if (msgForMATR.event != 0x00) - { - status = rtems_message_queue_send( queue_id_prc0, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC0); - } - - if (status != RTEMS_SUCCESSFUL) { - printf("in AVF0 *** Error sending message to MATR, code %d\n", status); - } - } -} - -rtems_task prc0_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; - // - spw_ioctl_pkt_send spw_ioctl_send_ASM; - rtems_status_code status; - rtems_id queue_id; - rtems_id queue_id_q_p0; - Header_TM_LFR_SCIENCE_ASM_t headerASM; - bp_packet_with_spare packet_norm_bp1_f0; - bp_packet packet_norm_bp2_f0; - bp_packet packet_sbm_bp1_f0; - bp_packet packet_sbm_bp2_f0; - - unsigned long long int localTime; - - ASM_init_header( &headerASM ); - - //************* - // NORM headers - BP_init_header_with_spare( &packet_norm_bp1_f0.header, - APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F0, - PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0, NB_BINS_COMPRESSED_SM_F0 ); - BP_init_header( &packet_norm_bp2_f0.header, - APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F0, - PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0, NB_BINS_COMPRESSED_SM_F0); - - //**************************** - // BURST SBM1 and SBM2 headers - if ( lfrRequestedMode == LFR_MODE_BURST ) - { - BP_init_header( &packet_sbm_bp1_f0.header, - APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F0, - PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0); - BP_init_header( &packet_sbm_bp2_f0.header, - APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F0, - PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0); - } - else if ( lfrRequestedMode == LFR_MODE_SBM1 ) - { - BP_init_header( &packet_sbm_bp1_f0.header, - APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP1_F0, - PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0); - BP_init_header( &packet_sbm_bp2_f0.header, - APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP2_F0, - PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0); - } - else if ( lfrRequestedMode == LFR_MODE_SBM2 ) - { - BP_init_header( &packet_sbm_bp1_f0.header, - APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F0, - PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0); - BP_init_header( &packet_sbm_bp2_f0.header, - APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F0, - PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0); - } - else - { - PRINTF1("in PRC0 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode) - } - - status = get_message_queue_id_send( &queue_id ); - if (status != RTEMS_SUCCESSFUL) - { - PRINTF1("in PRC0 *** ERR get_message_queue_id_send %d\n", status) - } - status = get_message_queue_id_prc0( &queue_id_q_p0); - if (status != RTEMS_SUCCESSFUL) - { - PRINTF1("in PRC0 *** ERR get_message_queue_id_prc0 %d\n", status) - } - - BOOT_PRINTF1("in PRC0 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode) - - while(1){ - status = rtems_message_queue_receive( queue_id_q_p0, incomingData, &size, //************************************ - RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0 - - incomingMsg = (asm_msg*) incomingData; - - localTime = getTimeAsUnsignedLongLongInt( ); - //**************** - //**************** - // BURST SBM1 SBM2 - //**************** - //**************** - if (incomingMsg->event & RTEMS_EVENT_BURST_SBM_BP1_F0 ) - { - // 1) compress the matrix for Basic Parameters calculation - ASM_compress_reorganize_and_divide( incomingMsg->burst_sbm->matrix, compressed_sm_sbm_f0, - nb_sm_before_f0.burst_sbm_bp1, - NB_BINS_COMPRESSED_SM_SBM_F0, NB_BINS_TO_AVERAGE_ASM_SBM_F0, - ASM_F0_INDICE_START); - // 2) compute the BP1 set - - // 3) send the BP1 set - set_time( packet_sbm_bp1_f0.header.time, (unsigned char *) &incomingMsg->coarseTime ); - set_time( packet_sbm_bp1_f0.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); - BP_send( (char *) &packet_sbm_bp1_f0.header, queue_id, - PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0 + PACKET_LENGTH_DELTA); - // 4) compute the BP2 set if needed - if ( incomingMsg->event & RTEMS_EVENT_BURST_SBM_BP2_F0 ) - { - // 1) compute the BP2 set - - // 2) send the BP2 set - set_time( packet_sbm_bp2_f0.header.time, (unsigned char *) &incomingMsg->coarseTime ); - set_time( packet_sbm_bp2_f0.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); - BP_send( (char *) &packet_sbm_bp2_f0.header, queue_id, - PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0 + PACKET_LENGTH_DELTA); - } - } - - //***** - //***** - // NORM - //***** - //***** - if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F0) - { - // 1) compress the matrix for Basic Parameters calculation - ASM_compress_reorganize_and_divide( incomingMsg->norm->matrix, compressed_sm_norm_f0, - nb_sm_before_f0.norm_bp1, - 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 - set_time( packet_norm_bp1_f0.header.time, (unsigned char *) &incomingMsg->coarseTime ); - set_time( packet_norm_bp1_f0.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); - BP_send( (char *) &packet_norm_bp1_f0.header, queue_id, - PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0 + PACKET_LENGTH_DELTA); - if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F0) - { - // 1) compute the BP2 set using the same ASM as the one used for BP1 - - // 2) send the BP2 set - set_time( packet_norm_bp2_f0.header.time, (unsigned char *) &incomingMsg->coarseTime ); - set_time( packet_norm_bp2_f0.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); - BP_send( (char *) &packet_norm_bp2_f0.header, queue_id, - PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0 + PACKET_LENGTH_DELTA); - } - } - - if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F0) - { - // 1) reorganize the ASM and divide - ASM_reorganize_and_divide( incomingMsg->norm->matrix, - asm_f0_reorganized, - nb_sm_before_f0.norm_bp1 ); - // 2) convert the float array in a char array - ASM_convert( asm_f0_reorganized, asm_f0_char); - // 3) send the spectral matrix packets - set_time( headerASM.time , (unsigned char *) &incomingMsg->coarseTime ); - set_time( headerASM.acquisitionTime, (unsigned char *) &incomingMsg->coarseTime ); - ASM_send( &headerASM, asm_f0_char, SID_NORM_ASM_F0, &spw_ioctl_send_ASM, queue_id); - } - - } -} - -//***************** -// PROCESSING AT F1 - -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 msgForMATR; - ring_node_sm *ring_node_tab[8]; - - 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; - - 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 - - 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 - ring_node_tab[NB_SM_BEFORE_AVF1-1] = ring_node_for_averaging_sm_f1; - for ( i = 2; i < (NB_SM_BEFORE_AVF1+1); i++ ) - { - ring_node_for_averaging_sm_f1 = ring_node_for_averaging_sm_f1->previous; - ring_node_tab[NB_SM_BEFORE_AVF1-i] = ring_node_for_averaging_sm_f1; - } - - // 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 ); - - // update nb_average - nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF1; - nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF1; - nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF1; - nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF1; - nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF1; - - //**************************************** - // initialize the mesage for the MATR task - msgForMATR.event = 0x00; // this composite event will be sent to the PRC1 task - msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f1; - msgForMATR.norm = current_ring_node_asm_norm_f1; -// msgForMATR.coarseTime = ( (unsigned int *) (ring_node_tab[0]->buffer_address) )[0]; -// msgForMATR.fineTime = ( (unsigned int *) (ring_node_tab[0]->buffer_address) )[1]; - msgForMATR.coarseTime = time_management_regs->coarse_time; - msgForMATR.fineTime = time_management_regs->fine_time; - - 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) || (lfrCurrentMode == LFR_MODE_SBM2) ) - { - msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_SBM_BP1_F1; - } - } - - if (nb_sbm_bp2 == nb_sm_before_f1.burst_sbm_bp2) - { - nb_sbm_bp2 = 0; - if ( (lfrCurrentMode == LFR_MODE_BURST) || (lfrCurrentMode == LFR_MODE_SBM2) ) - { - msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_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) ) - { - msgForMATR.event = msgForMATR.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) ) - { - msgForMATR.event = msgForMATR.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) ) - { - msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F1; - } - } - - //************************* - // send the message to MATR - if (msgForMATR.event != 0x00) - { - status = rtems_message_queue_send( queue_id_prc1, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC1); - } - - if (status != RTEMS_SUCCESSFUL) { - printf("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; - // - spw_ioctl_pkt_send spw_ioctl_send_ASM; - rtems_status_code status; - rtems_id queue_id_send; - rtems_id queue_id_q_p1; - Header_TM_LFR_SCIENCE_ASM_t headerASM; - bp_packet_with_spare packet_norm_bp1; - bp_packet packet_norm_bp2; - bp_packet packet_sbm_bp1; - bp_packet packet_sbm_bp2; - - unsigned long long int localTime; - - ASM_init_header( &headerASM ); - - //************* - // NORM headers - BP_init_header_with_spare( &packet_norm_bp1.header, - 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.header, - 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.header, - APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F1, - PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F1); - BP_init_header( &packet_sbm_bp2.header, - APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F1, - PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F1); - } - else if ( lfrRequestedMode == LFR_MODE_SBM2 ) - { - BP_init_header( &packet_sbm_bp1.header, - 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.header, - 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; - - localTime = getTimeAsUnsignedLongLongInt( ); - //*********** - //*********** - // BURST SBM2 - //*********** - //*********** - if (incomingMsg->event & RTEMS_EVENT_BURST_SBM_BP1_F1 ) - { - // 1) compress the matrix for Basic Parameters calculation - ASM_compress_reorganize_and_divide( incomingMsg->burst_sbm->matrix, compressed_sm_sbm_f1, - nb_sm_before_f1.burst_sbm_bp1, - NB_BINS_COMPRESSED_SM_SBM_F1, NB_BINS_TO_AVERAGE_ASM_SBM_F1, - ASM_F1_INDICE_START); - // 2) compute the BP1 set - - // 3) send the BP1 set - set_time( packet_sbm_bp1.header.time, (unsigned char *) &incomingMsg->coarseTime ); - set_time( packet_sbm_bp1.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); - BP_send( (char *) &packet_sbm_bp1.header, queue_id_send, - PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 + PACKET_LENGTH_DELTA); - // 4) compute the BP2 set if needed - if ( incomingMsg->event & RTEMS_EVENT_BURST_SBM_BP2_F1 ) - { - // 1) compute the BP2 set - - // 2) send the BP2 set - set_time( packet_sbm_bp2.header.time, (unsigned char *) &incomingMsg->coarseTime ); - set_time( packet_sbm_bp2.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); - BP_send( (char *) &packet_sbm_bp2.header, queue_id_send, - PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1 + PACKET_LENGTH_DELTA); - } - } - - //***** - //***** - // NORM - //***** - //***** - if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F1) - { - // 1) compress the matrix for Basic Parameters calculation - ASM_compress_reorganize_and_divide( incomingMsg->norm->matrix, compressed_sm_norm_f1, - nb_sm_before_f0.norm_bp1, - 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 - set_time( packet_norm_bp1.header.time, (unsigned char *) &incomingMsg->coarseTime ); - set_time( packet_norm_bp1.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); - BP_send( (char *) &packet_norm_bp1.header, queue_id_send, - PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 + PACKET_LENGTH_DELTA); - if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F1) - { - // 1) compute the BP2 set - - // 2) send the BP2 set - set_time( packet_norm_bp2.header.time, (unsigned char *) &incomingMsg->coarseTime ); - set_time( packet_norm_bp2.header.acquisitionTime, (unsigned char *) &incomingMsg->fineTime ); - BP_send( (char *) &packet_norm_bp2.header, queue_id_send, - PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 + PACKET_LENGTH_DELTA); - } - } - - if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F1) - { - // 1) reorganize the ASM and divide - ASM_reorganize_and_divide( incomingMsg->norm->matrix, - asm_f1_reorganized, - nb_sm_before_f0.norm_bp1 ); - // 2) convert the float array in a char array - ASM_convert( asm_f1_reorganized, asm_f1_char); - // 3) send the spectral matrix packets - set_time( headerASM.time , (unsigned char *) &incomingMsg->coarseTime ); - set_time( headerASM.acquisitionTime, (unsigned char *) &incomingMsg->coarseTime ); - ASM_send( &headerASM, asm_f1_char, SID_NORM_ASM_F1, &spw_ioctl_send_ASM, queue_id_send); - } - - } -} - -//****************** -// Spectral Matrices - -void SM_init_rings( void ) -{ - unsigned char i; - - // F0 RING - sm_ring_f0[0].next = (ring_node_sm*) &sm_ring_f0[1]; - sm_ring_f0[0].previous = (ring_node_sm*) &sm_ring_f0[NB_RING_NODES_SM_F0-1]; - sm_ring_f0[0].buffer_address = - (int) &sm_f0[ 0 ]; - - sm_ring_f0[NB_RING_NODES_SM_F0-1].next = (ring_node_sm*) &sm_ring_f0[0]; - sm_ring_f0[NB_RING_NODES_SM_F0-1].previous = (ring_node_sm*) &sm_ring_f0[NB_RING_NODES_SM_F0-2]; - sm_ring_f0[NB_RING_NODES_SM_F0-1].buffer_address = - (int) &sm_f0[ (NB_RING_NODES_SM_F0-1) * TOTAL_SIZE_SM ]; - - for(i=1; imatrixF0_Address0 = sm_ring_f0[0].buffer_address; - DEBUG_PRINTF1("spectral_matrix_regs->matrixF0_Address0 @%x\n", spectral_matrix_regs->matrixF0_Address0) -} - -void ASM_init_rings( void ) -{ - //*** - // F0 - // NORM - ASM_generic_init_ring( asm_ring_norm_f0, NB_RING_NODES_ASM_NORM_F0 ); - // BURST_SBM - ASM_generic_init_ring( asm_ring_burst_sbm_f0, NB_RING_NODES_ASM_BURST_SBM_F0 ); - - //*** - // F1 - //***** - // NORM - ASM_generic_init_ring( asm_ring_norm_f1, NB_RING_NODES_ASM_NORM_F1 ); - // BURST_SBM - ASM_generic_init_ring( asm_ring_burst_sbm_f1, NB_RING_NODES_ASM_BURST_SBM_F1 ); -} - -void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes ) -{ - unsigned char i; - - ring[ nbNodes - 1 ].next - = (ring_node_asm*) &ring[ 0 ]; - - for(i=0; itargetLogicalAddress = CCSDS_DESTINATION_ID; - header->protocolIdentifier = CCSDS_PROTOCOLE_ID; - header->reserved = 0x00; - header->userApplication = CCSDS_USER_APP; - header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8); - header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); - header->packetSequenceControl[0] = 0xc0; - header->packetSequenceControl[1] = 0x00; - header->packetLength[0] = 0x00; - header->packetLength[1] = 0x00; - // DATA FIELD HEADER - header->spare1_pusVersion_spare2 = 0x10; - header->serviceType = TM_TYPE_LFR_SCIENCE; // service type - header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype - header->destinationID = TM_DESTINATION_ID_GROUND; - // AUXILIARY DATA HEADER - header->sid = 0x00; - header->biaStatusInfo = 0x00; - header->pa_lfr_pkt_cnt_asm = 0x00; - header->pa_lfr_pkt_nr_asm = 0x00; - header->time[0] = 0x00; - header->time[0] = 0x00; - header->time[0] = 0x00; - header->time[0] = 0x00; - header->time[0] = 0x00; - header->time[0] = 0x00; - header->pa_lfr_asm_blk_nr[0] = 0x00; // BLK_NR MSB - header->pa_lfr_asm_blk_nr[1] = 0x00; // BLK_NR LSB -} - -void SM_average( float *averaged_spec_mat_f0, float *averaged_spec_mat_f1, - ring_node_sm *ring_node_tab[], - unsigned int nbAverageNormF0, unsigned int nbAverageSBM1F0 ) -{ - float sum; - unsigned int i; - - for(i=0; ibuffer_address) ) [ i ] - + ( (int *) (ring_node_tab[1]->buffer_address) ) [ i ] - + ( (int *) (ring_node_tab[2]->buffer_address) ) [ i ] - + ( (int *) (ring_node_tab[3]->buffer_address) ) [ i ] - + ( (int *) (ring_node_tab[4]->buffer_address) ) [ i ] - + ( (int *) (ring_node_tab[5]->buffer_address) ) [ i ] - + ( (int *) (ring_node_tab[6]->buffer_address) ) [ i ] - + ( (int *) (ring_node_tab[7]->buffer_address) ) [ i ]; - - if ( (nbAverageNormF0 == 0) && (nbAverageSBM1F0 == 0) ) - { - averaged_spec_mat_f0[ i ] = sum; - averaged_spec_mat_f1[ i ] = sum; - } - else if ( (nbAverageNormF0 != 0) && (nbAverageSBM1F0 != 0) ) - { - averaged_spec_mat_f0[ i ] = ( averaged_spec_mat_f0[ i ] + sum ); - averaged_spec_mat_f1[ i ] = ( averaged_spec_mat_f1[ i ] + sum ); - } - else if ( (nbAverageNormF0 != 0) && (nbAverageSBM1F0 == 0) ) - { - averaged_spec_mat_f0[ i ] = ( averaged_spec_mat_f0[ i ] + sum ); - averaged_spec_mat_f1[ i ] = sum; - } - else - { - PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNormF0, nbAverageSBM1F0) - } - } -} - -void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider ) -{ - int frequencyBin; - int asmComponent; - unsigned int offsetAveragedSpecMatReorganized; - unsigned int offsetAveragedSpecMat; - - for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++) - { - for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ ) - { - offsetAveragedSpecMatReorganized = - frequencyBin * NB_VALUES_PER_SM - + asmComponent; - offsetAveragedSpecMat = - asmComponent * NB_BINS_PER_SM - + frequencyBin; - averaged_spec_mat_reorganized[offsetAveragedSpecMatReorganized ] = - averaged_spec_mat[ offsetAveragedSpecMat ] / 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 ) -{ - 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 ] ) / (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; frequencyBindlen = TOTAL_SIZE_ASM_F0_IN_BYTES / 2; // 2 packets will be sent - spw_ioctl_send->data = &spectral_matrix[ - ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0) ) * NB_VALUES_PER_SM ) * 2 - ]; - length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0; - header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0) >> 8 ); // BLK_NR MSB - header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0); // BLK_NR LSB - break; - case SID_NORM_ASM_F1: - spw_ioctl_send->dlen = TOTAL_SIZE_ASM_F1_IN_BYTES / 2; // 2 packets will be sent - spw_ioctl_send->data = &spectral_matrix[ - ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1) ) * NB_VALUES_PER_SM ) * 2 - ]; - length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1; - header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1) >> 8 ); // BLK_NR MSB - header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1); // BLK_NR LSB - break; - case SID_NORM_ASM_F2: - break; - default: - PRINTF1("ERR *** in ASM_send *** unexpected sid %d\n", sid) - break; - } - spw_ioctl_send->hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM + CCSDS_PROTOCOLE_EXTRA_BYTES; - spw_ioctl_send->hdr = (char *) header; - spw_ioctl_send->options = 0; - - // (2) BUILD THE HEADER - header->packetLength[0] = (unsigned char) (length>>8); - header->packetLength[1] = (unsigned char) (length); - header->sid = (unsigned char) sid; // SID - header->pa_lfr_pkt_cnt_asm = 2; - header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1); - - // (3) SET PACKET TIME - header->time[0] = (unsigned char) (time_management_regs->coarse_time>>24); - header->time[1] = (unsigned char) (time_management_regs->coarse_time>>16); - header->time[2] = (unsigned char) (time_management_regs->coarse_time>>8); - header->time[3] = (unsigned char) (time_management_regs->coarse_time); - header->time[4] = (unsigned char) (time_management_regs->fine_time>>8); - header->time[5] = (unsigned char) (time_management_regs->fine_time); - // - header->acquisitionTime[0] = (unsigned char) (time_management_regs->coarse_time>>24); - header->acquisitionTime[1] = (unsigned char) (time_management_regs->coarse_time>>16); - header->acquisitionTime[2] = (unsigned char) (time_management_regs->coarse_time>>8); - header->acquisitionTime[3] = (unsigned char) (time_management_regs->coarse_time); - header->acquisitionTime[4] = (unsigned char) (time_management_regs->fine_time>>8); - header->acquisitionTime[5] = (unsigned char) (time_management_regs->fine_time); - - // (4) SEND PACKET - status = rtems_message_queue_send( queue_id, spw_ioctl_send, ACTION_MSG_SPW_IOCTL_SEND_SIZE); - if (status != RTEMS_SUCCESSFUL) { - printf("in ASM_send *** ERR %d\n", (int) status); - } - } -} - -//***************** -// Basic Parameters - -void BP_init_header( Header_TM_LFR_SCIENCE_BP_t *header, - unsigned int apid, unsigned char sid, - unsigned int packetLength, unsigned char blkNr ) -{ - header->targetLogicalAddress = CCSDS_DESTINATION_ID; - header->protocolIdentifier = CCSDS_PROTOCOLE_ID; - header->reserved = 0x00; - header->userApplication = CCSDS_USER_APP; - header->packetID[0] = (unsigned char) (apid >> 8); - header->packetID[1] = (unsigned char) (apid); - header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; - header->packetSequenceControl[1] = 0x00; - header->packetLength[0] = (unsigned char) (packetLength >> 8); - header->packetLength[1] = (unsigned char) (packetLength); - // DATA FIELD HEADER - header->spare1_pusVersion_spare2 = 0x10; - header->serviceType = TM_TYPE_LFR_SCIENCE; // service type - header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype - header->destinationID = TM_DESTINATION_ID_GROUND; - // AUXILIARY DATA HEADER - header->sid = sid; - header->biaStatusInfo = 0x00; - header->time[0] = 0x00; - header->time[0] = 0x00; - header->time[0] = 0x00; - header->time[0] = 0x00; - header->time[0] = 0x00; - header->time[0] = 0x00; - header->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB - header->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB -} - -void BP_init_header_with_spare(Header_TM_LFR_SCIENCE_BP_with_spare_t *header, - unsigned int apid, unsigned char sid, - unsigned int packetLength , unsigned char blkNr) -{ - header->targetLogicalAddress = CCSDS_DESTINATION_ID; - header->protocolIdentifier = CCSDS_PROTOCOLE_ID; - header->reserved = 0x00; - header->userApplication = CCSDS_USER_APP; - header->packetID[0] = (unsigned char) (apid >> 8); - header->packetID[1] = (unsigned char) (apid); - header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; - header->packetSequenceControl[1] = 0x00; - header->packetLength[0] = (unsigned char) (packetLength >> 8); - header->packetLength[1] = (unsigned char) (packetLength); - // DATA FIELD HEADER - header->spare1_pusVersion_spare2 = 0x10; - header->serviceType = TM_TYPE_LFR_SCIENCE; // service type - header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype - header->destinationID = TM_DESTINATION_ID_GROUND; - // AUXILIARY DATA HEADER - header->sid = sid; - header->biaStatusInfo = 0x00; - header->time[0] = 0x00; - header->time[0] = 0x00; - header->time[0] = 0x00; - header->time[0] = 0x00; - header->time[0] = 0x00; - header->time[0] = 0x00; - header->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB - header->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB -} - -void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend ) -{ - rtems_status_code status; - - // SEND PACKET - status = rtems_message_queue_send( queue_id, data, nbBytesToSend); - if (status != RTEMS_SUCCESSFUL) - { - printf("ERR *** in BP_send *** ERR %d\n", (int) status); - } -} - -//****************** -// general functions - -void reset_spectral_matrix_regs( void ) -{ - /** This function resets the spectral matrices module registers. - * - * The registers affected by this function are located at the following offset addresses: - * - * - 0x00 config - * - 0x04 status - * - 0x08 matrixF0_Address0 - * - 0x10 matrixFO_Address1 - * - 0x14 matrixF1_Address - * - 0x18 matrixF2_Address - * - */ - - spectral_matrix_regs->config = 0x00; - spectral_matrix_regs->status = 0x00; - - spectral_matrix_regs->matrixF0_Address0 = current_ring_node_sm_f0->buffer_address; - spectral_matrix_regs->matrixFO_Address1 = current_ring_node_sm_f0->buffer_address; - spectral_matrix_regs->matrixF1_Address = current_ring_node_sm_f1->buffer_address; - spectral_matrix_regs->matrixF2_Address = current_ring_node_sm_f2->buffer_address; -} - -void set_time( unsigned char *time, unsigned char * timeInBuffer ) -{ -// time[0] = timeInBuffer[2]; -// time[1] = timeInBuffer[3]; -// time[2] = timeInBuffer[0]; -// time[3] = timeInBuffer[1]; -// time[4] = timeInBuffer[6]; -// time[5] = timeInBuffer[7]; - - time[0] = timeInBuffer[0]; - time[1] = timeInBuffer[1]; - time[2] = timeInBuffer[2]; - time[3] = timeInBuffer[3]; - time[4] = timeInBuffer[6]; - time[5] = timeInBuffer[7]; -} - - diff --git a/src/tc_handler.c b/src/tc_handler.c --- a/src/tc_handler.c +++ b/src/tc_handler.c @@ -575,7 +575,7 @@ int restart_science_tasks(unsigned char * */ - rtems_status_code status[8]; + rtems_status_code status[10]; rtems_status_code ret; ret = RTEMS_SUCCESSFUL; @@ -628,10 +628,23 @@ int restart_science_tasks(unsigned char PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[7]) } + status[8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 ); + if (status[8] != RTEMS_SUCCESSFUL) + { + PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[8]) + } + + status[9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 ); + if (status[9] != RTEMS_SUCCESSFUL) + { + PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[9]) + } + if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) || (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) || (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) || - (status[6] != RTEMS_SUCCESSFUL) || (status[7] != RTEMS_SUCCESSFUL) ) + (status[6] != RTEMS_SUCCESSFUL) || (status[7] != RTEMS_SUCCESSFUL) || + (status[8] != RTEMS_SUCCESSFUL) || (status[9] != RTEMS_SUCCESSFUL)) { ret = RTEMS_UNSATISFIED; } @@ -681,6 +694,22 @@ int suspend_science_tasks() PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status) } } + if (status == RTEMS_SUCCESSFUL) // suspend AVF2 + { + status = rtems_task_suspend( Task_id[TASKID_AVF2] ); + if (status != RTEMS_SUCCESSFUL) + { + PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status) + } + } + if (status == RTEMS_SUCCESSFUL) // suspend PRC2 + { + status = rtems_task_suspend( Task_id[TASKID_PRC2] ); + if (status != RTEMS_SUCCESSFUL) + { + PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status) + } + } if (status == RTEMS_SUCCESSFUL) // suspend WFRM { status = rtems_task_suspend( Task_id[TASKID_WFRM] ); @@ -740,8 +769,8 @@ void launch_waveform_picker( unsigned ch void launch_spectral_matrix( void ) { SM_reset_current_ring_nodes(); - ASM_reset_current_ring_nodes(); reset_spectral_matrix_regs(); + reset_nb_sm(); struct grgpio_regs_str *grgpio_regs = (struct grgpio_regs_str *) REGS_ADDR_GRGPIO; grgpio_regs->io_port_direction_register = @@ -757,8 +786,8 @@ void launch_spectral_matrix( void ) void launch_spectral_matrix_simu( void ) { SM_reset_current_ring_nodes(); - ASM_reset_current_ring_nodes(); reset_spectral_matrix_regs(); + reset_nb_sm(); // Spectral Matrices simulator timer_start( (gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR );