##// END OF EJS Templates
HG_REPOSITORIES » LPP » INSTRUMENTATION » SOLO_LFR » DEV_PLE
RSS

Clone from http://pc-instru.lpp.polytechnique.fr:5000/DEV_PLE

Integration of basic parameters functions in the flight software...
paul -
r179:f0fdfd2b8c4c VHDL_0_1_28
parent child
Show More
Show file before | Show file after | Hide comments
@@ -0,0 +1,191
1 # This file was generated by an application wizard of Qt Creator.
2 # The code below handles deployment to Android and Maemo, aswell as copying
3 # of the application data to shadow build directories on desktop.
4 # It is recommended not to modify this file, since newer versions of Qt Creator
5 # may offer an updated version of it.
6
7 defineTest(qtcAddDeployment) {
8 for(deploymentfolder, DEPLOYMENTFOLDERS) {
9 item = item$${deploymentfolder}
10 greaterThan(QT_MAJOR_VERSION, 4) {
11 itemsources = $${item}.files
12 } else {
13 itemsources = $${item}.sources
14 }
15 $$itemsources = $$eval($${deploymentfolder}.source)
16 itempath = $${item}.path
17 $$itempath= $$eval($${deploymentfolder}.target)
18 export($$itemsources)
19 export($$itempath)
20 DEPLOYMENT += $$item
21 }
22
23 MAINPROFILEPWD = $$PWD
24
25 android-no-sdk {
26 for(deploymentfolder, DEPLOYMENTFOLDERS) {
27 item = item$${deploymentfolder}
28 itemfiles = $${item}.files
29 $$itemfiles = $$eval($${deploymentfolder}.source)
30 itempath = $${item}.path
31 $$itempath = /data/user/qt/$$eval($${deploymentfolder}.target)
32 export($$itemfiles)
33 export($$itempath)
34 INSTALLS += $$item
35 }
36
37 target.path = /data/user/qt
38
39 export(target.path)
40 INSTALLS += target
41 } else:android {
42 for(deploymentfolder, DEPLOYMENTFOLDERS) {
43 item = item$${deploymentfolder}
44 itemfiles = $${item}.files
45 $$itemfiles = $$eval($${deploymentfolder}.source)
46 itempath = $${item}.path
47 $$itempath = /assets/$$eval($${deploymentfolder}.target)
48 export($$itemfiles)
49 export($$itempath)
50 INSTALLS += $$item
51 }
52
53 x86 {
54 target.path = /libs/x86
55 } else: armeabi-v7a {
56 target.path = /libs/armeabi-v7a
57 } else {
58 target.path = /libs/armeabi
59 }
60
61 export(target.path)
62 INSTALLS += target
63 } else:win32 {
64 copyCommand =
65 for(deploymentfolder, DEPLOYMENTFOLDERS) {
66 source = $$MAINPROFILEPWD/$$eval($${deploymentfolder}.source)
67 source = $$replace(source, /, \\)
68 sourcePathSegments = $$split(source, \\)
69 target = $$OUT_PWD/$$eval($${deploymentfolder}.target)/$$last(sourcePathSegments)
70 target = $$replace(target, /, \\)
71 target ~= s,\\\\\\.?\\\\,\\,
72 !isEqual(source,$$target) {
73 !isEmpty(copyCommand):copyCommand += &&
74 isEqual(QMAKE_DIR_SEP, \\) {
75 copyCommand += $(COPY_DIR) \"$$source\" \"$$target\"
76 } else {
77 source = $$replace(source, \\\\, /)
78 target = $$OUT_PWD/$$eval($${deploymentfolder}.target)
79 target = $$replace(target, \\\\, /)
80 copyCommand += test -d \"$$target\" || mkdir -p \"$$target\" && cp -r \"$$source\" \"$$target\"
81 }
82 }
83 }
84 !isEmpty(copyCommand) {
85 copyCommand = @echo Copying application data... && $$copyCommand
86 copydeploymentfolders.commands = $$copyCommand
87 first.depends = $(first) copydeploymentfolders
88 export(first.depends)
89 export(copydeploymentfolders.commands)
90 QMAKE_EXTRA_TARGETS += first copydeploymentfolders
91 }
92 } else:ios {
93 copyCommand =
94 for(deploymentfolder, DEPLOYMENTFOLDERS) {
95 source = $$MAINPROFILEPWD/$$eval($${deploymentfolder}.source)
96 source = $$replace(source, \\\\, /)
97 target = $CODESIGNING_FOLDER_PATH/$$eval($${deploymentfolder}.target)
98 target = $$replace(target, \\\\, /)
99 sourcePathSegments = $$split(source, /)
100 targetFullPath = $$target/$$last(sourcePathSegments)
101 targetFullPath ~= s,/\\.?/,/,
102 !isEqual(source,$$targetFullPath) {
103 !isEmpty(copyCommand):copyCommand += &&
104 copyCommand += mkdir -p \"$$target\"
105 copyCommand += && cp -r \"$$source\" \"$$target\"
106 }
107 }
108 !isEmpty(copyCommand) {
109 copyCommand = echo Copying application data... && $$copyCommand
110 !isEmpty(QMAKE_POST_LINK): QMAKE_POST_LINK += ";"
111 QMAKE_POST_LINK += "$$copyCommand"
112 export(QMAKE_POST_LINK)
113 }
114 } else:unix {
115 maemo5 {
116 desktopfile.files = $${TARGET}.desktop
117 desktopfile.path = /usr/share/applications/hildon
118 icon.files = $${TARGET}64.png
119 icon.path = /usr/share/icons/hicolor/64x64/apps
120 } else:!isEmpty(MEEGO_VERSION_MAJOR) {
121 desktopfile.files = $${TARGET}_harmattan.desktop
122 desktopfile.path = /usr/share/applications
123 icon.files = $${TARGET}80.png
124 icon.path = /usr/share/icons/hicolor/80x80/apps
125 } else { # Assumed to be a Desktop Unix
126 copyCommand =
127 for(deploymentfolder, DEPLOYMENTFOLDERS) {
128 source = $$MAINPROFILEPWD/$$eval($${deploymentfolder}.source)
129 source = $$replace(source, \\\\, /)
130 macx {
131 target = $$OUT_PWD/$${TARGET}.app/Contents/Resources/$$eval($${deploymentfolder}.target)
132 } else {
133 target = $$OUT_PWD/$$eval($${deploymentfolder}.target)
134 }
135 target = $$replace(target, \\\\, /)
136 sourcePathSegments = $$split(source, /)
137 targetFullPath = $$target/$$last(sourcePathSegments)
138 targetFullPath ~= s,/\\.?/,/,
139 !isEqual(source,$$targetFullPath) {
140 !isEmpty(copyCommand):copyCommand += &&
141 copyCommand += $(MKDIR) \"$$target\"
142 copyCommand += && $(COPY_DIR) \"$$source\" \"$$target\"
143 }
144 }
145 !isEmpty(copyCommand) {
146 copyCommand = @echo Copying application data... && $$copyCommand
147 copydeploymentfolders.commands = $$copyCommand
148 first.depends = $(first) copydeploymentfolders
149 export(first.depends)
150 export(copydeploymentfolders.commands)
151 QMAKE_EXTRA_TARGETS += first copydeploymentfolders
152 }
153 }
154 !isEmpty(target.path) {
155 installPrefix = $${target.path}
156 } else {
157 installPrefix = /opt/$${TARGET}
158 }
159 for(deploymentfolder, DEPLOYMENTFOLDERS) {
160 item = item$${deploymentfolder}
161 itemfiles = $${item}.files
162 $$itemfiles = $$eval($${deploymentfolder}.source)
163 itempath = $${item}.path
164 $$itempath = $${installPrefix}/$$eval($${deploymentfolder}.target)
165 export($$itemfiles)
166 export($$itempath)
167 INSTALLS += $$item
168 }
169
170 !isEmpty(desktopfile.path) {
171 export(icon.files)
172 export(icon.path)
173 export(desktopfile.files)
174 export(desktopfile.path)
175 INSTALLS += icon desktopfile
176 }
177
178 isEmpty(target.path) {
179 target.path = $${installPrefix}/bin
180 export(target.path)
181 }
182 INSTALLS += target
183 }
184
185 export (ICON)
186 export (INSTALLS)
187 export (DEPLOYMENT)
188 export (LIBS)
189 export (QMAKE_EXTRA_TARGETS)
190 }
191
Show file before | Show file after | Hide comments
@@ -0,0 +1,13
1 TEMPLATE = app
2 CONFIG += console
3 CONFIG -= app_bundle
4 CONFIG -= qt
5
6 SOURCES += main.c
7
8 include(deployment.pri)
9 qtcAddDeployment()
10
11 HEADERS += \
12 functions.h
13
Show file before | Show file after | Hide comments
@@ -0,0 +1,65
1 #define NB_VALUES_PER_SM 25
2 #define NB_BINS_PER_SM 128
3
4 #define NB_BINS_COMPRESSED_SM_F0 11
5 #define ASM_F0_INDICE_START 17 // 88 bins
6 #define ASM_F0_INDICE_STOP 104 // 2 packets of 44 bins
7 #define NB_BINS_TO_AVERAGE_ASM_F0 8
8
9 void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
10 {
11 int frequencyBin;
12 int asmComponent;
13 unsigned int offsetASM;
14 unsigned int offsetASMReorganized;
15
16 // BUILD DATA
17 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
18 {
19 for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
20 {
21 offsetASMReorganized =
22 frequencyBin * NB_VALUES_PER_SM
23 + asmComponent;
24 offsetASM =
25 asmComponent * NB_BINS_PER_SM
26 + frequencyBin;
27 averaged_spec_mat_reorganized[offsetASMReorganized ] =
28 averaged_spec_mat[ offsetASM ] / divider;
29 }
30 }
31 }
32
33 void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
34 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
35 {
36 int frequencyBin;
37 int asmComponent;
38 int offsetASM;
39 int offsetCompressed;
40 int k;
41
42 // BUILD DATA
43 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
44 {
45 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
46 {
47 offsetCompressed = // NO TIME OFFSET
48 frequencyBin * NB_VALUES_PER_SM
49 + asmComponent;
50 offsetASM = // NO TIME OFFSET
51 asmComponent * NB_BINS_PER_SM
52 + ASMIndexStart
53 + frequencyBin * nbBinsToAverage;
54 compressed_spec_mat[ offsetCompressed ] = 0;
55 for ( k = 0; k < nbBinsToAverage; k++ )
56 {
57 compressed_spec_mat[offsetCompressed ] =
58 ( compressed_spec_mat[ offsetCompressed ]
59 + averaged_spec_mat[ offsetASM + k ] );
60 }
61 compressed_spec_mat[ offsetCompressed ] =
62 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
63 }
64 }
65 }
Show file before | Show file after | Hide comments
@@ -0,0 +1,64
1 #include <stdio.h>
2
3 #include "functions.h"
4
5 int main(void)
6 {
7 printf("Hello World!\n");
8
9 unsigned int asmComponent;
10 unsigned int frequencyBin;
11 unsigned int offset_input_ASM;
12
13 float input_ASM [ NB_VALUES_PER_SM * NB_BINS_PER_SM ];
14 float output_ASM [ NB_VALUES_PER_SM * NB_BINS_PER_SM ];
15 float output_ASM_compressed [ NB_VALUES_PER_SM * NB_BINS_COMPRESSED_SM_F0 ];
16
17 //*******
18 // TEST 1
19
20 offset_input_ASM = 0;
21
22 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
23 {
24 for (frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++)
25 {
26 offset_input_ASM = asmComponent * NB_BINS_PER_SM + frequencyBin;
27 input_ASM[ offset_input_ASM ] = asmComponent;
28 }
29 }
30
31 ASM_reorganize_and_divide( input_ASM, output_ASM,
32 1 ); // divider
33
34 ASM_compress_reorganize_and_divide( input_ASM, output_ASM_compressed,
35 1, // divider
36 NB_BINS_COMPRESSED_SM_F0,
37 NB_BINS_TO_AVERAGE_ASM_F0,
38 ASM_F0_INDICE_START);
39
40 //*******
41 // TEST 2
42 offset_input_ASM = 0;
43
44 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
45 {
46 for (frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++)
47 {
48 offset_input_ASM = asmComponent * NB_BINS_PER_SM + frequencyBin;
49 input_ASM[ offset_input_ASM ] = asmComponent * NB_BINS_PER_SM + frequencyBin;
50 }
51 }
52
53 ASM_reorganize_and_divide( input_ASM, output_ASM,
54 1 ); // divider
55
56 ASM_compress_reorganize_and_divide( input_ASM, output_ASM_compressed,
57 10, // divider
58 NB_BINS_COMPRESSED_SM_F0,
59 NB_BINS_TO_AVERAGE_ASM_F0,
60 ASM_F0_INDICE_START);
61
62 return 0;
63 }
64
Show file before | Show file after | Hide comments
@@ -1,2 +1,2
1 19349b3a5e90c2bacc9d369aa948c68aa9e8d5f0 LFR_basic-parameters
2 da5613aff4446e5c98b3c56bc32ce7008b3e2340 header/lfr_common_headers
1 a309a930a482e851061936696121f4a1cf7005de LFR_basic-parameters
2 2b5dc338fb623046072d6eb98c26ad884e17f95e header/lfr_common_headers
Show file before | Show file after | Hide comments
@@ -1,110 +1,111
1 1 TEMPLATE = app
2 2 # CONFIG += console v8 sim
3 3 # CONFIG options = verbose *** boot_messages *** debug_messages *** cpu_usage_report *** stack_report *** vhdl_dev *** debug_tch
4 4 # lpp_dpu_destid
5 5 CONFIG += console verbose lpp_dpu_destid
6 6 CONFIG -= qt
7 7
8 8 include(./sparc.pri)
9 9
10 10 # flight software version
11 11 SWVERSION=-1-0
12 12 DEFINES += SW_VERSION_N1=2 # major
13 13 DEFINES += SW_VERSION_N2=0 # minor
14 14 DEFINES += SW_VERSION_N3=2 # patch
15 DEFINES += SW_VERSION_N4=0 # internal
15 DEFINES += SW_VERSION_N4=1 # internal
16 16
17 17 # <GCOV>
18 18 #QMAKE_CFLAGS_RELEASE += -fprofile-arcs -ftest-coverage
19 19 #LIBS += -lgcov /opt/GCOV/01A/lib/overload.o -lc
20 20 # </GCOV>
21 21
22 22 # <CHANGE BEFORE FLIGHT>
23 23 contains( CONFIG, lpp_dpu_destid ) {
24 24 DEFINES += LPP_DPU_DESTID
25 25 }
26 26 # </CHANGE BEFORE FLIGHT>
27 27
28 28 contains( CONFIG, debug_tch ) {
29 29 DEFINES += DEBUG_TCH
30 30 }
31 DEFINES += LSB_FIRST_TCH
31 32
32 33 contains( CONFIG, vhdl_dev ) {
33 34 DEFINES += VHDL_DEV
34 35 }
35 36
36 37 contains( CONFIG, verbose ) {
37 38 DEFINES += PRINT_MESSAGES_ON_CONSOLE
38 39 }
39 40
40 41 contains( CONFIG, debug_messages ) {
41 42 DEFINES += DEBUG_MESSAGES
42 43 }
43 44
44 45 contains( CONFIG, cpu_usage_report ) {
45 46 DEFINES += PRINT_TASK_STATISTICS
46 47 }
47 48
48 49 contains( CONFIG, stack_report ) {
49 50 DEFINES += PRINT_STACK_REPORT
50 51 }
51 52
52 53 contains( CONFIG, boot_messages ) {
53 54 DEFINES += BOOT_MESSAGES
54 55 }
55 56
56 57 #doxygen.target = doxygen
57 58 #doxygen.commands = doxygen ../doc/Doxyfile
58 59 #QMAKE_EXTRA_TARGETS += doxygen
59 60
60 61 TARGET = fsw
61 62
62 63 INCLUDEPATH += \
63 64 $${PWD}/../src \
64 65 $${PWD}/../header \
65 66 $${PWD}/../header/lfr_common_headers \
66 67 $${PWD}/../header/processing \
67 $${PWD}/../src/LFR_basic-parameters
68 $${PWD}/../LFR_basic-parameters
68 69
69 70 SOURCES += \
70 71 ../src/wf_handler.c \
71 72 ../src/tc_handler.c \
72 73 ../src/fsw_misc.c \
73 74 ../src/fsw_init.c \
74 75 ../src/fsw_globals.c \
75 76 ../src/fsw_spacewire.c \
76 77 ../src/tc_load_dump_parameters.c \
77 78 ../src/tm_lfr_tc_exe.c \
78 79 ../src/tc_acceptance.c \
79 80 ../src/processing/fsw_processing.c \
80 81 ../src/processing/avf0_prc0.c \
81 82 ../src/processing/avf1_prc1.c \
82 83 ../src/processing/avf2_prc2.c \
83 84 ../src/lfr_cpu_usage_report.c \
84 ../src/LFR_basic-parameters/basic_parameters.c
85 ../LFR_basic-parameters/basic_parameters.c
85 86
86 87 HEADERS += \
87 88 ../header/wf_handler.h \
88 89 ../header/tc_handler.h \
89 90 ../header/grlib_regs.h \
90 91 ../header/fsw_misc.h \
91 92 ../header/fsw_init.h \
92 93 ../header/fsw_spacewire.h \
93 94 ../header/tc_load_dump_parameters.h \
94 95 ../header/tm_lfr_tc_exe.h \
95 96 ../header/tc_acceptance.h \
96 97 ../header/processing/fsw_processing.h \
97 98 ../header/processing/avf0_prc0.h \
98 99 ../header/processing/avf1_prc1.h \
99 100 ../header/processing/avf2_prc2.h \
100 101 ../header/fsw_params_wf_handler.h \
101 102 ../header/lfr_cpu_usage_report.h \
102 ../src/LFR_basic-parameters/basic_parameters.h \
103 ../src/LFR_basic-parameters/basic_parameters_params.h \
104 103 ../header/lfr_common_headers/ccsds_types.h \
105 104 ../header/lfr_common_headers/fsw_params.h \
106 105 ../header/lfr_common_headers/fsw_params_nb_bytes.h \
107 106 ../header/lfr_common_headers/fsw_params_processing.h \
108 107 ../header/lfr_common_headers/TC_types.h \
109 ../header/lfr_common_headers/tm_byte_positions.h
108 ../header/lfr_common_headers/tm_byte_positions.h \
109 ../LFR_basic-parameters/basic_parameters.h \
110 ../LFR_basic-parameters/basic_parameters_params.h
110 111
Show file before | Show file after | Hide comments
@@ -1,124 +1,128
1 1 #ifndef GRLIB_REGS_H_INCLUDED
2 2 #define GRLIB_REGS_H_INCLUDED
3 3
4 4 #define NB_GPTIMER 3
5 5
6 6 struct apbuart_regs_str{
7 7 volatile unsigned int data;
8 8 volatile unsigned int status;
9 9 volatile unsigned int ctrl;
10 10 volatile unsigned int scaler;
11 11 volatile unsigned int fifoDebug;
12 12 };
13 13
14 14 struct grgpio_regs_str{
15 15 volatile int io_port_data_register;
16 16 int io_port_output_register;
17 17 int io_port_direction_register;
18 18 int interrupt_mak_register;
19 19 int interrupt_polarity_register;
20 20 int interrupt_edge_register;
21 21 int bypass_register;
22 22 int reserved;
23 23 // 0x20-0x3c interrupt map register(s)
24 24 };
25 25
26 26 typedef struct {
27 27 volatile unsigned int counter;
28 28 volatile unsigned int reload;
29 29 volatile unsigned int ctrl;
30 30 volatile unsigned int unused;
31 31 } timer_regs_t;
32 32
33 33 typedef struct {
34 34 volatile unsigned int scaler_value;
35 35 volatile unsigned int scaler_reload;
36 36 volatile unsigned int conf;
37 37 volatile unsigned int unused0;
38 38 timer_regs_t timer[NB_GPTIMER];
39 39 } gptimer_regs_t;
40 40
41 41 typedef struct {
42 42 volatile int ctrl; // bit 0 forces the load of the coarse_time_load value and resets the fine_time
43 43 // bit 1 is the soft reset for the time management module
44 44 // bit 2 is the soft reset for the waveform picker and the spectral matrix modules, set to 1 after HW reset
45 45 volatile int coarse_time_load;
46 46 volatile int coarse_time;
47 47 volatile int fine_time;
48 48 } time_management_regs_t;
49 49
50 50 // PDB >= 0.1.28
51 51 typedef struct{
52 52 int data_shaping; // 0x00 00 *** R1 R0 SP1 SP0 BW
53 53 int run_burst_enable; // 0x04 01 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
54 54 int addr_data_f0_0; // 0x08
55 55 int addr_data_f0_1; // 0x0c
56 56 int addr_data_f1_0; // 0x10
57 57 int addr_data_f1_1; // 0x14
58 58 int addr_data_f2_0; // 0x18
59 59 int addr_data_f2_1; // 0x1c
60 60 int addr_data_f3_0; // 0x20
61 61 int addr_data_f3_1; // 0x24
62 62 volatile int status; // 0x28
63 63 int delta_snapshot; // 0x2c
64 64 int delta_f0; // 0x30
65 65 int delta_f0_2; // 0x34
66 66 int delta_f1; // 0x38
67 67 int delta_f2; // 0x3c
68 68 int nb_data_by_buffer; // 0x40 number of samples in a buffer = 2688
69 69 int snapshot_param; // 0x44
70 70 int start_date; // 0x48
71 71 //
72 72 volatile unsigned int f0_0_coarse_time; // 0x4c
73 73 volatile unsigned int f0_0_fine_time; // 0x50
74 74 volatile unsigned int f0_1_coarse_time; // 0x54
75 75 volatile unsigned int f0_1_fine_time; // 0x58
76 76 //
77 77 volatile unsigned int f1_0_coarse_time; // 0x5c
78 78 volatile unsigned int f1_0_fine_time; // 0x60
79 79 volatile unsigned int f1_1_coarse_time; // 0x64
80 80 volatile unsigned int f1_1_fine_time; // 0x68
81 81 //
82 82 volatile unsigned int f2_0_coarse_time; // 0x6c
83 83 volatile unsigned int f2_0_fine_time; // 0x70
84 84 volatile unsigned int f2_1_coarse_time; // 0x74
85 85 volatile unsigned int f2_1_fine_time; // 0x78
86 86 //
87 87 volatile unsigned int f3_0_coarse_time; // 0x7c
88 88 volatile unsigned int f3_0_fine_time; // 0x80
89 89 volatile unsigned int f3_1_coarse_time; // 0x84
90 90 volatile unsigned int f3_1_fine_time; // 0x88
91 91 //
92 92 unsigned int buffer_length; // 0x8c = buffer length in burst 2688 / 16 = 168
93 //
94 volatile unsigned int v; // 0x90
95 volatile unsigned int e1; // 0x94
96 volatile unsigned int e2; // 0x98
93 97 } waveform_picker_regs_0_1_18_t;
94 98
95 99 typedef struct {
96 100 volatile int config; // 0x00
97 101 volatile int status; // 0x04
98 102 volatile int f0_0_address; // 0x08
99 103 volatile int f0_1_address; // 0x0C
100 104 //
101 105 volatile int f1_0_address; // 0x10
102 106 volatile int f1_1_address; // 0x14
103 107 volatile int f2_0_address; // 0x18
104 108 volatile int f2_1_address; // 0x1C
105 109 //
106 110 volatile unsigned int f0_0_coarse_time; // 0x20
107 111 volatile unsigned int f0_0_fine_time; // 0x24
108 112 volatile unsigned int f0_1_coarse_time; // 0x28
109 113 volatile unsigned int f0_1_fine_time; // 0x2C
110 114 //
111 115 volatile unsigned int f1_0_coarse_time; // 0x30
112 116 volatile unsigned int f1_0_fine_time; // 0x34
113 117 volatile unsigned int f1_1_coarse_time; // 0x38
114 volatile unsigned int f1_1_time_time; // 0x3C
118 volatile unsigned int f1_1_fine_time; // 0x3C
115 119 //
116 120 volatile unsigned int f2_0_coarse_time; // 0x40
117 121 volatile unsigned int f2_0_fine_time; // 0x44
118 122 volatile unsigned int f2_1_coarse_time; // 0x48
119 123 volatile unsigned int f2_1_fine_time; // 0x4C
120 124 //
121 125 unsigned int matrix_length; // 0x50, length of a spectral matrix in burst 3200 / 16 = 200 = 0xc8
122 126 } spectral_matrix_regs_t;
123 127
124 128 #endif // GRLIB_REGS_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,36 +1,37
1 1 #ifndef AVF0_PRC0_H_INCLUDED
2 2 #define AVF0_PRC0_H_INCLUDED
3 3
4 4 #include "fsw_processing.h"
5 5 #include "basic_parameters.h"
6 6
7 7 typedef struct {
8 8 unsigned int norm_bp1;
9 9 unsigned int norm_bp2;
10 10 unsigned int norm_asm;
11 11 unsigned int burst_sbm_bp1;
12 12 unsigned int burst_sbm_bp2;
13 13 unsigned int burst_bp1;
14 14 unsigned int burst_bp2;
15 15 unsigned int sbm1_bp1;
16 16 unsigned int sbm1_bp2;
17 17 unsigned int sbm2_bp1;
18 18 unsigned int sbm2_bp2;
19 19 } nb_sm_before_bp_asm_f0;
20 20
21 21 //************
22 22 // RTEMS TASKS
23 23 rtems_task avf0_task( rtems_task_argument lfrRequestedMode );
24 24 rtems_task prc0_task( rtems_task_argument lfrRequestedMode );
25 25
26 26 //**********
27 27 // FUNCTIONS
28 28
29 29 void reset_nb_sm_f0( unsigned char lfrMode );
30 void init_k_coefficients_f0( void );
31 void test_TCH( void );
30 32
31 33 //*******
32 34 // EXTERN
33 extern ring_node *ring_node_for_averaging_sm_f0;
34 35 extern rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id );
35 36
36 37 #endif // AVF0_PRC0_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,33 +1,34
1 1 #ifndef AVF1_PRC1_H
2 2 #define AVF1_PRC1_H
3 3
4 4 #include "fsw_processing.h"
5 #include "basic_parameters.h"
5 6
6 7 typedef struct {
7 8 unsigned int norm_bp1;
8 9 unsigned int norm_bp2;
9 10 unsigned int norm_asm;
10 11 unsigned int burst_sbm_bp1;
11 12 unsigned int burst_sbm_bp2;
12 13 unsigned int burst_bp1;
13 14 unsigned int burst_bp2;
14 15 unsigned int sbm2_bp1;
15 16 unsigned int sbm2_bp2;
16 17 } nb_sm_before_bp_asm_f1;
17 18
18 19 //************
19 20 // RTEMS TASKS
20 21 rtems_task avf1_task( rtems_task_argument lfrRequestedMode );
21 22 rtems_task prc1_task( rtems_task_argument lfrRequestedMode );
22 23
23 24 //**********
24 25 // FUNCTIONS
25 26
26 27 void reset_nb_sm_f1( unsigned char lfrMode );
28 void init_k_coefficients_f1( void );
27 29
28 30 //*******
29 31 // EXTERN
30 extern struct ring_node *ring_node_for_averaging_sm_f1;
31 32 extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
32 33
33 34 #endif // AVF1_PRC1_H
Show file before | Show file after | Hide comments
@@ -1,28 +1,29
1 1 #ifndef AVF2_PRC2_H
2 2 #define AVF2_PRC2_H
3 3
4 4 #include "fsw_processing.h"
5 #include "basic_parameters.h"
5 6
6 7 typedef struct {
7 8 unsigned int norm_bp1;
8 9 unsigned int norm_bp2;
9 10 unsigned int norm_asm;
10 11 } nb_sm_before_bp_asm_f2;
11 12
12 13 //************
13 14 // RTEMS TASKS
14 15 rtems_task avf2_task( rtems_task_argument lfrRequestedMode );
15 16 rtems_task prc2_task( rtems_task_argument lfrRequestedMode );
16 17
17 18 //**********
18 19 // FUNCTIONS
19 20
20 21 void reset_nb_sm_f2( void );
21 void SM_average_f2(float *averaged_spec_mat_f2, ring_node *ring_node, unsigned int nbAverageNormF2 );
22 void SM_average_f2(float *averaged_spec_mat_f2, ring_node *ring_node, unsigned int nbAverageNormF2 , asm_msg *msgForMATR);
23 void init_k_coefficients_f2( void );
22 24
23 25 //*******
24 26 // EXTERN
25 extern struct ring_node *ring_node_for_averaging_sm_f2;
26 27 extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
27 28
28 29 #endif // AVF2_PRC2_H
Show file before | Show file after | Hide comments
@@ -1,267 +1,298
1 1 #ifndef FSW_PROCESSING_H_INCLUDED
2 2 #define FSW_PROCESSING_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <grspw.h>
6 6 #include <math.h>
7 7 #include <stdlib.h> // abs() is in the stdlib
8 8 #include <stdio.h> // printf()
9 9 #include <math.h>
10 10 #include <grlib_regs.h>
11 11
12 12 #include "fsw_params.h"
13 13 #include "fsw_spacewire.h"
14 14
15 15 typedef struct ring_node_asm
16 16 {
17 17 struct ring_node_asm *next;
18 18 float matrix[ TOTAL_SIZE_SM ];
19 19 unsigned int status;
20 20 } ring_node_asm;
21 21
22 22 typedef struct
23 23 {
24 Header_TM_LFR_SCIENCE_BP_t header;
24 unsigned char targetLogicalAddress;
25 unsigned char protocolIdentifier;
26 unsigned char reserved;
27 unsigned char userApplication;
28 unsigned char packetID[2];
29 unsigned char packetSequenceControl[2];
30 unsigned char packetLength[2];
31 // DATA FIELD HEADER
32 unsigned char spare1_pusVersion_spare2;
33 unsigned char serviceType;
34 unsigned char serviceSubType;
35 unsigned char destinationID;
36 unsigned char time[6];
37 // AUXILIARY HEADER
38 unsigned char sid;
39 unsigned char biaStatusInfo;
40 unsigned char acquisitionTime[6];
41 unsigned char pa_lfr_bp_blk_nr[2];
42 // SOURCE DATA
25 43 unsigned char data[ 30 * 22 ]; // MAX size is 22 * 30 [TM_LFR_SCIENCE_BURST_BP2_F1]
26 44 } bp_packet;
27 45
28 46 typedef struct
29 47 {
30 48 Header_TM_LFR_SCIENCE_BP_with_spare_t header;
31 49 unsigned char data[ 9 * 13 ]; // only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1
32 50 } bp_packet_with_spare;
33 51
34 52 typedef struct
35 53 {
36 54 ring_node_asm *norm;
37 55 ring_node_asm *burst_sbm;
38 56 rtems_event_set event;
39 unsigned int coarseTime;
40 unsigned int fineTime;
57 unsigned int coarseTimeNORM;
58 unsigned int fineTimeNORM;
59 unsigned int coarseTimeSBM;
60 unsigned int fineTimeSBM;
41 61 } asm_msg;
42 62
43 63 extern volatile int sm_f0[ ];
44 64 extern volatile int sm_f1[ ];
45 65 extern volatile int sm_f2[ ];
46 66
47 67 // parameters
48 68 extern struct param_local_str param_local;
49 69
50 70 // registers
51 71 extern time_management_regs_t *time_management_regs;
52 extern spectral_matrix_regs_t *spectral_matrix_regs;
72 extern volatile spectral_matrix_regs_t *spectral_matrix_regs;
53 73
54 74 extern rtems_name misc_name[5];
55 75 extern rtems_id Task_id[20]; /* array of task ids */
56 76
77 //
78 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel);
57 79 // ISR
58 80 rtems_isr spectral_matrices_isr( rtems_vector_number vector );
59 81 rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector );
60 82
61 83 //******************
62 84 // Spectral Matrices
63 85 void reset_nb_sm( void );
64 86 // SM
65 87 void SM_init_rings( void );
66 88 void SM_reset_current_ring_nodes( void );
67 89 // ASM
68 90 void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes );
69 91
70 92 //*****************
71 93 // Basic Parameters
72 94
73 95 void BP_reset_current_ring_nodes( void );
74 void BP_init_header( Header_TM_LFR_SCIENCE_BP_t *header,
96 void BP_init_header(bp_packet *header,
75 97 unsigned int apid, unsigned char sid,
76 98 unsigned int packetLength , unsigned char blkNr);
77 99 void BP_init_header_with_spare( Header_TM_LFR_SCIENCE_BP_with_spare_t *header,
78 100 unsigned int apid, unsigned char sid,
79 101 unsigned int packetLength, unsigned char blkNr );
80 102 void BP_send( char *data,
81 103 rtems_id queue_id ,
82 104 unsigned int nbBytesToSend , unsigned int sid );
83 105
84 106 //******************
85 107 // general functions
86 108 void reset_sm_status( void );
87 109 void reset_spectral_matrix_regs( void );
88 110 void set_time(unsigned char *time, unsigned char *timeInBuffer );
89 111 unsigned long long int get_acquisition_time( unsigned char *timePtr );
90 void close_matrix_actions( unsigned int *nb_sm, unsigned int nb_sm_before_avf, rtems_id avf_task_id,
91 ring_node *node_for_averaging, ring_node *ringNode, unsigned long long int time );
92 112 unsigned char getSID( rtems_event_set event );
93 113
94 114 extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
95 115 extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
96 116
97 117 //***************************************
98 118 // DEFINITIONS OF STATIC INLINE FUNCTIONS
99 119 static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
100 120 ring_node *ring_node_tab[],
101 unsigned int nbAverageNORM, unsigned int nbAverageSBM );
121 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
122 asm_msg *msgForMATR );
102 123 static inline void SM_average_debug( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
103 124 ring_node *ring_node_tab[],
104 125 unsigned int nbAverageNORM, unsigned int nbAverageSBM );
105 126 static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized,
106 127 float divider );
107 128 static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat,
108 129 float divider,
109 130 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
110 131 static inline void ASM_convert(volatile float *input_matrix, char *output_matrix);
111 132
112 133 void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
113 134 ring_node *ring_node_tab[],
114 unsigned int nbAverageNORM, unsigned int nbAverageSBM )
135 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
136 asm_msg *msgForMATR )
115 137 {
116 138 float sum;
117 139 unsigned int i;
118 140
119 141 for(i=0; i<TOTAL_SIZE_SM; i++)
120 142 {
121 143 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ]
122 144 + ( (int *) (ring_node_tab[1]->buffer_address) ) [ i ]
123 145 + ( (int *) (ring_node_tab[2]->buffer_address) ) [ i ]
124 146 + ( (int *) (ring_node_tab[3]->buffer_address) ) [ i ]
125 147 + ( (int *) (ring_node_tab[4]->buffer_address) ) [ i ]
126 148 + ( (int *) (ring_node_tab[5]->buffer_address) ) [ i ]
127 149 + ( (int *) (ring_node_tab[6]->buffer_address) ) [ i ]
128 150 + ( (int *) (ring_node_tab[7]->buffer_address) ) [ i ];
129 151
130 152 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
131 153 {
132 154 averaged_spec_mat_NORM[ i ] = sum;
133 155 averaged_spec_mat_SBM[ i ] = sum;
156 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
157 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
158 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
159 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
134 160 }
135 161 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
136 162 {
137 163 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
138 164 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
139 165 }
140 166 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
141 167 {
142 168 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
143 169 averaged_spec_mat_SBM[ i ] = sum;
170 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
171 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
144 172 }
145 173 else
146 174 {
147 175 PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
148 176 }
149 177 }
150 178 }
151 179
152 180 void SM_average_debug( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
153 181 ring_node *ring_node_tab[],
154 182 unsigned int nbAverageNORM, unsigned int nbAverageSBM )
155 183 {
156 184 float sum;
157 185 unsigned int i;
158 186
159 187 for(i=0; i<TOTAL_SIZE_SM; i++)
160 188 {
161 189 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ];
162 190
163 191 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
164 192 {
165 193 averaged_spec_mat_NORM[ i ] = sum;
166 194 averaged_spec_mat_SBM[ i ] = sum;
167 195 }
168 196 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
169 197 {
170 198 averaged_spec_mat_NORM[ i ] = sum;
171 199 averaged_spec_mat_SBM[ i ] = sum;
172 200 }
173 201 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
174 202 {
175 203 averaged_spec_mat_NORM[ i ] = sum;
176 204 averaged_spec_mat_SBM[ i ] = sum;
177 205 }
178 206 else
179 207 {
180 208 PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
181 209 }
182 210 }
183 211 }
184 212
185 213 void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
186 214 {
187 215 int frequencyBin;
188 216 int asmComponent;
189 unsigned int offsetAveragedSpecMatReorganized;
190 unsigned int offsetAveragedSpecMat;
217 unsigned int offsetASM;
218 unsigned int offsetASMReorganized;
191 219
220 // BUILD DATA
192 221 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
193 222 {
194 223 for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
195 224 {
196 offsetAveragedSpecMatReorganized =
225 offsetASMReorganized =
197 226 frequencyBin * NB_VALUES_PER_SM
198 227 + asmComponent;
199 offsetAveragedSpecMat =
228 offsetASM =
200 229 asmComponent * NB_BINS_PER_SM
201 230 + frequencyBin;
202 averaged_spec_mat_reorganized[offsetAveragedSpecMatReorganized ] =
203 averaged_spec_mat[ offsetAveragedSpecMat ] / divider;
231 averaged_spec_mat_reorganized[offsetASMReorganized ] =
232 averaged_spec_mat[ offsetASM ] / divider;
204 233 }
205 234 }
206 235 }
207 236
208 237 void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
209 238 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
210 239 {
211 240 int frequencyBin;
212 241 int asmComponent;
213 242 int offsetASM;
214 243 int offsetCompressed;
215 244 int k;
216 245
217 // build data
246 // BUILD DATA
218 247 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
219 248 {
220 249 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
221 250 {
222 251 offsetCompressed = // NO TIME OFFSET
223 252 frequencyBin * NB_VALUES_PER_SM
224 253 + asmComponent;
225 254 offsetASM = // NO TIME OFFSET
226 255 asmComponent * NB_BINS_PER_SM
227 256 + ASMIndexStart
228 257 + frequencyBin * nbBinsToAverage;
229 258 compressed_spec_mat[ offsetCompressed ] = 0;
230 259 for ( k = 0; k < nbBinsToAverage; k++ )
231 260 {
232 261 compressed_spec_mat[offsetCompressed ] =
233 262 ( compressed_spec_mat[ offsetCompressed ]
234 + averaged_spec_mat[ offsetASM + k ] ) / (divider * nbBinsToAverage);
263 + averaged_spec_mat[ offsetASM + k ] );
235 264 }
265 compressed_spec_mat[ offsetCompressed ] =
266 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
236 267 }
237 268 }
238 269 }
239 270
240 271 void ASM_convert( volatile float *input_matrix, char *output_matrix)
241 272 {
242 273 unsigned int frequencyBin;
243 274 unsigned int asmComponent;
244 275 char * pt_char_input;
245 276 char * pt_char_output;
246 277 unsigned int offsetInput;
247 278 unsigned int offsetOutput;
248 279
249 280 pt_char_input = (char*) &input_matrix;
250 281 pt_char_output = (char*) &output_matrix;
251 282
252 283 // convert all other data
253 284 for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++)
254 285 {
255 286 for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++)
256 287 {
257 288 offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ;
258 289 offsetOutput = 2 * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ;
259 290 pt_char_input = (char*) &input_matrix [ offsetInput ];
260 291 pt_char_output = (char*) &output_matrix[ offsetOutput ];
261 292 pt_char_output[0] = pt_char_input[0]; // bits 31 downto 24 of the float
262 293 pt_char_output[1] = pt_char_input[1]; // bits 23 downto 16 of the float
263 294 }
264 295 }
265 296 }
266 297
267 298 #endif // FSW_PROCESSING_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,782 +1,787
1 1 /** This is the RTEMS initialization module.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * This module contains two very different information:
7 7 * - specific instructions to configure the compilation of the RTEMS executive
8 8 * - functions related to the fligth softwre initialization, especially the INIT RTEMS task
9 9 *
10 10 */
11 11
12 12 //*************************
13 13 // GPL reminder to be added
14 14 //*************************
15 15
16 16 #include <rtems.h>
17 17
18 18 /* configuration information */
19 19
20 20 #define CONFIGURE_INIT
21 21
22 22 #include <bsp.h> /* for device driver prototypes */
23 23
24 24 /* configuration information */
25 25
26 26 #define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
27 27 #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
28 28
29 29 #define CONFIGURE_MAXIMUM_TASKS 20
30 30 #define CONFIGURE_RTEMS_INIT_TASKS_TABLE
31 31 #define CONFIGURE_EXTRA_TASK_STACKS (3 * RTEMS_MINIMUM_STACK_SIZE)
32 32 #define CONFIGURE_LIBIO_MAXIMUM_FILE_DESCRIPTORS 32
33 33 #define CONFIGURE_INIT_TASK_PRIORITY 1 // instead of 100
34 34 #define CONFIGURE_INIT_TASK_MODE (RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT)
35 35 #define CONFIGURE_INIT_TASK_ATTRIBUTES (RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT)
36 36 #define CONFIGURE_MAXIMUM_DRIVERS 16
37 37 #define CONFIGURE_MAXIMUM_PERIODS 5
38 38 #define CONFIGURE_MAXIMUM_TIMERS 5 // STAT (1s), send SWF (0.3s), send CWF3 (1s)
39 39 #define CONFIGURE_MAXIMUM_MESSAGE_QUEUES 5
40 40 #ifdef PRINT_STACK_REPORT
41 41 #define CONFIGURE_STACK_CHECKER_ENABLED
42 42 #endif
43 43
44 44 #include <rtems/confdefs.h>
45 45
46 46 /* If --drvmgr was enabled during the configuration of the RTEMS kernel */
47 47 #ifdef RTEMS_DRVMGR_STARTUP
48 48 #ifdef LEON3
49 49 /* Add Timer and UART Driver */
50 50 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
51 51 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
52 52 #endif
53 53 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
54 54 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
55 55 #endif
56 56 #endif
57 57 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
58 58 #include <drvmgr/drvmgr_confdefs.h>
59 59 #endif
60 60
61 61 #include "fsw_init.h"
62 62 #include "fsw_config.c"
63 63
64 64 rtems_task Init( rtems_task_argument ignored )
65 65 {
66 66 /** This is the RTEMS INIT taks, it the first task launched by the system.
67 67 *
68 68 * @param unused is the starting argument of the RTEMS task
69 69 *
70 70 * The INIT task create and run all other RTEMS tasks.
71 71 *
72 72 */
73 73
74 74 unsigned char *vhdlVersion;
75 75
76 76 reset_lfr();
77 77
78 78 reset_local_time();
79 79
80 80 rtems_cpu_usage_reset();
81 81
82 82 rtems_status_code status;
83 83 rtems_status_code status_spw;
84 84 rtems_isr_entry old_isr_handler;
85 85
86 86 // UART settings
87 87 send_console_outputs_on_apbuart_port();
88 88 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
89 89 enable_apbuart_transmitter();
90 90 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
91 91
92 92 PRINTF("\n\n\n\n\n")
93 93 PRINTF("*************************\n")
94 94 PRINTF("** LFR Flight Software **\n")
95 95 PRINTF1("** %d.", SW_VERSION_N1)
96 96 PRINTF1("%d." , SW_VERSION_N2)
97 97 PRINTF1("%d." , SW_VERSION_N3)
98 98 PRINTF1("%d **\n", SW_VERSION_N4)
99 99
100 100 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
101 101 PRINTF("** VHDL **\n")
102 102 PRINTF1("** %d.", vhdlVersion[1])
103 103 PRINTF1("%d." , vhdlVersion[2])
104 104 PRINTF1("%d **\n", vhdlVersion[3])
105 105 PRINTF("*************************\n")
106 106 PRINTF("\n\n")
107 107
108 108 init_parameter_dump();
109 109 init_local_mode_parameters();
110 110 init_housekeeping_parameters();
111 init_k_coefficients_f0();
112 init_k_coefficients_f1();
113 init_k_coefficients_f2();
111 114
112 115 // waveform picker initialization
113 116 WFP_init_rings(); // initialize the waveform rings
114 117 WFP_reset_current_ring_nodes();
115 118 reset_waveform_picker_regs();
116 119
117 120 // spectral matrices initialization
118 121 SM_init_rings(); // initialize spectral matrices rings
119 122 SM_reset_current_ring_nodes();
120 123 reset_spectral_matrix_regs();
121 124
122 125 updateLFRCurrentMode();
123 126
124 127 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
125 128
126 129 create_names(); // create all names
127 130
128 131 status = create_message_queues(); // create message queues
129 132 if (status != RTEMS_SUCCESSFUL)
130 133 {
131 134 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
132 135 }
133 136
134 137 status = create_all_tasks(); // create all tasks
135 138 if (status != RTEMS_SUCCESSFUL)
136 139 {
137 140 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
138 141 }
139 142
140 143 // **************************
141 144 // <SPACEWIRE INITIALIZATION>
142 145 grspw_timecode_callback = &timecode_irq_handler;
143 146
144 147 status_spw = spacewire_open_link(); // (1) open the link
145 148 if ( status_spw != RTEMS_SUCCESSFUL )
146 149 {
147 150 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
148 151 }
149 152
150 153 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
151 154 {
152 155 status_spw = spacewire_configure_link( fdSPW );
153 156 if ( status_spw != RTEMS_SUCCESSFUL )
154 157 {
155 158 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
156 159 }
157 160 }
158 161
159 162 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
160 163 {
161 164 status_spw = spacewire_start_link( fdSPW );
162 165 if ( status_spw != RTEMS_SUCCESSFUL )
163 166 {
164 167 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
165 168 }
166 169 }
167 170 // </SPACEWIRE INITIALIZATION>
168 171 // ***************************
169 172
170 173 status = start_all_tasks(); // start all tasks
171 174 if (status != RTEMS_SUCCESSFUL)
172 175 {
173 176 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
174 177 }
175 178
176 179 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
177 180 status = start_recv_send_tasks();
178 181 if ( status != RTEMS_SUCCESSFUL )
179 182 {
180 183 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
181 184 }
182 185
183 186 // suspend science tasks, they will be restarted later depending on the mode
184 187 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
185 188 if (status != RTEMS_SUCCESSFUL)
186 189 {
187 190 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
188 191 }
189 192
190 193 //******************************
191 194 // <SPECTRAL MATRICES SIMULATOR>
192 195 LEON_Mask_interrupt( IRQ_SM_SIMULATOR );
193 196 configure_timer((gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR, CLKDIV_SM_SIMULATOR,
194 197 IRQ_SPARC_SM_SIMULATOR, spectral_matrices_isr_simu );
195 198 // </SPECTRAL MATRICES SIMULATOR>
196 199 //*******************************
197 200
198 201 // configure IRQ handling for the waveform picker unit
199 202 status = rtems_interrupt_catch( waveforms_isr,
200 203 IRQ_SPARC_WAVEFORM_PICKER,
201 204 &old_isr_handler) ;
202 205 // configure IRQ handling for the spectral matrices unit
203 206 status = rtems_interrupt_catch( spectral_matrices_isr,
204 207 IRQ_SPARC_SPECTRAL_MATRIX,
205 208 &old_isr_handler) ;
206 209
207 210 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
208 211 if ( status_spw != RTEMS_SUCCESSFUL )
209 212 {
210 213 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
211 214 if ( status != RTEMS_SUCCESSFUL ) {
212 215 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
213 216 }
214 217 }
215 218
216 219 BOOT_PRINTF("delete INIT\n")
217 220
221 // test_TCH();
222
218 223 status = rtems_task_delete(RTEMS_SELF);
219 224
220 225 }
221 226
222 227 void init_local_mode_parameters( void )
223 228 {
224 229 /** This function initialize the param_local global variable with default values.
225 230 *
226 231 */
227 232
228 233 unsigned int i;
229 234
230 235 // LOCAL PARAMETERS
231 236
232 237 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
233 238 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
234 239 BOOT_PRINTF1("nb_interrupt_f0_MAX = %d\n", param_local.local_nb_interrupt_f0_MAX)
235 240
236 241 // init sequence counters
237 242
238 243 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
239 244 {
240 245 sequenceCounters_TC_EXE[i] = 0x00;
241 246 }
242 247 sequenceCounters_SCIENCE_NORMAL_BURST = 0x00;
243 248 sequenceCounters_SCIENCE_SBM1_SBM2 = 0x00;
244 249 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
245 250 sequenceCounterParameterDump = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
246 251 }
247 252
248 253 void reset_local_time( void )
249 254 {
250 255 time_management_regs->ctrl = time_management_regs->ctrl | 0x02; // [0010] software reset, coarse time = 0x80000000
251 256 }
252 257
253 258 void create_names( void ) // create all names for tasks and queues
254 259 {
255 260 /** This function creates all RTEMS names used in the software for tasks and queues.
256 261 *
257 262 * @return RTEMS directive status codes:
258 263 * - RTEMS_SUCCESSFUL - successful completion
259 264 *
260 265 */
261 266
262 267 // task names
263 268 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
264 269 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
265 270 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
266 271 Task_name[TASKID_STAT] = rtems_build_name( 'S', 'T', 'A', 'T' );
267 272 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
268 273 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
269 274 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
270 275 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
271 276 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
272 277 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
273 278 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
274 279 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
275 280 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
276 281 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
277 282 Task_name[TASKID_WTDG] = rtems_build_name( 'W', 'T', 'D', 'G' );
278 283 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
279 284 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
280 285 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
281 286 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
282 287
283 288 // rate monotonic period names
284 289 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
285 290
286 291 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
287 292 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
288 293 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
289 294 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
290 295 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
291 296 }
292 297
293 298 int create_all_tasks( void ) // create all tasks which run in the software
294 299 {
295 300 /** This function creates all RTEMS tasks used in the software.
296 301 *
297 302 * @return RTEMS directive status codes:
298 303 * - RTEMS_SUCCESSFUL - task created successfully
299 304 * - RTEMS_INVALID_ADDRESS - id is NULL
300 305 * - RTEMS_INVALID_NAME - invalid task name
301 306 * - RTEMS_INVALID_PRIORITY - invalid task priority
302 307 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
303 308 * - RTEMS_TOO_MANY - too many tasks created
304 309 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
305 310 * - RTEMS_TOO_MANY - too many global objects
306 311 *
307 312 */
308 313
309 314 rtems_status_code status;
310 315
311 316 //**********
312 317 // SPACEWIRE
313 318 // RECV
314 319 status = rtems_task_create(
315 320 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
316 321 RTEMS_DEFAULT_MODES,
317 322 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
318 323 );
319 324 if (status == RTEMS_SUCCESSFUL) // SEND
320 325 {
321 326 status = rtems_task_create(
322 327 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE,
323 328 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
324 329 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
325 330 );
326 331 }
327 332 if (status == RTEMS_SUCCESSFUL) // WTDG
328 333 {
329 334 status = rtems_task_create(
330 335 Task_name[TASKID_WTDG], TASK_PRIORITY_WTDG, RTEMS_MINIMUM_STACK_SIZE,
331 336 RTEMS_DEFAULT_MODES,
332 337 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_WTDG]
333 338 );
334 339 }
335 340 if (status == RTEMS_SUCCESSFUL) // ACTN
336 341 {
337 342 status = rtems_task_create(
338 343 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
339 344 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
340 345 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
341 346 );
342 347 }
343 348 if (status == RTEMS_SUCCESSFUL) // SPIQ
344 349 {
345 350 status = rtems_task_create(
346 351 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
347 352 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
348 353 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
349 354 );
350 355 }
351 356
352 357 //******************
353 358 // SPECTRAL MATRICES
354 359 if (status == RTEMS_SUCCESSFUL) // AVF0
355 360 {
356 361 status = rtems_task_create(
357 362 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
358 363 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
359 364 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
360 365 );
361 366 }
362 367 if (status == RTEMS_SUCCESSFUL) // PRC0
363 368 {
364 369 status = rtems_task_create(
365 370 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * 2,
366 371 RTEMS_DEFAULT_MODES,
367 372 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
368 373 );
369 374 }
370 375 if (status == RTEMS_SUCCESSFUL) // AVF1
371 376 {
372 377 status = rtems_task_create(
373 378 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
374 379 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
375 380 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
376 381 );
377 382 }
378 383 if (status == RTEMS_SUCCESSFUL) // PRC1
379 384 {
380 385 status = rtems_task_create(
381 386 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * 2,
382 387 RTEMS_DEFAULT_MODES,
383 388 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
384 389 );
385 390 }
386 391 if (status == RTEMS_SUCCESSFUL) // AVF2
387 392 {
388 393 status = rtems_task_create(
389 394 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
390 395 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
391 396 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
392 397 );
393 398 }
394 399 if (status == RTEMS_SUCCESSFUL) // PRC2
395 400 {
396 401 status = rtems_task_create(
397 402 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * 2,
398 403 RTEMS_DEFAULT_MODES,
399 404 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
400 405 );
401 406 }
402 407
403 408 //****************
404 409 // WAVEFORM PICKER
405 410 if (status == RTEMS_SUCCESSFUL) // WFRM
406 411 {
407 412 status = rtems_task_create(
408 413 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
409 414 RTEMS_DEFAULT_MODES,
410 415 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
411 416 );
412 417 }
413 418 if (status == RTEMS_SUCCESSFUL) // CWF3
414 419 {
415 420 status = rtems_task_create(
416 421 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
417 422 RTEMS_DEFAULT_MODES,
418 423 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
419 424 );
420 425 }
421 426 if (status == RTEMS_SUCCESSFUL) // CWF2
422 427 {
423 428 status = rtems_task_create(
424 429 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
425 430 RTEMS_DEFAULT_MODES,
426 431 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
427 432 );
428 433 }
429 434 if (status == RTEMS_SUCCESSFUL) // CWF1
430 435 {
431 436 status = rtems_task_create(
432 437 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
433 438 RTEMS_DEFAULT_MODES,
434 439 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
435 440 );
436 441 }
437 442 if (status == RTEMS_SUCCESSFUL) // SWBD
438 443 {
439 444 status = rtems_task_create(
440 445 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
441 446 RTEMS_DEFAULT_MODES,
442 447 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
443 448 );
444 449 }
445 450
446 451 //*****
447 452 // MISC
448 453 if (status == RTEMS_SUCCESSFUL) // STAT
449 454 {
450 455 status = rtems_task_create(
451 456 Task_name[TASKID_STAT], TASK_PRIORITY_STAT, RTEMS_MINIMUM_STACK_SIZE,
452 457 RTEMS_DEFAULT_MODES,
453 458 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_STAT]
454 459 );
455 460 }
456 461 if (status == RTEMS_SUCCESSFUL) // DUMB
457 462 {
458 463 status = rtems_task_create(
459 464 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
460 465 RTEMS_DEFAULT_MODES,
461 466 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
462 467 );
463 468 }
464 469 if (status == RTEMS_SUCCESSFUL) // HOUS
465 470 {
466 471 status = rtems_task_create(
467 472 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
468 473 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
469 474 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
470 475 );
471 476 }
472 477
473 478 return status;
474 479 }
475 480
476 481 int start_recv_send_tasks( void )
477 482 {
478 483 rtems_status_code status;
479 484
480 485 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
481 486 if (status!=RTEMS_SUCCESSFUL) {
482 487 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
483 488 }
484 489
485 490 if (status == RTEMS_SUCCESSFUL) // SEND
486 491 {
487 492 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
488 493 if (status!=RTEMS_SUCCESSFUL) {
489 494 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
490 495 }
491 496 }
492 497
493 498 return status;
494 499 }
495 500
496 501 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
497 502 {
498 503 /** This function starts all RTEMS tasks used in the software.
499 504 *
500 505 * @return RTEMS directive status codes:
501 506 * - RTEMS_SUCCESSFUL - ask started successfully
502 507 * - RTEMS_INVALID_ADDRESS - invalid task entry point
503 508 * - RTEMS_INVALID_ID - invalid task id
504 509 * - RTEMS_INCORRECT_STATE - task not in the dormant state
505 510 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
506 511 *
507 512 */
508 513 // starts all the tasks fot eh flight software
509 514
510 515 rtems_status_code status;
511 516
512 517 //**********
513 518 // SPACEWIRE
514 519 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
515 520 if (status!=RTEMS_SUCCESSFUL) {
516 521 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
517 522 }
518 523
519 524 if (status == RTEMS_SUCCESSFUL) // WTDG
520 525 {
521 526 status = rtems_task_start( Task_id[TASKID_WTDG], wtdg_task, 1 );
522 527 if (status!=RTEMS_SUCCESSFUL) {
523 528 BOOT_PRINTF("in INIT *** Error starting TASK_WTDG\n")
524 529 }
525 530 }
526 531
527 532 if (status == RTEMS_SUCCESSFUL) // ACTN
528 533 {
529 534 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
530 535 if (status!=RTEMS_SUCCESSFUL) {
531 536 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
532 537 }
533 538 }
534 539
535 540 //******************
536 541 // SPECTRAL MATRICES
537 542 if (status == RTEMS_SUCCESSFUL) // AVF0
538 543 {
539 544 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
540 545 if (status!=RTEMS_SUCCESSFUL) {
541 546 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
542 547 }
543 548 }
544 549 if (status == RTEMS_SUCCESSFUL) // PRC0
545 550 {
546 551 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
547 552 if (status!=RTEMS_SUCCESSFUL) {
548 553 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
549 554 }
550 555 }
551 556 if (status == RTEMS_SUCCESSFUL) // AVF1
552 557 {
553 558 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
554 559 if (status!=RTEMS_SUCCESSFUL) {
555 560 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
556 561 }
557 562 }
558 563 if (status == RTEMS_SUCCESSFUL) // PRC1
559 564 {
560 565 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
561 566 if (status!=RTEMS_SUCCESSFUL) {
562 567 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
563 568 }
564 569 }
565 570 if (status == RTEMS_SUCCESSFUL) // AVF2
566 571 {
567 572 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
568 573 if (status!=RTEMS_SUCCESSFUL) {
569 574 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
570 575 }
571 576 }
572 577 if (status == RTEMS_SUCCESSFUL) // PRC2
573 578 {
574 579 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
575 580 if (status!=RTEMS_SUCCESSFUL) {
576 581 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
577 582 }
578 583 }
579 584
580 585 //****************
581 586 // WAVEFORM PICKER
582 587 if (status == RTEMS_SUCCESSFUL) // WFRM
583 588 {
584 589 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
585 590 if (status!=RTEMS_SUCCESSFUL) {
586 591 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
587 592 }
588 593 }
589 594 if (status == RTEMS_SUCCESSFUL) // CWF3
590 595 {
591 596 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
592 597 if (status!=RTEMS_SUCCESSFUL) {
593 598 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
594 599 }
595 600 }
596 601 if (status == RTEMS_SUCCESSFUL) // CWF2
597 602 {
598 603 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
599 604 if (status!=RTEMS_SUCCESSFUL) {
600 605 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
601 606 }
602 607 }
603 608 if (status == RTEMS_SUCCESSFUL) // CWF1
604 609 {
605 610 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
606 611 if (status!=RTEMS_SUCCESSFUL) {
607 612 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
608 613 }
609 614 }
610 615 if (status == RTEMS_SUCCESSFUL) // SWBD
611 616 {
612 617 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
613 618 if (status!=RTEMS_SUCCESSFUL) {
614 619 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
615 620 }
616 621 }
617 622
618 623 //*****
619 624 // MISC
620 625 if (status == RTEMS_SUCCESSFUL) // HOUS
621 626 {
622 627 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
623 628 if (status!=RTEMS_SUCCESSFUL) {
624 629 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
625 630 }
626 631 }
627 632 if (status == RTEMS_SUCCESSFUL) // DUMB
628 633 {
629 634 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
630 635 if (status!=RTEMS_SUCCESSFUL) {
631 636 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
632 637 }
633 638 }
634 639 if (status == RTEMS_SUCCESSFUL) // STAT
635 640 {
636 641 status = rtems_task_start( Task_id[TASKID_STAT], stat_task, 1 );
637 642 if (status!=RTEMS_SUCCESSFUL) {
638 643 BOOT_PRINTF("in INIT *** Error starting TASK_STAT\n")
639 644 }
640 645 }
641 646
642 647 return status;
643 648 }
644 649
645 650 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
646 651 {
647 652 rtems_status_code status_recv;
648 653 rtems_status_code status_send;
649 654 rtems_status_code status_q_p0;
650 655 rtems_status_code status_q_p1;
651 656 rtems_status_code status_q_p2;
652 657 rtems_status_code ret;
653 658 rtems_id queue_id;
654 659
655 660 //****************************************
656 661 // create the queue for handling valid TCs
657 662 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
658 663 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
659 664 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
660 665 if ( status_recv != RTEMS_SUCCESSFUL ) {
661 666 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
662 667 }
663 668
664 669 //************************************************
665 670 // create the queue for handling TM packet sending
666 671 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
667 672 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
668 673 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
669 674 if ( status_send != RTEMS_SUCCESSFUL ) {
670 675 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
671 676 }
672 677
673 678 //*****************************************************************************
674 679 // create the queue for handling averaged spectral matrices for processing @ f0
675 680 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
676 681 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
677 682 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
678 683 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
679 684 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
680 685 }
681 686
682 687 //*****************************************************************************
683 688 // create the queue for handling averaged spectral matrices for processing @ f1
684 689 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
685 690 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
686 691 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
687 692 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
688 693 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
689 694 }
690 695
691 696 //*****************************************************************************
692 697 // create the queue for handling averaged spectral matrices for processing @ f2
693 698 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
694 699 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
695 700 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
696 701 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
697 702 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
698 703 }
699 704
700 705 if ( status_recv != RTEMS_SUCCESSFUL )
701 706 {
702 707 ret = status_recv;
703 708 }
704 709 else if( status_send != RTEMS_SUCCESSFUL )
705 710 {
706 711 ret = status_send;
707 712 }
708 713 else if( status_q_p0 != RTEMS_SUCCESSFUL )
709 714 {
710 715 ret = status_q_p0;
711 716 }
712 717 else if( status_q_p1 != RTEMS_SUCCESSFUL )
713 718 {
714 719 ret = status_q_p1;
715 720 }
716 721 else
717 722 {
718 723 ret = status_q_p2;
719 724 }
720 725
721 726 return ret;
722 727 }
723 728
724 729 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
725 730 {
726 731 rtems_status_code status;
727 732 rtems_name queue_name;
728 733
729 734 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
730 735
731 736 status = rtems_message_queue_ident( queue_name, 0, queue_id );
732 737
733 738 return status;
734 739 }
735 740
736 741 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
737 742 {
738 743 rtems_status_code status;
739 744 rtems_name queue_name;
740 745
741 746 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
742 747
743 748 status = rtems_message_queue_ident( queue_name, 0, queue_id );
744 749
745 750 return status;
746 751 }
747 752
748 753 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
749 754 {
750 755 rtems_status_code status;
751 756 rtems_name queue_name;
752 757
753 758 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
754 759
755 760 status = rtems_message_queue_ident( queue_name, 0, queue_id );
756 761
757 762 return status;
758 763 }
759 764
760 765 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
761 766 {
762 767 rtems_status_code status;
763 768 rtems_name queue_name;
764 769
765 770 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
766 771
767 772 status = rtems_message_queue_ident( queue_name, 0, queue_id );
768 773
769 774 return status;
770 775 }
771 776
772 777 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
773 778 {
774 779 rtems_status_code status;
775 780 rtems_name queue_name;
776 781
777 782 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
778 783
779 784 status = rtems_message_queue_ident( queue_name, 0, queue_id );
780 785
781 786 return status;
782 787 }
Show file before | Show file after | Hide comments
@@ -1,574 +1,487
1 1 /** General usage functions and RTEMS tasks.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 */
7 7
8 8 #include "fsw_misc.h"
9 9
10 10 void configure_timer(gptimer_regs_t *gptimer_regs, unsigned char timer, unsigned int clock_divider,
11 11 unsigned char interrupt_level, rtems_isr (*timer_isr)() )
12 12 {
13 13 /** This function configures a GPTIMER timer instantiated in the VHDL design.
14 14 *
15 15 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
16 16 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
17 17 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
18 18 * @param interrupt_level is the interrupt level that the timer drives.
19 19 * @param timer_isr is the interrupt subroutine that will be attached to the IRQ driven by the timer.
20 20 *
21 21 * Interrupt levels are described in the SPARC documentation sparcv8.pdf p.76
22 22 *
23 23 */
24 24
25 25 rtems_status_code status;
26 26 rtems_isr_entry old_isr_handler;
27 27
28 28 gptimer_regs->timer[timer].ctrl = 0x00; // reset the control register
29 29
30 30 status = rtems_interrupt_catch( timer_isr, interrupt_level, &old_isr_handler) ; // see sparcv8.pdf p.76 for interrupt levels
31 31 if (status!=RTEMS_SUCCESSFUL)
32 32 {
33 33 PRINTF("in configure_timer *** ERR rtems_interrupt_catch\n")
34 34 }
35 35
36 36 timer_set_clock_divider( gptimer_regs, timer, clock_divider);
37 37 }
38 38
39 39 void timer_start(gptimer_regs_t *gptimer_regs, unsigned char timer)
40 40 {
41 41 /** This function starts a GPTIMER timer.
42 42 *
43 43 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
44 44 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
45 45 *
46 46 */
47 47
48 48 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000010; // clear pending IRQ if any
49 49 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000004; // LD load value from the reload register
50 50 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000001; // EN enable the timer
51 51 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000002; // RS restart
52 52 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000008; // IE interrupt enable
53 53 }
54 54
55 55 void timer_stop(gptimer_regs_t *gptimer_regs, unsigned char timer)
56 56 {
57 57 /** This function stops a GPTIMER timer.
58 58 *
59 59 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
60 60 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
61 61 *
62 62 */
63 63
64 64 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & 0xfffffffe; // EN enable the timer
65 65 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & 0xffffffef; // IE interrupt enable
66 66 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000010; // clear pending IRQ if any
67 67 }
68 68
69 69 void timer_set_clock_divider(gptimer_regs_t *gptimer_regs, unsigned char timer, unsigned int clock_divider)
70 70 {
71 71 /** This function sets the clock divider of a GPTIMER timer.
72 72 *
73 73 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
74 74 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
75 75 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
76 76 *
77 77 */
78 78
79 79 gptimer_regs->timer[timer].reload = clock_divider; // base clock frequency is 1 MHz
80 80 }
81 81
82 82 int send_console_outputs_on_apbuart_port( void ) // Send the console outputs on the apbuart port
83 83 {
84 84 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
85 85
86 86 apbuart_regs->ctrl = APBUART_CTRL_REG_MASK_TE;
87 87
88 88 return 0;
89 89 }
90 90
91 91 int enable_apbuart_transmitter( void ) // set the bit 1, TE Transmitter Enable to 1 in the APBUART control register
92 92 {
93 93 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
94 94
95 95 apbuart_regs->ctrl = apbuart_regs->ctrl | APBUART_CTRL_REG_MASK_TE;
96 96
97 97 return 0;
98 98 }
99 99
100 100 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value)
101 101 {
102 102 /** This function sets the scaler reload register of the apbuart module
103 103 *
104 104 * @param regs is the address of the apbuart registers in memory
105 105 * @param value is the value that will be stored in the scaler register
106 106 *
107 107 * The value shall be set by the software to get data on the serial interface.
108 108 *
109 109 */
110 110
111 111 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) regs;
112 112
113 113 apbuart_regs->scaler = value;
114 114 BOOT_PRINTF1("OK *** apbuart port scaler reload register set to 0x%x\n", value)
115 115 }
116 116
117 117 //************
118 118 // RTEMS TASKS
119 119
120 120 rtems_task stat_task(rtems_task_argument argument)
121 121 {
122 122 int i;
123 123 int j;
124 124 i = 0;
125 125 j = 0;
126 126 BOOT_PRINTF("in STAT *** \n")
127 127 while(1){
128 128 rtems_task_wake_after(1000);
129 129 PRINTF1("%d\n", j)
130 130 if (i == CPU_USAGE_REPORT_PERIOD) {
131 131 // #ifdef PRINT_TASK_STATISTICS
132 132 // rtems_cpu_usage_report();
133 133 // rtems_cpu_usage_reset();
134 134 // #endif
135 135 i = 0;
136 136 }
137 137 else i++;
138 138 j++;
139 139 }
140 140 }
141 141
142 142 rtems_task hous_task(rtems_task_argument argument)
143 143 {
144 144 rtems_status_code status;
145 145 rtems_status_code spare_status;
146 146 rtems_id queue_id;
147 147 rtems_rate_monotonic_period_status period_status;
148 148
149 149 status = get_message_queue_id_send( &queue_id );
150 150 if (status != RTEMS_SUCCESSFUL)
151 151 {
152 152 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
153 153 }
154 154
155 155 BOOT_PRINTF("in HOUS ***\n")
156 156
157 157 if (rtems_rate_monotonic_ident( name_hk_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
158 158 status = rtems_rate_monotonic_create( name_hk_rate_monotonic, &HK_id );
159 159 if( status != RTEMS_SUCCESSFUL ) {
160 160 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status )
161 161 }
162 162 }
163 163
164 164 housekeeping_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
165 165 housekeeping_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
166 166 housekeeping_packet.reserved = DEFAULT_RESERVED;
167 167 housekeeping_packet.userApplication = CCSDS_USER_APP;
168 168 housekeeping_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
169 169 housekeeping_packet.packetID[1] = (unsigned char) (APID_TM_HK);
170 170 housekeeping_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
171 171 housekeeping_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
172 172 housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
173 173 housekeeping_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
174 174 housekeeping_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
175 175 housekeeping_packet.serviceType = TM_TYPE_HK;
176 176 housekeeping_packet.serviceSubType = TM_SUBTYPE_HK;
177 177 housekeeping_packet.destinationID = TM_DESTINATION_ID_GROUND;
178 178 housekeeping_packet.sid = SID_HK;
179 179
180 180 status = rtems_rate_monotonic_cancel(HK_id);
181 181 if( status != RTEMS_SUCCESSFUL ) {
182 182 PRINTF1( "ERR *** in HOUS *** rtems_rate_monotonic_cancel(HK_id) ***code: %d\n", status )
183 183 }
184 184 else {
185 185 DEBUG_PRINTF("OK *** in HOUS *** rtems_rate_monotonic_cancel(HK_id)\n")
186 186 }
187 187
188 188 // startup phase
189 189 status = rtems_rate_monotonic_period( HK_id, SY_LFR_TIME_SYN_TIMEOUT_in_ticks );
190 190 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
191 191 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
192 192 while(period_status.state != RATE_MONOTONIC_EXPIRED ) // after SY_LFR_TIME_SYN_TIMEOUT ms, starts HK anyway
193 193 {
194 194 if ((time_management_regs->coarse_time & 0x80000000) == 0x00000000) // check time synchronization
195 195 {
196 196 break; // break if LFR is synchronized
197 197 }
198 198 else
199 199 {
200 200 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
201 201 // sched_yield();
202 202 status = rtems_task_wake_after( 10 ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 100 ms = 10 * 10 ms
203 203 }
204 204 }
205 205 status = rtems_rate_monotonic_cancel(HK_id);
206 206 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
207 207
208 208 while(1){ // launch the rate monotonic task
209 209 status = rtems_rate_monotonic_period( HK_id, HK_PERIOD );
210 210 if ( status != RTEMS_SUCCESSFUL ) {
211 211 PRINTF1( "in HOUS *** ERR period: %d\n", status);
212 212 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_6 );
213 213 }
214 214 else {
215 215 housekeeping_packet.packetSequenceControl[0] = (unsigned char) (sequenceCounterHK >> 8);
216 216 housekeeping_packet.packetSequenceControl[1] = (unsigned char) (sequenceCounterHK );
217 217 increment_seq_counter( &sequenceCounterHK );
218 218
219 219 housekeeping_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
220 220 housekeeping_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
221 221 housekeeping_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
222 222 housekeeping_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
223 223 housekeeping_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
224 224 housekeeping_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
225 225
226 226 spacewire_update_statistics();
227 227
228 // get_v_e1_e2_f3( housekeeping_packet.hk_lfr_sc_v_f3 );
228 get_v_e1_e2_f3( housekeeping_packet.hk_lfr_sc_v_f3 );
229 229 get_cpu_load( (unsigned char *) &housekeeping_packet.hk_lfr_cpu_load );
230 230
231 231 // SEND PACKET
232 232 status = rtems_message_queue_send( queue_id, &housekeeping_packet,
233 233 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
234 234 if (status != RTEMS_SUCCESSFUL) {
235 235 PRINTF1("in HOUS *** ERR send: %d\n", status)
236 236 }
237 237 }
238 238 }
239 239
240 240 PRINTF("in HOUS *** deleting task\n")
241 241
242 242 status = rtems_task_delete( RTEMS_SELF ); // should not return
243 243 printf( "rtems_task_delete returned with status of %d.\n", status );
244 244 return;
245 245 }
246 246
247 247 rtems_task dumb_task( rtems_task_argument unused )
248 248 {
249 249 /** This RTEMS taks is used to print messages without affecting the general behaviour of the software.
250 250 *
251 251 * @param unused is the starting argument of the RTEMS task
252 252 *
253 253 * The DUMB taks waits for RTEMS events and print messages depending on the incoming events.
254 254 *
255 255 */
256 256
257 257 unsigned int i;
258 258 unsigned int intEventOut;
259 259 unsigned int coarse_time = 0;
260 260 unsigned int fine_time = 0;
261 261 rtems_event_set event_out;
262 262
263 263 char *DumbMessages[12] = {"in DUMB *** default", // RTEMS_EVENT_0
264 264 "in DUMB *** timecode_irq_handler", // RTEMS_EVENT_1
265 265 "in DUMB *** f3 buffer changed", // RTEMS_EVENT_2
266 266 "in DUMB *** in SMIQ *** Error sending event to AVF0", // RTEMS_EVENT_3
267 267 "in DUMB *** spectral_matrices_isr *** Error sending event to SMIQ", // RTEMS_EVENT_4
268 268 "in DUMB *** waveforms_simulator_isr", // RTEMS_EVENT_5
269 269 "VHDL SM *** two buffers f0 ready", // RTEMS_EVENT_6
270 270 "ready for dump", // RTEMS_EVENT_7
271 271 "VHDL ERR *** spectral matrix", // RTEMS_EVENT_8
272 272 "tick", // RTEMS_EVENT_9
273 273 "VHDL ERR *** waveform picker", // RTEMS_EVENT_10
274 274 "VHDL ERR *** unexpected ready matrix values" // RTEMS_EVENT_11
275 275 };
276 276
277 277 BOOT_PRINTF("in DUMB *** \n")
278 278
279 279 while(1){
280 280 rtems_event_receive(RTEMS_EVENT_0 | RTEMS_EVENT_1 | RTEMS_EVENT_2 | RTEMS_EVENT_3
281 281 | RTEMS_EVENT_4 | RTEMS_EVENT_5 | RTEMS_EVENT_6 | RTEMS_EVENT_7
282 282 | RTEMS_EVENT_8 | RTEMS_EVENT_9,
283 283 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT
284 284 intEventOut = (unsigned int) event_out;
285 285 for ( i=0; i<32; i++)
286 286 {
287 287 if ( ((intEventOut >> i) & 0x0001) != 0)
288 288 {
289 289 coarse_time = time_management_regs->coarse_time;
290 290 fine_time = time_management_regs->fine_time;
291 291 printf("in DUMB *** coarse: %x, fine: %x, %s\n", coarse_time, fine_time, DumbMessages[i]);
292 292 if (i==8)
293 293 {
294 294 }
295 295 if (i==10)
296 296 {
297 297 }
298 298 }
299 299 }
300 300 }
301 301 }
302 302
303 303 //*****************************
304 304 // init housekeeping parameters
305 305
306 306 void init_housekeeping_parameters( void )
307 307 {
308 308 /** This function initialize the housekeeping_packet global variable with default values.
309 309 *
310 310 */
311 311
312 312 unsigned int i = 0;
313 313 unsigned char *parameters;
314 314
315 315 parameters = (unsigned char*) &housekeeping_packet.lfr_status_word;
316 316 for(i = 0; i< SIZE_HK_PARAMETERS; i++)
317 317 {
318 318 parameters[i] = 0x00;
319 319 }
320 320 // init status word
321 321 housekeeping_packet.lfr_status_word[0] = DEFAULT_STATUS_WORD_BYTE0;
322 322 housekeeping_packet.lfr_status_word[1] = DEFAULT_STATUS_WORD_BYTE1;
323 323 // init software version
324 324 housekeeping_packet.lfr_sw_version[0] = SW_VERSION_N1;
325 325 housekeeping_packet.lfr_sw_version[1] = SW_VERSION_N2;
326 326 housekeeping_packet.lfr_sw_version[2] = SW_VERSION_N3;
327 327 housekeeping_packet.lfr_sw_version[3] = SW_VERSION_N4;
328 328 // init fpga version
329 329 parameters = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
330 330 housekeeping_packet.lfr_fpga_version[0] = parameters[1]; // n1
331 331 housekeeping_packet.lfr_fpga_version[1] = parameters[2]; // n2
332 332 housekeeping_packet.lfr_fpga_version[2] = parameters[3]; // n3
333 333 }
334 334
335 335 void increment_seq_counter( unsigned short *packetSequenceControl )
336 336 {
337 337 /** This function increment the sequence counter psased in argument.
338 338 *
339 339 * The increment does not affect the grouping flag. In case of an overflow, the counter is reset to 0.
340 340 *
341 341 */
342 342
343 343 unsigned short segmentation_grouping_flag;
344 344 unsigned short sequence_cnt;
345 345
346 346 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8; // keep bits 7 downto 6
347 347 sequence_cnt = (*packetSequenceControl) & 0x3fff; // [0011 1111 1111 1111]
348 348
349 349 if ( sequence_cnt < SEQ_CNT_MAX)
350 350 {
351 351 sequence_cnt = sequence_cnt + 1;
352 352 }
353 353 else
354 354 {
355 355 sequence_cnt = 0;
356 356 }
357 357
358 358 *packetSequenceControl = segmentation_grouping_flag | sequence_cnt ;
359 359 }
360 360
361 361 void getTime( unsigned char *time)
362 362 {
363 363 /** This function write the current local time in the time buffer passed in argument.
364 364 *
365 365 */
366 366
367 367 time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
368 368 time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
369 369 time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
370 370 time[3] = (unsigned char) (time_management_regs->coarse_time);
371 371 time[4] = (unsigned char) (time_management_regs->fine_time>>8);
372 372 time[5] = (unsigned char) (time_management_regs->fine_time);
373 373 }
374 374
375 375 unsigned long long int getTimeAsUnsignedLongLongInt( )
376 376 {
377 377 /** This function write the current local time in the time buffer passed in argument.
378 378 *
379 379 */
380 380 unsigned long long int time;
381 381
382 382 time = ( (unsigned long long int) (time_management_regs->coarse_time & 0x7fffffff) << 16 )
383 383 + time_management_regs->fine_time;
384 384
385 385 return time;
386 386 }
387 387
388 388 void send_dumb_hk( void )
389 389 {
390 390 Packet_TM_LFR_HK_t dummy_hk_packet;
391 391 unsigned char *parameters;
392 392 unsigned int i;
393 393 rtems_id queue_id;
394 394
395 395 dummy_hk_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
396 396 dummy_hk_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
397 397 dummy_hk_packet.reserved = DEFAULT_RESERVED;
398 398 dummy_hk_packet.userApplication = CCSDS_USER_APP;
399 399 dummy_hk_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
400 400 dummy_hk_packet.packetID[1] = (unsigned char) (APID_TM_HK);
401 401 dummy_hk_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
402 402 dummy_hk_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
403 403 dummy_hk_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
404 404 dummy_hk_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
405 405 dummy_hk_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
406 406 dummy_hk_packet.serviceType = TM_TYPE_HK;
407 407 dummy_hk_packet.serviceSubType = TM_SUBTYPE_HK;
408 408 dummy_hk_packet.destinationID = TM_DESTINATION_ID_GROUND;
409 409 dummy_hk_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
410 410 dummy_hk_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
411 411 dummy_hk_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
412 412 dummy_hk_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
413 413 dummy_hk_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
414 414 dummy_hk_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
415 415 dummy_hk_packet.sid = SID_HK;
416 416
417 417 // init status word
418 418 dummy_hk_packet.lfr_status_word[0] = 0xff;
419 419 dummy_hk_packet.lfr_status_word[1] = 0xff;
420 420 // init software version
421 421 dummy_hk_packet.lfr_sw_version[0] = SW_VERSION_N1;
422 422 dummy_hk_packet.lfr_sw_version[1] = SW_VERSION_N2;
423 423 dummy_hk_packet.lfr_sw_version[2] = SW_VERSION_N3;
424 424 dummy_hk_packet.lfr_sw_version[3] = SW_VERSION_N4;
425 425 // init fpga version
426 426 parameters = (unsigned char *) (REGS_ADDR_WAVEFORM_PICKER + 0xb0);
427 427 dummy_hk_packet.lfr_fpga_version[0] = parameters[1]; // n1
428 428 dummy_hk_packet.lfr_fpga_version[1] = parameters[2]; // n2
429 429 dummy_hk_packet.lfr_fpga_version[2] = parameters[3]; // n3
430 430
431 431 parameters = (unsigned char *) &dummy_hk_packet.hk_lfr_cpu_load;
432 432
433 433 for (i=0; i<100; i++)
434 434 {
435 435 parameters[i] = 0xff;
436 436 }
437 437
438 438 get_message_queue_id_send( &queue_id );
439 439
440 440 rtems_message_queue_send( queue_id, &dummy_hk_packet,
441 441 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
442 442 }
443 443
444 444 void get_v_e1_e2_f3( unsigned char *spacecraft_potential )
445 445 {
446 unsigned long long int localTime_asLong;
447 unsigned long long int f3_0_AcquisitionTime_asLong;
448 unsigned long long int f3_1_AcquisitionTime_asLong;
449 unsigned long long int deltaT;
450 unsigned long long int deltaT_f3_0;
451 unsigned long long int deltaT_f3_1;
452 unsigned char *bufferPtr;
453
454 unsigned int offset_in_samples;
455 unsigned int offset_in_bytes;
456 unsigned char f3;
457
458 bufferPtr = NULL;
459 deltaT = 0;
460 deltaT_f3_0 = 0xffffffff;
461 deltaT_f3_1 = 0xffffffff;
462 f3 = 16; // v, e1 and e2 will be picked up each second, f3 = 16 Hz
463
464 if (lfrCurrentMode == LFR_MODE_STANDBY)
465 {
466 spacecraft_potential[0] = 0x00;
467 spacecraft_potential[1] = 0x00;
468 spacecraft_potential[2] = 0x00;
469 spacecraft_potential[3] = 0x00;
470 spacecraft_potential[4] = 0x00;
471 spacecraft_potential[5] = 0x00;
472 }
473 else
474 {
475 localTime_asLong = get_acquisition_time( (unsigned char *) &time_management_regs->coarse_time );
476 f3_0_AcquisitionTime_asLong = get_acquisition_time( (unsigned char *) &waveform_picker_regs->f3_0_coarse_time );
477 f3_1_AcquisitionTime_asLong = get_acquisition_time( (unsigned char *) &waveform_picker_regs->f3_1_coarse_time );
478 printf("localTime 0x%llx, f3_0 0x%llx, f3_1 0x%llx\n",
479 localTime_asLong,
480 f3_0_AcquisitionTime_asLong,
481 f3_1_AcquisitionTime_asLong);
482
483 if ( localTime_asLong >= f3_0_AcquisitionTime_asLong )
484 {
485 deltaT_f3_0 = localTime_asLong - f3_0_AcquisitionTime_asLong;
486 }
487
488 if ( localTime_asLong > f3_1_AcquisitionTime_asLong )
489 {
490 deltaT_f3_1 = localTime_asLong - f3_1_AcquisitionTime_asLong;
491 }
446 unsigned char* v_ptr;
447 unsigned char* e1_ptr;
448 unsigned char* e2_ptr;
492 449
493 if ( (deltaT_f3_0 != 0xffffffff) && (deltaT_f3_1 != 0xffffffff) )
494 {
495 if ( deltaT_f3_0 > deltaT_f3_1 )
496 {
497 deltaT = deltaT_f3_1;
498 bufferPtr = (unsigned char*) waveform_picker_regs->addr_data_f3_1;
499 }
500 else
501 {
502 deltaT = deltaT_f3_0;
503 bufferPtr = (unsigned char*) waveform_picker_regs->addr_data_f3_0;
504 }
505 }
506 else if ( (deltaT_f3_0 == 0xffffffff) && (deltaT_f3_1 != 0xffffffff) )
507 {
508 deltaT = deltaT_f3_1;
509 bufferPtr = (unsigned char*) waveform_picker_regs->addr_data_f3_1;
510 }
511 else if ( (deltaT_f3_0 != 0xffffffff) && (deltaT_f3_1 == 0xffffffff) )
512 {
513 deltaT = deltaT_f3_0;
514 bufferPtr = (unsigned char*) waveform_picker_regs->addr_data_f3_1;
515 }
516 else
517 {
518 deltaT = 0xffffffff;
519 }
450 v_ptr = (unsigned char *) &waveform_picker_regs->v;
451 e1_ptr = (unsigned char *) &waveform_picker_regs->e1;
452 e2_ptr = (unsigned char *) &waveform_picker_regs->e2;
520 453
521 if ( deltaT == 0xffffffff )
522 {
523 spacecraft_potential[0] = 0x00;
524 spacecraft_potential[1] = 0x00;
525 spacecraft_potential[2] = 0x00;
526 spacecraft_potential[3] = 0x00;
527 spacecraft_potential[4] = 0x00;
528 spacecraft_potential[5] = 0x00;
529 }
530 else
531 {
532 offset_in_samples = ( (double) deltaT ) / 65536. * f3;
533 if ( offset_in_samples > (NB_SAMPLES_PER_SNAPSHOT - 1) )
534 {
535 PRINTF1("ERR *** in get_v_e1_e2_f3 *** trying to read out of the buffer, counter = %d\n", offset_in_samples)
536 offset_in_samples = NB_SAMPLES_PER_SNAPSHOT - 1;
537 }
538 offset_in_bytes = offset_in_samples * NB_WORDS_SWF_BLK * 4;
539 spacecraft_potential[0] = bufferPtr[ offset_in_bytes + 0];
540 spacecraft_potential[1] = bufferPtr[ offset_in_bytes + 1];
541 spacecraft_potential[2] = bufferPtr[ offset_in_bytes + 2];
542 spacecraft_potential[3] = bufferPtr[ offset_in_bytes + 3];
543 spacecraft_potential[4] = bufferPtr[ offset_in_bytes + 4];
544 spacecraft_potential[5] = bufferPtr[ offset_in_bytes + 5];
545 }
546 }
454 spacecraft_potential[0] = v_ptr[2];
455 spacecraft_potential[1] = v_ptr[3];
456 spacecraft_potential[2] = e1_ptr[2];
457 spacecraft_potential[3] = e1_ptr[3];
458 spacecraft_potential[4] = e2_ptr[2];
459 spacecraft_potential[5] = e2_ptr[3];
547 460 }
548 461
549 462 void get_cpu_load( unsigned char *resource_statistics )
550 463 {
551 464 unsigned char cpu_load;
552 465
553 466 cpu_load = lfr_rtems_cpu_usage_report();
554 467
555 468 // HK_LFR_CPU_LOAD
556 469 resource_statistics[0] = cpu_load;
557 470
558 471 // HK_LFR_CPU_LOAD_MAX
559 472 if (cpu_load > resource_statistics[1])
560 473 {
561 474 resource_statistics[1] = cpu_load;
562 475 }
563 476
564 477 // CPU_LOAD_AVE
565 478 resource_statistics[2] = 0;
566 479
567 480 #ifndef PRINT_TASK_STATISTICS
568 481 rtems_cpu_usage_reset();
569 482 #endif
570 483
571 484 }
572 485
573 486
574 487
Show file before | Show file after | Hide comments
@@ -1,1103 +1,1113
1 1 /** Functions related to the SpaceWire interface.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle SpaceWire transmissions:
7 7 * - configuration of the SpaceWire link
8 8 * - SpaceWire related interruption requests processing
9 9 * - transmission of TeleMetry packets by a dedicated RTEMS task
10 10 * - reception of TeleCommands by a dedicated RTEMS task
11 11 *
12 12 */
13 13
14 14 #include "fsw_spacewire.h"
15 15
16 16 rtems_name semq_name;
17 17 rtems_id semq_id;
18 18
19 19 //*****************
20 20 // waveform headers
21 21 Header_TM_LFR_SCIENCE_CWF_t headerCWF;
22 22 Header_TM_LFR_SCIENCE_SWF_t headerSWF;
23 23 Header_TM_LFR_SCIENCE_ASM_t headerASM;
24 24
25 25 //***********
26 26 // RTEMS TASK
27 27 rtems_task spiq_task(rtems_task_argument unused)
28 28 {
29 29 /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver.
30 30 *
31 31 * @param unused is the starting argument of the RTEMS task
32 32 *
33 33 */
34 34
35 35 rtems_event_set event_out;
36 36 rtems_status_code status;
37 37 int linkStatus;
38 38
39 39 BOOT_PRINTF("in SPIQ *** \n")
40 40
41 41 while(true){
42 42 rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT
43 43 PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n")
44 44
45 45 // [0] SUSPEND RECV AND SEND TASKS
46 46 status = rtems_task_suspend( Task_id[ TASKID_RECV ] );
47 47 if ( status != RTEMS_SUCCESSFUL ) {
48 48 PRINTF("in SPIQ *** ERR suspending RECV Task\n")
49 49 }
50 50 status = rtems_task_suspend( Task_id[ TASKID_SEND ] );
51 51 if ( status != RTEMS_SUCCESSFUL ) {
52 52 PRINTF("in SPIQ *** ERR suspending SEND Task\n")
53 53 }
54 54
55 55 // [1] CHECK THE LINK
56 56 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1)
57 57 if ( linkStatus != 5) {
58 58 PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus)
59 59 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
60 60 }
61 61
62 62 // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT
63 63 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2)
64 64 if ( linkStatus != 5 ) // [2.a] not in run state, reset the link
65 65 {
66 66 spacewire_compute_stats_offsets();
67 67 status = spacewire_reset_link( );
68 68 }
69 69 else // [2.b] in run state, start the link
70 70 {
71 71 status = spacewire_stop_and_start_link( fdSPW ); // start the link
72 72 if ( status != RTEMS_SUCCESSFUL)
73 73 {
74 74 PRINTF1("in SPIQ *** ERR spacewire_start_link %d\n", status)
75 75 }
76 76 }
77 77
78 78 // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS
79 79 if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully
80 80 {
81 81 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
82 82 if ( status != RTEMS_SUCCESSFUL ) {
83 83 PRINTF("in SPIQ *** ERR resuming SEND Task\n")
84 84 }
85 85 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
86 86 if ( status != RTEMS_SUCCESSFUL ) {
87 87 PRINTF("in SPIQ *** ERR resuming RECV Task\n")
88 88 }
89 89 }
90 90 else // [3.b] the link is not in run state, go in STANDBY mode
91 91 {
92 92 status = stop_current_mode();
93 93 if ( status != RTEMS_SUCCESSFUL ) {
94 94 PRINTF1("in SPIQ *** ERR stop_current_mode *** code %d\n", status)
95 95 }
96 96 status = enter_mode( LFR_MODE_STANDBY, 0 );
97 97 if ( status != RTEMS_SUCCESSFUL ) {
98 98 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
99 99 }
100 100 // wake the WTDG task up to wait for the link recovery
101 101 status = rtems_event_send ( Task_id[TASKID_WTDG], RTEMS_EVENT_0 );
102 102 status = rtems_task_suspend( RTEMS_SELF );
103 103 }
104 104 }
105 105 }
106 106
107 107 rtems_task recv_task( rtems_task_argument unused )
108 108 {
109 109 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
110 110 *
111 111 * @param unused is the starting argument of the RTEMS task
112 112 *
113 113 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
114 114 * 1. It reads the incoming data.
115 115 * 2. Launches the acceptance procedure.
116 116 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
117 117 *
118 118 */
119 119
120 120 int len;
121 121 ccsdsTelecommandPacket_t currentTC;
122 122 unsigned char computed_CRC[ 2 ];
123 123 unsigned char currentTC_LEN_RCV[ 2 ];
124 124 unsigned char destinationID;
125 125 unsigned int estimatedPacketLength;
126 126 unsigned int parserCode;
127 127 rtems_status_code status;
128 128 rtems_id queue_recv_id;
129 129 rtems_id queue_send_id;
130 130
131 131 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
132 132
133 133 status = get_message_queue_id_recv( &queue_recv_id );
134 134 if (status != RTEMS_SUCCESSFUL)
135 135 {
136 136 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
137 137 }
138 138
139 139 status = get_message_queue_id_send( &queue_send_id );
140 140 if (status != RTEMS_SUCCESSFUL)
141 141 {
142 142 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
143 143 }
144 144
145 145 BOOT_PRINTF("in RECV *** \n")
146 146
147 147 while(1)
148 148 {
149 149 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
150 150 if (len == -1){ // error during the read call
151 151 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
152 152 }
153 153 else {
154 154 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
155 155 PRINTF("in RECV *** packet lenght too short\n")
156 156 }
157 157 else {
158 158 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // => -3 is for Prot ID, Reserved and User App bytes
159 159 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> 8);
160 160 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
161 161 // CHECK THE TC
162 162 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
163 163 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
164 164 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
165 165 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
166 166 || (parserCode == WRONG_SRC_ID) )
167 167 { // send TM_LFR_TC_EXE_CORRUPTED
168 168 PRINTF1("TC corrupted received, with code: %d\n", parserCode)
169 169 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
170 170 &&
171 171 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
172 172 )
173 173 {
174 174 if ( parserCode == WRONG_SRC_ID )
175 175 {
176 176 destinationID = SID_TC_GROUND;
177 177 }
178 178 else
179 179 {
180 180 destinationID = currentTC.sourceID;
181 181 }
182 182 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
183 183 computed_CRC, currentTC_LEN_RCV,
184 184 destinationID );
185 185 }
186 186 }
187 187 else
188 188 { // send valid TC to the action launcher
189 189 status = rtems_message_queue_send( queue_recv_id, &currentTC,
190 190 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + 3);
191 191 }
192 192 }
193 193 }
194 194 }
195 195 }
196 196
197 197 rtems_task send_task( rtems_task_argument argument)
198 198 {
199 199 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
200 200 *
201 201 * @param unused is the starting argument of the RTEMS task
202 202 *
203 203 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
204 204 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
205 205 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
206 206 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
207 207 * data it contains.
208 208 *
209 209 */
210 210
211 211 rtems_status_code status; // RTEMS status code
212 212 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
213 213 ring_node *incomingRingNodePtr;
214 214 int ring_node_address;
215 215 char *charPtr;
216 216 spw_ioctl_pkt_send *spw_ioctl_send;
217 217 size_t size; // size of the incoming TC packet
218 218 u_int32_t count;
219 219 rtems_id queue_id;
220 220 unsigned char sid;
221 221
222 222 incomingRingNodePtr = NULL;
223 223 ring_node_address = 0;
224 224 charPtr = (char *) &ring_node_address;
225 225 sid = 0;
226 226
227 227 init_header_cwf( &headerCWF );
228 228 init_header_swf( &headerSWF );
229 229 init_header_asm( &headerASM );
230 230
231 231 status = get_message_queue_id_send( &queue_id );
232 232 if (status != RTEMS_SUCCESSFUL)
233 233 {
234 234 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
235 235 }
236 236
237 237 BOOT_PRINTF("in SEND *** \n")
238 238
239 239 while(1)
240 240 {
241 241 status = rtems_message_queue_receive( queue_id, incomingData, &size,
242 242 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
243 243
244 244 if (status!=RTEMS_SUCCESSFUL)
245 245 {
246 246 PRINTF1("in SEND *** (1) ERR = %d\n", status)
247 247 }
248 248 else
249 249 {
250 250 if ( size == sizeof(ring_node*) )
251 251 {
252 252 charPtr[0] = incomingData[0];
253 253 charPtr[1] = incomingData[1];
254 254 charPtr[2] = incomingData[2];
255 255 charPtr[3] = incomingData[3];
256 256 incomingRingNodePtr = (ring_node*) ring_node_address;
257 257 sid = incomingRingNodePtr->sid;
258 258 // printf("sid = %d\n", incomingRingNodePtr->sid);
259 259 if ( (sid==SID_NORM_CWF_LONG_F3)
260 260 || (sid==SID_BURST_CWF_F2 )
261 261 || (sid==SID_SBM1_CWF_F1 )
262 262 || (sid==SID_SBM2_CWF_F2 ))
263 263 {
264 264 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
265 265 }
266 266 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
267 267 {
268 268 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
269 269 }
270 270 else if ( (sid==SID_NORM_CWF_F3) )
271 271 {
272 272 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
273 273 }
274 274 else if ( (sid==SID_NORM_ASM_F0) || (SID_NORM_ASM_F1) || (SID_NORM_ASM_F2) )
275 275 {
276 276 spw_send_asm( incomingRingNodePtr, &headerASM );
277 277 }
278 278 else
279 279 {
280 280 printf("unexpected sid = %d\n", sid);
281 281 }
282 282 }
283 283 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
284 284 {
285 285 status = write( fdSPW, incomingData, size );
286 286 if (status == -1){
287 287 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
288 288 }
289 289 }
290 290 else // the incoming message is a spw_ioctl_pkt_send structure
291 291 {
292 292 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
293 293 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
294 294 if (status == -1){
295 295 printf("size = %d, %x, %x, %x, %x, %x\n",
296 296 size,
297 297 incomingData[0],
298 298 incomingData[1],
299 299 incomingData[2],
300 300 incomingData[3],
301 301 incomingData[4]);
302 302 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
303 303 }
304 304 }
305 305 }
306 306
307 307 status = rtems_message_queue_get_number_pending( queue_id, &count );
308 308 if (status != RTEMS_SUCCESSFUL)
309 309 {
310 310 PRINTF1("in SEND *** (3) ERR = %d\n", status)
311 311 }
312 312 else
313 313 {
314 314 if (count > maxCount)
315 315 {
316 316 maxCount = count;
317 317 }
318 318 }
319 319 }
320 320 }
321 321
322 322 rtems_task wtdg_task( rtems_task_argument argument )
323 323 {
324 324 rtems_event_set event_out;
325 325 rtems_status_code status;
326 326 int linkStatus;
327 327
328 328 BOOT_PRINTF("in WTDG ***\n")
329 329
330 330 while(1)
331 331 {
332 332 // wait for an RTEMS_EVENT
333 333 rtems_event_receive( RTEMS_EVENT_0,
334 334 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
335 335 PRINTF("in WTDG *** wait for the link\n")
336 336 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
337 337 while( linkStatus != 5) // wait for the link
338 338 {
339 339 rtems_task_wake_after( 10 );
340 340 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
341 341 }
342 342
343 343 status = spacewire_stop_and_start_link( fdSPW );
344 344
345 345 if (status != RTEMS_SUCCESSFUL)
346 346 {
347 347 PRINTF1("in WTDG *** ERR link not started %d\n", status)
348 348 }
349 349 else
350 350 {
351 351 PRINTF("in WTDG *** OK link started\n")
352 352 }
353 353
354 354 // restart the SPIQ task
355 355 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
356 356 if ( status != RTEMS_SUCCESSFUL ) {
357 357 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
358 358 }
359 359
360 360 // restart RECV and SEND
361 361 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
362 362 if ( status != RTEMS_SUCCESSFUL ) {
363 363 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
364 364 }
365 365 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
366 366 if ( status != RTEMS_SUCCESSFUL ) {
367 367 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
368 368 }
369 369 }
370 370 }
371 371
372 372 //****************
373 373 // OTHER FUNCTIONS
374 374 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
375 375 {
376 376 /** This function opens the SpaceWire link.
377 377 *
378 378 * @return a valid file descriptor in case of success, -1 in case of a failure
379 379 *
380 380 */
381 381 rtems_status_code status;
382 382
383 383 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
384 384 if ( fdSPW < 0 ) {
385 385 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
386 386 }
387 387 else
388 388 {
389 389 status = RTEMS_SUCCESSFUL;
390 390 }
391 391
392 392 return status;
393 393 }
394 394
395 395 int spacewire_start_link( int fd )
396 396 {
397 397 rtems_status_code status;
398 398
399 399 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
400 400 // -1 default hardcoded driver timeout
401 401
402 402 return status;
403 403 }
404 404
405 405 int spacewire_stop_and_start_link( int fd )
406 406 {
407 407 rtems_status_code status;
408 408
409 409 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
410 410 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
411 411 // -1 default hardcoded driver timeout
412 412
413 413 return status;
414 414 }
415 415
416 416 int spacewire_configure_link( int fd )
417 417 {
418 418 /** This function configures the SpaceWire link.
419 419 *
420 420 * @return GR-RTEMS-DRIVER directive status codes:
421 421 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
422 422 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
423 423 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
424 424 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
425 425 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
426 426 * - 5 EIO - Error when writing to grswp hardware registers.
427 427 * - 2 ENOENT - No such file or directory
428 428 */
429 429
430 430 rtems_status_code status;
431 431
432 432 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
433 433 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
434 434
435 435 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
436 436 if (status!=RTEMS_SUCCESSFUL) {
437 437 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
438 438 }
439 439 //
440 440 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
441 441 if (status!=RTEMS_SUCCESSFUL) {
442 442 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
443 443 }
444 444 //
445 445 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
446 446 if (status!=RTEMS_SUCCESSFUL) {
447 447 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
448 448 }
449 449 //
450 450 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
451 451 if (status!=RTEMS_SUCCESSFUL) {
452 452 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
453 453 }
454 454 //
455 455 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
456 456 if (status!=RTEMS_SUCCESSFUL) {
457 457 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
458 458 }
459 459 //
460 460 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
461 461 if (status!=RTEMS_SUCCESSFUL) {
462 462 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
463 463 }
464 464 //
465 465 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, 0x0909); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
466 466 if (status!=RTEMS_SUCCESSFUL) {
467 467 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
468 468 }
469 469
470 470 return status;
471 471 }
472 472
473 473 int spacewire_reset_link( void )
474 474 {
475 475 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
476 476 *
477 477 * @return RTEMS directive status code:
478 478 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
479 479 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
480 480 *
481 481 */
482 482
483 483 rtems_status_code status_spw;
484 484 int i;
485 485
486 486 for ( i=0; i<SY_LFR_DPU_CONNECT_ATTEMPT; i++ )
487 487 {
488 488 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
489 489
490 490 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
491 491
492 492 status_spw = spacewire_stop_and_start_link( fdSPW );
493 493 if ( status_spw != RTEMS_SUCCESSFUL )
494 494 {
495 495 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw)
496 496 }
497 497
498 498 if ( status_spw == RTEMS_SUCCESSFUL)
499 499 {
500 500 break;
501 501 }
502 502 }
503 503
504 504 return status_spw;
505 505 }
506 506
507 507 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
508 508 {
509 509 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
510 510 *
511 511 * @param val is the value, 0 or 1, used to set the value of the NP bit.
512 512 * @param regAddr is the address of the GRSPW control register.
513 513 *
514 514 * NP is the bit 20 of the GRSPW control register.
515 515 *
516 516 */
517 517
518 518 unsigned int *spwptr = (unsigned int*) regAddr;
519 519
520 520 if (val == 1) {
521 521 *spwptr = *spwptr | 0x00100000; // [NP] set the No port force bit
522 522 }
523 523 if (val== 0) {
524 524 *spwptr = *spwptr & 0xffdfffff;
525 525 }
526 526 }
527 527
528 528 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
529 529 {
530 530 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
531 531 *
532 532 * @param val is the value, 0 or 1, used to set the value of the RE bit.
533 533 * @param regAddr is the address of the GRSPW control register.
534 534 *
535 535 * RE is the bit 16 of the GRSPW control register.
536 536 *
537 537 */
538 538
539 539 unsigned int *spwptr = (unsigned int*) regAddr;
540 540
541 541 if (val == 1)
542 542 {
543 543 *spwptr = *spwptr | 0x00010000; // [RE] set the RMAP Enable bit
544 544 }
545 545 if (val== 0)
546 546 {
547 547 *spwptr = *spwptr & 0xfffdffff;
548 548 }
549 549 }
550 550
551 551 void spacewire_compute_stats_offsets( void )
552 552 {
553 553 /** This function computes the SpaceWire statistics offsets in case of a SpaceWire related interruption raising.
554 554 *
555 555 * The offsets keep a record of the statistics in case of a reset of the statistics. They are added to the current statistics
556 556 * to keep the counters consistent even after a reset of the SpaceWire driver (the counter are set to zero by the driver when it
557 557 * during the open systel call).
558 558 *
559 559 */
560 560
561 561 spw_stats spacewire_stats_grspw;
562 562 rtems_status_code status;
563 563
564 564 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
565 565
566 566 spacewire_stats_backup.packets_received = spacewire_stats_grspw.packets_received
567 567 + spacewire_stats.packets_received;
568 568 spacewire_stats_backup.packets_sent = spacewire_stats_grspw.packets_sent
569 569 + spacewire_stats.packets_sent;
570 570 spacewire_stats_backup.parity_err = spacewire_stats_grspw.parity_err
571 571 + spacewire_stats.parity_err;
572 572 spacewire_stats_backup.disconnect_err = spacewire_stats_grspw.disconnect_err
573 573 + spacewire_stats.disconnect_err;
574 574 spacewire_stats_backup.escape_err = spacewire_stats_grspw.escape_err
575 575 + spacewire_stats.escape_err;
576 576 spacewire_stats_backup.credit_err = spacewire_stats_grspw.credit_err
577 577 + spacewire_stats.credit_err;
578 578 spacewire_stats_backup.write_sync_err = spacewire_stats_grspw.write_sync_err
579 579 + spacewire_stats.write_sync_err;
580 580 spacewire_stats_backup.rx_rmap_header_crc_err = spacewire_stats_grspw.rx_rmap_header_crc_err
581 581 + spacewire_stats.rx_rmap_header_crc_err;
582 582 spacewire_stats_backup.rx_rmap_data_crc_err = spacewire_stats_grspw.rx_rmap_data_crc_err
583 583 + spacewire_stats.rx_rmap_data_crc_err;
584 584 spacewire_stats_backup.early_ep = spacewire_stats_grspw.early_ep
585 585 + spacewire_stats.early_ep;
586 586 spacewire_stats_backup.invalid_address = spacewire_stats_grspw.invalid_address
587 587 + spacewire_stats.invalid_address;
588 588 spacewire_stats_backup.rx_eep_err = spacewire_stats_grspw.rx_eep_err
589 589 + spacewire_stats.rx_eep_err;
590 590 spacewire_stats_backup.rx_truncated = spacewire_stats_grspw.rx_truncated
591 591 + spacewire_stats.rx_truncated;
592 592 }
593 593
594 594 void spacewire_update_statistics( void )
595 595 {
596 596 rtems_status_code status;
597 597 spw_stats spacewire_stats_grspw;
598 598
599 599 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
600 600
601 601 spacewire_stats.packets_received = spacewire_stats_backup.packets_received
602 602 + spacewire_stats_grspw.packets_received;
603 603 spacewire_stats.packets_sent = spacewire_stats_backup.packets_sent
604 604 + spacewire_stats_grspw.packets_sent;
605 605 spacewire_stats.parity_err = spacewire_stats_backup.parity_err
606 606 + spacewire_stats_grspw.parity_err;
607 607 spacewire_stats.disconnect_err = spacewire_stats_backup.disconnect_err
608 608 + spacewire_stats_grspw.disconnect_err;
609 609 spacewire_stats.escape_err = spacewire_stats_backup.escape_err
610 610 + spacewire_stats_grspw.escape_err;
611 611 spacewire_stats.credit_err = spacewire_stats_backup.credit_err
612 612 + spacewire_stats_grspw.credit_err;
613 613 spacewire_stats.write_sync_err = spacewire_stats_backup.write_sync_err
614 614 + spacewire_stats_grspw.write_sync_err;
615 615 spacewire_stats.rx_rmap_header_crc_err = spacewire_stats_backup.rx_rmap_header_crc_err
616 616 + spacewire_stats_grspw.rx_rmap_header_crc_err;
617 617 spacewire_stats.rx_rmap_data_crc_err = spacewire_stats_backup.rx_rmap_data_crc_err
618 618 + spacewire_stats_grspw.rx_rmap_data_crc_err;
619 619 spacewire_stats.early_ep = spacewire_stats_backup.early_ep
620 620 + spacewire_stats_grspw.early_ep;
621 621 spacewire_stats.invalid_address = spacewire_stats_backup.invalid_address
622 622 + spacewire_stats_grspw.invalid_address;
623 623 spacewire_stats.rx_eep_err = spacewire_stats_backup.rx_eep_err
624 624 + spacewire_stats_grspw.rx_eep_err;
625 625 spacewire_stats.rx_truncated = spacewire_stats_backup.rx_truncated
626 626 + spacewire_stats_grspw.rx_truncated;
627 627 //spacewire_stats.tx_link_err;
628 628
629 629 //****************************
630 630 // DPU_SPACEWIRE_IF_STATISTICS
631 631 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (spacewire_stats.packets_received >> 8);
632 632 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (spacewire_stats.packets_received);
633 633 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (spacewire_stats.packets_sent >> 8);
634 634 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (spacewire_stats.packets_sent);
635 635 //housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt;
636 636 //housekeeping_packet.hk_lfr_dpu_spw_last_timc;
637 637
638 638 //******************************************
639 639 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
640 640 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) spacewire_stats.parity_err;
641 641 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) spacewire_stats.disconnect_err;
642 642 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) spacewire_stats.escape_err;
643 643 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) spacewire_stats.credit_err;
644 644 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) spacewire_stats.write_sync_err;
645 645
646 646 //*********************************************
647 647 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
648 648 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) spacewire_stats.early_ep;
649 649 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) spacewire_stats.invalid_address;
650 650 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) spacewire_stats.rx_eep_err;
651 651 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) spacewire_stats.rx_truncated;
652 652 }
653 653
654 654 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
655 655 {
656 656 // rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_9 );
657 657 struct grgpio_regs_str *grgpio_regs = (struct grgpio_regs_str *) REGS_ADDR_GRGPIO;
658 658
659 659 grgpio_regs->io_port_direction_register =
660 660 grgpio_regs->io_port_direction_register | 0x04; // [0000 0100], 0 = output disabled, 1 = output enabled
661 661
662 662 if ( (grgpio_regs->io_port_output_register & 0x04) == 0x04 )
663 663 {
664 664 grgpio_regs->io_port_output_register = grgpio_regs->io_port_output_register & 0xfb; // [1111 1011]
665 665 }
666 666 else
667 667 {
668 668 grgpio_regs->io_port_output_register = grgpio_regs->io_port_output_register | 0x04; // [0000 0100]
669 669 }
670 670 }
671 671
672 672 rtems_timer_service_routine user_routine( rtems_id timer_id, void *user_data )
673 673 {
674 674 int linkStatus;
675 675 rtems_status_code status;
676 676
677 677 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
678 678
679 679 if ( linkStatus == 5) {
680 680 PRINTF("in spacewire_reset_link *** link is running\n")
681 681 status = RTEMS_SUCCESSFUL;
682 682 }
683 683 }
684 684
685 685 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
686 686 {
687 687 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
688 688 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
689 689 header->reserved = DEFAULT_RESERVED;
690 690 header->userApplication = CCSDS_USER_APP;
691 691 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
692 692 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
693 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
694 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
693 header->packetLength[0] = 0x00;
694 header->packetLength[1] = 0x00;
695 695 // DATA FIELD HEADER
696 696 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
697 697 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
698 698 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
699 699 header->destinationID = TM_DESTINATION_ID_GROUND;
700 700 header->time[0] = 0x00;
701 701 header->time[0] = 0x00;
702 702 header->time[0] = 0x00;
703 703 header->time[0] = 0x00;
704 704 header->time[0] = 0x00;
705 705 header->time[0] = 0x00;
706 706 // AUXILIARY DATA HEADER
707 707 header->sid = 0x00;
708 708 header->hkBIA = DEFAULT_HKBIA;
709 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
710 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
709 header->blkNr[0] = 0x00;
710 header->blkNr[1] = 0x00;
711 711 }
712 712
713 713 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
714 714 {
715 715 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
716 716 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
717 717 header->reserved = DEFAULT_RESERVED;
718 718 header->userApplication = CCSDS_USER_APP;
719 719 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
720 720 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
721 721 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
722 722 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
723 723 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
724 724 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
725 725 // DATA FIELD HEADER
726 726 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
727 727 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
728 728 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
729 729 header->destinationID = TM_DESTINATION_ID_GROUND;
730 730 header->time[0] = 0x00;
731 731 header->time[0] = 0x00;
732 732 header->time[0] = 0x00;
733 733 header->time[0] = 0x00;
734 734 header->time[0] = 0x00;
735 735 header->time[0] = 0x00;
736 736 // AUXILIARY DATA HEADER
737 737 header->sid = 0x00;
738 738 header->hkBIA = DEFAULT_HKBIA;
739 739 header->pktCnt = DEFAULT_PKTCNT; // PKT_CNT
740 740 header->pktNr = 0x00;
741 741 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
742 742 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
743 743 }
744 744
745 745 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
746 746 {
747 747 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
748 748 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
749 749 header->reserved = DEFAULT_RESERVED;
750 750 header->userApplication = CCSDS_USER_APP;
751 751 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
752 752 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
753 753 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
754 754 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
755 755 header->packetLength[0] = 0x00;
756 756 header->packetLength[1] = 0x00;
757 757 // DATA FIELD HEADER
758 758 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
759 759 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
760 760 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
761 761 header->destinationID = TM_DESTINATION_ID_GROUND;
762 762 header->time[0] = 0x00;
763 763 header->time[0] = 0x00;
764 764 header->time[0] = 0x00;
765 765 header->time[0] = 0x00;
766 766 header->time[0] = 0x00;
767 767 header->time[0] = 0x00;
768 768 // AUXILIARY DATA HEADER
769 769 header->sid = 0x00;
770 770 header->biaStatusInfo = 0x00;
771 771 header->pa_lfr_pkt_cnt_asm = 0x00;
772 772 header->pa_lfr_pkt_nr_asm = 0x00;
773 773 header->pa_lfr_asm_blk_nr[0] = 0x00;
774 774 header->pa_lfr_asm_blk_nr[1] = 0x00;
775 775 }
776 776
777 777 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
778 778 Header_TM_LFR_SCIENCE_CWF_t *header )
779 779 {
780 780 /** This function sends CWF CCSDS packets (F2, F1 or F0).
781 781 *
782 782 * @param waveform points to the buffer containing the data that will be send.
783 783 * @param sid is the source identifier of the data that will be sent.
784 784 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
785 785 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
786 786 * contain information to setup the transmission of the data packets.
787 787 *
788 788 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
789 789 *
790 790 */
791 791
792 792 unsigned int i;
793 793 int ret;
794 794 unsigned int coarseTime;
795 795 unsigned int fineTime;
796 796 rtems_status_code status;
797 797 spw_ioctl_pkt_send spw_ioctl_send_CWF;
798 798 int *dataPtr;
799 799 unsigned char sid;
800 800
801 801 spw_ioctl_send_CWF.hlen = TM_HEADER_LEN + 4 + 10; // + 4 is for the protocole extra header, + 10 is for the auxiliary header
802 802 spw_ioctl_send_CWF.options = 0;
803 803
804 804 ret = LFR_DEFAULT;
805 805 sid = (unsigned char) ring_node_to_send->sid;
806 806
807 807 coarseTime = ring_node_to_send->coarseTime;
808 808 fineTime = ring_node_to_send->fineTime;
809 809 dataPtr = (int*) ring_node_to_send->buffer_address;
810 810
811 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
812 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
813 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
814 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
815
811 816 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
812 817 {
813 818 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
814 819 spw_ioctl_send_CWF.hdr = (char*) header;
815 820 // BUILD THE DATA
816 821 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
817 822
818 823 // SET PACKET SEQUENCE CONTROL
819 824 increment_seq_counter_source_id( header->packetSequenceControl, sid );
820 825
821 826 // SET SID
822 827 header->sid = sid;
823 828
824 829 // SET PACKET TIME
825 830 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
826 831 //
827 832 header->time[0] = header->acquisitionTime[0];
828 833 header->time[1] = header->acquisitionTime[1];
829 834 header->time[2] = header->acquisitionTime[2];
830 835 header->time[3] = header->acquisitionTime[3];
831 836 header->time[4] = header->acquisitionTime[4];
832 837 header->time[5] = header->acquisitionTime[5];
833 838
834 839 // SET PACKET ID
835 840 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
836 841 {
837 842 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8);
838 843 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
839 844 }
840 845 else
841 846 {
842 847 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
843 848 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
844 849 }
845 850
846 851 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
847 852 if (status != RTEMS_SUCCESSFUL) {
848 853 printf("%d-%d, ERR %d\n", sid, i, (int) status);
849 854 ret = LFR_DEFAULT;
850 855 }
851 856 }
852 857
853 858 return ret;
854 859 }
855 860
856 861 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
857 862 Header_TM_LFR_SCIENCE_SWF_t *header )
858 863 {
859 864 /** This function sends SWF CCSDS packets (F2, F1 or F0).
860 865 *
861 866 * @param waveform points to the buffer containing the data that will be send.
862 867 * @param sid is the source identifier of the data that will be sent.
863 868 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
864 869 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
865 870 * contain information to setup the transmission of the data packets.
866 871 *
867 872 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
868 873 *
869 874 */
870 875
871 876 unsigned int i;
872 877 int ret;
873 878 unsigned int coarseTime;
874 879 unsigned int fineTime;
875 880 rtems_status_code status;
876 881 spw_ioctl_pkt_send spw_ioctl_send_SWF;
877 882 int *dataPtr;
878 883 unsigned char sid;
879 884
880 885 spw_ioctl_send_SWF.hlen = TM_HEADER_LEN + 4 + 12; // + 4 is for the protocole extra header, + 12 is for the auxiliary header
881 886 spw_ioctl_send_SWF.options = 0;
882 887
883 888 ret = LFR_DEFAULT;
884 889
885 890 coarseTime = ring_node_to_send->coarseTime;
886 891 fineTime = ring_node_to_send->fineTime;
887 892 dataPtr = (int*) ring_node_to_send->buffer_address;
888 893 sid = ring_node_to_send->sid;
889 894
890 895 for (i=0; i<7; i++) // send waveform
891 896 {
892 897 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
893 898 spw_ioctl_send_SWF.hdr = (char*) header;
894 899
895 900 // SET PACKET SEQUENCE CONTROL
896 901 increment_seq_counter_source_id( header->packetSequenceControl, sid );
897 902
898 903 // SET PACKET LENGTH AND BLKNR
899 904 if (i == 6)
900 905 {
901 906 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
902 907 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8);
903 908 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
904 909 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> 8);
905 910 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
906 911 }
907 912 else
908 913 {
909 914 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
910 915 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8);
911 916 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
912 917 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> 8);
913 918 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
914 919 }
915 920
916 921 // SET PACKET TIME
917 922 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
918 923 //
919 924 header->time[0] = header->acquisitionTime[0];
920 925 header->time[1] = header->acquisitionTime[1];
921 926 header->time[2] = header->acquisitionTime[2];
922 927 header->time[3] = header->acquisitionTime[3];
923 928 header->time[4] = header->acquisitionTime[4];
924 929 header->time[5] = header->acquisitionTime[5];
925 930
926 931 // SET SID
927 932 header->sid = sid;
928 933
929 934 // SET PKTNR
930 935 header->pktNr = i+1; // PKT_NR
931 936
932 937 // SEND PACKET
933 938 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
934 939 if (status != RTEMS_SUCCESSFUL) {
935 940 printf("%d-%d, ERR %d\n", sid, i, (int) status);
936 941 ret = LFR_DEFAULT;
937 942 }
938 943 }
939 944
940 945 return ret;
941 946 }
942 947
943 948 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
944 949 Header_TM_LFR_SCIENCE_CWF_t *header )
945 950 {
946 951 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
947 952 *
948 953 * @param waveform points to the buffer containing the data that will be send.
949 954 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
950 955 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
951 956 * contain information to setup the transmission of the data packets.
952 957 *
953 958 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
954 959 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
955 960 *
956 961 */
957 962
958 963 unsigned int i;
959 964 int ret;
960 965 unsigned int coarseTime;
961 966 unsigned int fineTime;
962 967 rtems_status_code status;
963 968 spw_ioctl_pkt_send spw_ioctl_send_CWF;
964 969 char *dataPtr;
965 970 unsigned char sid;
966 971
967 972 spw_ioctl_send_CWF.hlen = TM_HEADER_LEN + 4 + 10; // + 4 is for the protocole extra header, + 10 is for the auxiliary header
968 973 spw_ioctl_send_CWF.options = 0;
969 974
970 975 ret = LFR_DEFAULT;
971 976 sid = ring_node_to_send->sid;
972 977
973 978 coarseTime = ring_node_to_send->coarseTime;
974 979 fineTime = ring_node_to_send->fineTime;
975 980 dataPtr = (char*) ring_node_to_send->buffer_address;
976 981
982 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> 8);
983 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
984 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> 8);
985 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
986
977 987 //*********************
978 988 // SEND CWF3_light DATA
979 989 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
980 990 {
981 991 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
982 992 spw_ioctl_send_CWF.hdr = (char*) header;
983 993 // BUILD THE DATA
984 994 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
985 995
986 996 // SET PACKET SEQUENCE COUNTER
987 997 increment_seq_counter_source_id( header->packetSequenceControl, sid );
988 998
989 999 // SET SID
990 1000 header->sid = sid;
991 1001
992 1002 // SET PACKET TIME
993 1003 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
994 1004 //
995 1005 header->time[0] = header->acquisitionTime[0];
996 1006 header->time[1] = header->acquisitionTime[1];
997 1007 header->time[2] = header->acquisitionTime[2];
998 1008 header->time[3] = header->acquisitionTime[3];
999 1009 header->time[4] = header->acquisitionTime[4];
1000 1010 header->time[5] = header->acquisitionTime[5];
1001 1011
1002 1012 // SET PACKET ID
1003 1013 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1004 1014 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1005 1015
1006 1016 // SEND PACKET
1007 1017 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1008 1018 if (status != RTEMS_SUCCESSFUL) {
1009 1019 printf("%d-%d, ERR %d\n", sid, i, (int) status);
1010 1020 ret = LFR_DEFAULT;
1011 1021 }
1012 1022 }
1013 1023
1014 1024 return ret;
1015 1025 }
1016 1026
1017 1027 void spw_send_asm( ring_node *ring_node_to_send,
1018 1028 Header_TM_LFR_SCIENCE_ASM_t *header )
1019 1029 {
1020 1030 unsigned int i;
1021 1031 unsigned int length = 0;
1022 1032 rtems_status_code status;
1023 1033 unsigned int sid;
1024 1034 char *spectral_matrix;
1025 1035 int coarseTime;
1026 1036 int fineTime;
1027 1037 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1028 1038
1029 1039 sid = ring_node_to_send->sid;
1030 1040 spectral_matrix = (char*) ring_node_to_send->buffer_address;
1031 1041 coarseTime = ring_node_to_send->coarseTime;
1032 1042 fineTime = ring_node_to_send->fineTime;
1033 1043
1034 1044 for (i=0; i<2; i++)
1035 1045 {
1036 1046 // (1) BUILD THE DATA
1037 1047 switch(sid)
1038 1048 {
1039 1049 case SID_NORM_ASM_F0:
1040 1050 spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F0_IN_BYTES / 2; // 2 packets will be sent
1041 1051 spw_ioctl_send_ASM.data = &spectral_matrix[
1042 1052 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0) ) * NB_VALUES_PER_SM ) * 2
1043 1053 ];
1044 1054 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0;
1045 1055 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0) >> 8 ); // BLK_NR MSB
1046 1056 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0); // BLK_NR LSB
1047 1057 break;
1048 1058 case SID_NORM_ASM_F1:
1049 1059 spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F1_IN_BYTES / 2; // 2 packets will be sent
1050 1060 spw_ioctl_send_ASM.data = &spectral_matrix[
1051 1061 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1) ) * NB_VALUES_PER_SM ) * 2
1052 1062 ];
1053 1063 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1;
1054 1064 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1) >> 8 ); // BLK_NR MSB
1055 1065 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1); // BLK_NR LSB
1056 1066 break;
1057 1067 case SID_NORM_ASM_F2:
1058 1068 spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F2_IN_BYTES / 2; // 2 packets will be sent
1059 1069 spw_ioctl_send_ASM.data = &spectral_matrix[
1060 1070 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM ) * 2
1061 1071 ];
1062 1072 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1063 1073 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB
1064 1074 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1065 1075 break;
1066 1076 default:
1067 1077 PRINTF1("ERR *** in spw_send_asm *** unexpected sid %d\n", sid)
1068 1078 break;
1069 1079 }
1070 1080 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM + CCSDS_PROTOCOLE_EXTRA_BYTES;
1071 1081 spw_ioctl_send_ASM.hdr = (char *) header;
1072 1082 spw_ioctl_send_ASM.options = 0;
1073 1083
1074 1084 // (2) BUILD THE HEADER
1075 1085 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1076 1086 header->packetLength[0] = (unsigned char) (length>>8);
1077 1087 header->packetLength[1] = (unsigned char) (length);
1078 1088 header->sid = (unsigned char) sid; // SID
1079 1089 header->pa_lfr_pkt_cnt_asm = 2;
1080 1090 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1081 1091
1082 1092 // (3) SET PACKET TIME
1083 1093 header->time[0] = (unsigned char) (coarseTime>>24);
1084 1094 header->time[1] = (unsigned char) (coarseTime>>16);
1085 1095 header->time[2] = (unsigned char) (coarseTime>>8);
1086 1096 header->time[3] = (unsigned char) (coarseTime);
1087 1097 header->time[4] = (unsigned char) (fineTime>>8);
1088 1098 header->time[5] = (unsigned char) (fineTime);
1089 1099 //
1090 1100 header->acquisitionTime[0] = header->time[0];
1091 1101 header->acquisitionTime[1] = header->time[1];
1092 1102 header->acquisitionTime[2] = header->time[2];
1093 1103 header->acquisitionTime[3] = header->time[3];
1094 1104 header->acquisitionTime[4] = header->time[4];
1095 1105 header->acquisitionTime[5] = header->time[5];
1096 1106
1097 1107 // (4) SEND PACKET
1098 1108 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1099 1109 if (status != RTEMS_SUCCESSFUL) {
1100 1110 printf("in ASM_send *** ERR %d\n", (int) status);
1101 1111 }
1102 1112 }
1103 1113 }
Show file before | Show file after | Hide comments
@@ -1,387 +1,498
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf0_prc0.h"
11 11 #include "fsw_processing.h"
12 12
13 13 nb_sm_before_bp_asm_f0 nb_sm_before_f0;
14 14
15 15 //***
16 16 // F0
17 17 ring_node_asm asm_ring_norm_f0 [ NB_RING_NODES_ASM_NORM_F0 ];
18 18 ring_node_asm asm_ring_burst_sbm_f0 [ NB_RING_NODES_ASM_BURST_SBM_F0 ];
19 19
20 20 ring_node ring_to_send_asm_f0 [ NB_RING_NODES_ASM_F0 ];
21 21 int buffer_asm_f0 [ NB_RING_NODES_ASM_F0 * TOTAL_SIZE_SM ];
22 22
23 23 float asm_f0_reorganized [ TOTAL_SIZE_SM ];
24 24 char asm_f0_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ];
25 25 float compressed_sm_norm_f0[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F0];
26 26 float compressed_sm_sbm_f0 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F0 ];
27 //unsigned char bp1_norm_f0 [ TOTAL_SIZE_BP1_NORM_F0 ];
28 //unsigned char bp1_sbm_f0 [ TOTAL_SIZE_BP1_SBM_F0 ];
27
28 float k_coeff_intercalib_f0_norm[ NB_BINS_COMPRESSED_SM_F0 * NB_K_COEFF_PER_BIN ]; // 11 * 32 = 352
29 float k_coeff_intercalib_f0_sbm[ NB_BINS_COMPRESSED_SM_SBM_F0 * NB_K_COEFF_PER_BIN ]; // 22 * 32 = 704
29 30
30 31 //************
31 32 // RTEMS TASKS
32 33
33 34 rtems_task avf0_task( rtems_task_argument lfrRequestedMode )
34 35 {
35 36 int i;
36 37
37 38 rtems_event_set event_out;
38 39 rtems_status_code status;
39 40 rtems_id queue_id_prc0;
40 41 asm_msg msgForMATR;
42 ring_node *nodeForAveraging;
41 43 ring_node *ring_node_tab[8];
42 44 ring_node_asm *current_ring_node_asm_burst_sbm_f0;
43 45 ring_node_asm *current_ring_node_asm_norm_f0;
44 46
45 47 unsigned int nb_norm_bp1;
46 48 unsigned int nb_norm_bp2;
47 49 unsigned int nb_norm_asm;
48 50 unsigned int nb_sbm_bp1;
49 51 unsigned int nb_sbm_bp2;
50 52
51 53 nb_norm_bp1 = 0;
52 54 nb_norm_bp2 = 0;
53 55 nb_norm_asm = 0;
54 56 nb_sbm_bp1 = 0;
55 57 nb_sbm_bp2 = 0;
56 58
57 59 reset_nb_sm_f0( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
58 60 ASM_generic_init_ring( asm_ring_norm_f0, NB_RING_NODES_ASM_NORM_F0 );
59 61 ASM_generic_init_ring( asm_ring_burst_sbm_f0, NB_RING_NODES_ASM_BURST_SBM_F0 );
60 62 current_ring_node_asm_norm_f0 = asm_ring_norm_f0;
61 63 current_ring_node_asm_burst_sbm_f0 = asm_ring_burst_sbm_f0;
62 64
63 65 BOOT_PRINTF1("in AVFO *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
64 66
65 67 status = get_message_queue_id_prc0( &queue_id_prc0 );
66 68 if (status != RTEMS_SUCCESSFUL)
67 69 {
68 70 PRINTF1("in MATR *** ERR get_message_queue_id_prc0 %d\n", status)
69 71 }
70 72
71 73 while(1){
72 74 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
73 75
74 76 //****************************************
75 77 // initialize the mesage for the MATR task
76 78 msgForMATR.norm = current_ring_node_asm_norm_f0;
77 79 msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f0;
78 80 msgForMATR.event = 0x00; // this composite event will be sent to the PRC0 task
79 msgForMATR.coarseTime = ring_node_for_averaging_sm_f0->coarseTime;
80 msgForMATR.fineTime = ring_node_for_averaging_sm_f0->fineTime;
81 81 //
82 82 //****************************************
83 83
84 ring_node_tab[NB_SM_BEFORE_AVF0-1] = ring_node_for_averaging_sm_f0;
84 nodeForAveraging = getRingNodeForAveraging( 0 );
85
86 ring_node_tab[NB_SM_BEFORE_AVF0-1] = nodeForAveraging;
85 87 for ( i = 2; i < (NB_SM_BEFORE_AVF0+1); i++ )
86 88 {
87 ring_node_for_averaging_sm_f0 = ring_node_for_averaging_sm_f0->previous;
88 ring_node_tab[NB_SM_BEFORE_AVF0-i] = ring_node_for_averaging_sm_f0;
89 nodeForAveraging = nodeForAveraging->previous;
90 ring_node_tab[NB_SM_BEFORE_AVF0-i] = nodeForAveraging;
89 91 }
90 92
91 93 // compute the average and store it in the averaged_sm_f1 buffer
92 94 SM_average( current_ring_node_asm_norm_f0->matrix,
93 95 current_ring_node_asm_burst_sbm_f0->matrix,
94 96 ring_node_tab,
95 nb_norm_bp1, nb_sbm_bp1 );
97 nb_norm_bp1, nb_sbm_bp1,
98 &msgForMATR );
96 99
97 100 // update nb_average
98 101 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0;
99 102 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0;
100 103 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0;
101 104 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0;
102 105 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0;
103 106
104 107 if (nb_sbm_bp1 == nb_sm_before_f0.burst_sbm_bp1)
105 108 {
106 109 nb_sbm_bp1 = 0;
107 110 // set another ring for the ASM storage
108 111 current_ring_node_asm_burst_sbm_f0 = current_ring_node_asm_burst_sbm_f0->next;
109 112 if ( lfrCurrentMode == LFR_MODE_BURST )
110 113 {
111 114 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP1_F0;
112 115 }
113 116 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
114 117 {
115 118 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP1_F0;
116 119 }
117 120 }
118 121
119 122 if (nb_sbm_bp2 == nb_sm_before_f0.burst_sbm_bp2)
120 123 {
121 124 nb_sbm_bp2 = 0;
122 125 if ( lfrCurrentMode == LFR_MODE_BURST )
123 126 {
124 127 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP2_F0;
125 128 }
126 129 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
127 130 {
128 131 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP2_F0;
129 132 }
130 133 }
131 134
132 135 if (nb_norm_bp1 == nb_sm_before_f0.norm_bp1)
133 136 {
134 137 nb_norm_bp1 = 0;
135 138 // set another ring for the ASM storage
136 139 current_ring_node_asm_norm_f0 = current_ring_node_asm_norm_f0->next;
137 140 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
138 141 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
139 142 {
140 143 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F0;
141 144 }
142 145 }
143 146
144 147 if (nb_norm_bp2 == nb_sm_before_f0.norm_bp2)
145 148 {
146 149 nb_norm_bp2 = 0;
147 150 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
148 151 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
149 152 {
150 153 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F0;
151 154 }
152 155 }
153 156
154 157 if (nb_norm_asm == nb_sm_before_f0.norm_asm)
155 158 {
156 159 nb_norm_asm = 0;
157 160 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
158 161 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
159 162 {
160 // PRINTF1("%lld\n", localTime)
161 163 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F0;
162 164 }
163 165 }
164 166
165 167 //*************************
166 168 // send the message to MATR
167 169 if (msgForMATR.event != 0x00)
168 170 {
169 171 status = rtems_message_queue_send( queue_id_prc0, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC0);
170 172 }
171 173
172 174 if (status != RTEMS_SUCCESSFUL) {
173 175 printf("in AVF0 *** Error sending message to MATR, code %d\n", status);
174 176 }
175 177 }
176 178 }
177 179
178 180 rtems_task prc0_task( rtems_task_argument lfrRequestedMode )
179 181 {
180 182 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
181 183 size_t size; // size of the incoming TC packet
182 184 asm_msg *incomingMsg;
183 185 //
184 186 unsigned char sid;
185 187 rtems_status_code status;
186 188 rtems_id queue_id;
187 189 rtems_id queue_id_q_p0;
188 bp_packet_with_spare packet_norm_bp1_f0;
189 bp_packet packet_norm_bp2_f0;
190 bp_packet packet_sbm_bp1_f0;
191 bp_packet packet_sbm_bp2_f0;
190 bp_packet_with_spare packet_norm_bp1;
191 bp_packet packet_norm_bp2;
192 bp_packet packet_sbm_bp1;
193 bp_packet packet_sbm_bp2;
192 194 ring_node *current_ring_node_to_send_asm_f0;
193 195
194 196 unsigned long long int localTime;
195 197
196 198 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
197 199 init_ring( ring_to_send_asm_f0, NB_RING_NODES_ASM_F0, (volatile int*) buffer_asm_f0, TOTAL_SIZE_SM );
198 200 current_ring_node_to_send_asm_f0 = ring_to_send_asm_f0;
199 201
200 202 //*************
201 203 // NORM headers
202 BP_init_header_with_spare( &packet_norm_bp1_f0.header,
204 BP_init_header_with_spare( &packet_norm_bp1.header,
203 205 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F0,
204 206 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0, NB_BINS_COMPRESSED_SM_F0 );
205 BP_init_header( &packet_norm_bp2_f0.header,
207 BP_init_header( &packet_norm_bp2,
206 208 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F0,
207 209 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0, NB_BINS_COMPRESSED_SM_F0);
208 210
209 211 //****************************
210 212 // BURST SBM1 and SBM2 headers
211 213 if ( lfrRequestedMode == LFR_MODE_BURST )
212 214 {
213 BP_init_header( &packet_sbm_bp1_f0.header,
215 BP_init_header( &packet_sbm_bp1,
214 216 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F0,
215 217 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
216 BP_init_header( &packet_sbm_bp2_f0.header,
218 BP_init_header( &packet_sbm_bp2,
217 219 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F0,
218 220 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
219 221 }
220 222 else if ( lfrRequestedMode == LFR_MODE_SBM1 )
221 223 {
222 BP_init_header( &packet_sbm_bp1_f0.header,
224 BP_init_header( &packet_sbm_bp1,
223 225 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP1_F0,
224 226 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
225 BP_init_header( &packet_sbm_bp2_f0.header,
227 BP_init_header( &packet_sbm_bp2,
226 228 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP2_F0,
227 229 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
228 230 }
229 231 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
230 232 {
231 BP_init_header( &packet_sbm_bp1_f0.header,
233 BP_init_header( &packet_sbm_bp1,
232 234 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F0,
233 235 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
234 BP_init_header( &packet_sbm_bp2_f0.header,
236 BP_init_header( &packet_sbm_bp2,
235 237 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F0,
236 238 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
237 239 }
238 240 else
239 241 {
240 242 PRINTF1("in PRC0 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
241 243 }
242 244
243 245 status = get_message_queue_id_send( &queue_id );
244 246 if (status != RTEMS_SUCCESSFUL)
245 247 {
246 248 PRINTF1("in PRC0 *** ERR get_message_queue_id_send %d\n", status)
247 249 }
248 250 status = get_message_queue_id_prc0( &queue_id_q_p0);
249 251 if (status != RTEMS_SUCCESSFUL)
250 252 {
251 253 PRINTF1("in PRC0 *** ERR get_message_queue_id_prc0 %d\n", status)
252 254 }
253 255
254 256 BOOT_PRINTF1("in PRC0 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
255 257
256 258 while(1){
257 259 status = rtems_message_queue_receive( queue_id_q_p0, incomingData, &size, //************************************
258 260 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
259 261
260 262 incomingMsg = (asm_msg*) incomingData;
261 263
262 264 localTime = getTimeAsUnsignedLongLongInt( );
263 265
264 266 //****************
265 267 //****************
266 268 // BURST SBM1 SBM2
267 269 //****************
268 270 //****************
269 271 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F0 ) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F0 ) )
270 272 {
271 273 sid = getSID( incomingMsg->event );
272 274 // 1) compress the matrix for Basic Parameters calculation
273 275 ASM_compress_reorganize_and_divide( incomingMsg->burst_sbm->matrix, compressed_sm_sbm_f0,
274 276 nb_sm_before_f0.burst_sbm_bp1,
275 277 NB_BINS_COMPRESSED_SM_SBM_F0, NB_BINS_TO_AVERAGE_ASM_SBM_F0,
276 278 ASM_F0_INDICE_START);
277 279 // 2) compute the BP1 set
278 // BP1_set( compressed_sm_norm_f0, NB_BINS_COMPRESSED_SM_SBM_F0, bp1_sbm_f0 );
280 BP1_set( compressed_sm_sbm_f0, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp1.data );
279 281 // 3) send the BP1 set
280 set_time( packet_sbm_bp1_f0.header.time, (unsigned char *) &incomingMsg->coarseTime );
281 set_time( packet_sbm_bp1_f0.header.acquisitionTime, (unsigned char *) &incomingMsg->coarseTime );
282 BP_send( (char *) &packet_sbm_bp1_f0, queue_id,
282 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
283 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
284 BP_send( (char *) &packet_sbm_bp1, queue_id,
283 285 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0 + PACKET_LENGTH_DELTA,
284 286 sid);
285 287 // 4) compute the BP2 set if needed
286 288 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F0) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F0) )
287 289 {
288 290 // 1) compute the BP2 set
289
291 BP2_set( compressed_sm_sbm_f0, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp2.data );
290 292 // 2) send the BP2 set
291 set_time( packet_sbm_bp2_f0.header.time, (unsigned char *) &incomingMsg->coarseTime );
292 set_time( packet_sbm_bp2_f0.header.acquisitionTime, (unsigned char *) &incomingMsg->coarseTime );
293 BP_send( (char *) &packet_sbm_bp2_f0, queue_id,
293 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
294 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
295 BP_send( (char *) &packet_sbm_bp2, queue_id,
294 296 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0 + PACKET_LENGTH_DELTA,
295 297 sid);
296 298 }
297 299 }
298 300
299 301 //*****
300 302 //*****
301 303 // NORM
302 304 //*****
303 305 //*****
304 306 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F0)
305 307 {
306 308 // 1) compress the matrix for Basic Parameters calculation
307 309 ASM_compress_reorganize_and_divide( incomingMsg->norm->matrix, compressed_sm_norm_f0,
308 310 nb_sm_before_f0.norm_bp1,
309 311 NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0,
310 312 ASM_F0_INDICE_START );
311 313 // 2) compute the BP1 set
312 // BP1_set( compressed_sm_norm_f0, NB_BINS_COMPRESSED_SM_F0, bp1_norm_f0 );
314 BP1_set( compressed_sm_norm_f0, k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp1.data );
313 315 // 3) send the BP1 set
314 set_time( packet_norm_bp1_f0.header.time, (unsigned char *) &incomingMsg->coarseTime );
315 set_time( packet_norm_bp1_f0.header.acquisitionTime, (unsigned char *) &incomingMsg->coarseTime );
316 BP_send( (char *) &packet_norm_bp1_f0, queue_id,
316 set_time( packet_norm_bp1.header.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
317 set_time( packet_norm_bp1.header.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
318 BP_send( (char *) &packet_norm_bp1, queue_id,
317 319 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0 + PACKET_LENGTH_DELTA,
318 320 SID_NORM_BP1_F0 );
319 321 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F0)
320 322 {
321 323 // 1) compute the BP2 set using the same ASM as the one used for BP1
322
324 BP2_set( compressed_sm_norm_f0, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp2.data );
323 325 // 2) send the BP2 set
324 set_time( packet_norm_bp2_f0.header.time, (unsigned char *) &incomingMsg->coarseTime );
325 set_time( packet_norm_bp2_f0.header.acquisitionTime, (unsigned char *) &incomingMsg->coarseTime );
326 BP_send( (char *) &packet_norm_bp2_f0, queue_id,
326 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
327 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
328 BP_send( (char *) &packet_norm_bp2, queue_id,
327 329 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0 + PACKET_LENGTH_DELTA,
328 330 SID_NORM_BP2_F0);
331
332 // < TMP DATA>
333 #define INDEX_COMPRESSED 1
334 unsigned int signif;
335 float significand;
336 unsigned int nbitexp = 6;
337 unsigned int nbitsig = 16 - nbitexp; // number of bits for the significand
338 unsigned int rangesig = (1 << nbitsig)-1; // == 2^nbitsig - 1
339 int expmax = 32;
340 int expmin = expmax - ((int) (1 << nbitexp)) + 1;
341 int exponent;
342 float auto_a0;
343 exponent = ( (int) ( (packet_norm_bp2.data[INDEX_COMPRESSED * NB_BYTES_PER_BP2] & 0xfc) >> 2) ) + expmin; // [1111 1100]
344 printf("exponent = %x, computed with exp = %x, expmin = %d\n",
345 exponent,
346 (packet_norm_bp2.data[INDEX_COMPRESSED * NB_BYTES_PER_BP2] & 0xfc) >> 2,
347 expmin);
348 signif = ( (packet_norm_bp2.data[INDEX_COMPRESSED * NB_BYTES_PER_BP2] & 0x3) << 8 ) + packet_norm_bp2.data[INDEX_COMPRESSED * NB_BYTES_PER_BP2+1];
349 significand = ( ( (float) signif ) / ( (float) rangesig) + 1) / 2;
350 auto_a0 = significand * pow(2,exponent);
351 printf("(BP2) [%d] compressed = %f *** AUTO A0 = %x, %x, exponent = %x, significand = %f ===> %f\n",
352 INDEX_COMPRESSED,
353 compressed_sm_norm_f0[INDEX_COMPRESSED * NB_VALUES_PER_SM],
354 packet_norm_bp2.data[ INDEX_COMPRESSED * NB_BYTES_PER_BP2],
355 packet_norm_bp2.data[ INDEX_COMPRESSED * NB_BYTES_PER_BP2 + 1],
356 exponent, significand, auto_a0 );
357 // printf("(BP2) 0 = %f, 1 = %f, 2 = %f, 3 = %f, 4 = %f, 5 = %f, 6 = %f, 7 = %f, 8 = %f, 9 = %f, 10 = %f,\n",
358 // compressed_sm_norm_f0[0 * NB_VALUES_PER_SM],
359 // compressed_sm_norm_f0[1 * NB_VALUES_PER_SM],
360 // compressed_sm_norm_f0[2 * NB_VALUES_PER_SM],
361 // compressed_sm_norm_f0[3 * NB_VALUES_PER_SM],
362 // compressed_sm_norm_f0[4 * NB_VALUES_PER_SM],
363 // compressed_sm_norm_f0[5 * NB_VALUES_PER_SM],
364 // compressed_sm_norm_f0[6 * NB_VALUES_PER_SM],
365 // compressed_sm_norm_f0[7 * NB_VALUES_PER_SM],
366 // compressed_sm_norm_f0[8 * NB_VALUES_PER_SM],
367 // compressed_sm_norm_f0[9 * NB_VALUES_PER_SM],
368 // compressed_sm_norm_f0[10 * NB_VALUES_PER_SM]);
369 // </TMP DATA>
370
329 371 }
330 372 }
331 373
332 374 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F0)
333 375 {
334 376 // 1) reorganize the ASM and divide
335 377 ASM_reorganize_and_divide( incomingMsg->norm->matrix,
336 378 asm_f0_reorganized,
337 379 nb_sm_before_f0.norm_bp1 );
338 380 // 2) convert the float array in a char array
339 381 ASM_convert( asm_f0_reorganized, (char*) current_ring_node_to_send_asm_f0->buffer_address );
340 current_ring_node_to_send_asm_f0->coarseTime = incomingMsg->coarseTime;
341 current_ring_node_to_send_asm_f0->fineTime = incomingMsg->fineTime;
382 current_ring_node_to_send_asm_f0->coarseTime = incomingMsg->coarseTimeNORM;
383 current_ring_node_to_send_asm_f0->fineTime = incomingMsg->fineTimeNORM;
342 384 current_ring_node_to_send_asm_f0->sid = SID_NORM_ASM_F0;
385
386 // < TMP DATA>
387 #define INDEX_TO_LOOK_AT 31
388 float b11;
389 unsigned char *b11_charPtr;
390 b11_charPtr = (unsigned char*) &b11;
391 b11_charPtr[0] = ((unsigned char *) current_ring_node_to_send_asm_f0->buffer_address)[(INDEX_TO_LOOK_AT * NB_VALUES_PER_SM) * 2];
392 b11_charPtr[1] = ((unsigned char *) current_ring_node_to_send_asm_f0->buffer_address)[(INDEX_TO_LOOK_AT * NB_VALUES_PER_SM) * 2 +1];
393 b11_charPtr[2] = 0x00;
394 b11_charPtr[3] = 0x00;
395 printf("(ASM) initial = %f, reorganized and divided = %f, converted = %f\n",
396 incomingMsg->norm->matrix[INDEX_TO_LOOK_AT], // 32 * 96 = 3072 Hz
397 asm_f0_reorganized[ INDEX_TO_LOOK_AT * NB_VALUES_PER_SM ],
398 b11);
399 // </TMP DATA>
400
343 401 // 3) send the spectral matrix packets
344 402 status = rtems_message_queue_send( queue_id, &current_ring_node_to_send_asm_f0, sizeof( ring_node* ) );
345 403 // change asm ring node
346 404 current_ring_node_to_send_asm_f0 = current_ring_node_to_send_asm_f0->next;
347 405 }
348 406
349 407 }
350 408 }
351 409
352 410 //**********
353 411 // FUNCTIONS
354 412
355 413 void reset_nb_sm_f0( unsigned char lfrMode )
356 414 {
357 415 nb_sm_before_f0.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 96;
358 416 nb_sm_before_f0.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 96;
359 417 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;
360 418 nb_sm_before_f0.sbm1_bp1 = parameter_dump_packet.sy_lfr_s1_bp_p0 * 24; // 0.25 s per digit
361 419 nb_sm_before_f0.sbm1_bp2 = parameter_dump_packet.sy_lfr_s1_bp_p1 * 96;
362 420 nb_sm_before_f0.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 96;
363 421 nb_sm_before_f0.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 96;
364 422 nb_sm_before_f0.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 96;
365 423 nb_sm_before_f0.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 96;
366 424
367 425 if (lfrMode == LFR_MODE_SBM1)
368 426 {
369 427 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm1_bp1;
370 428 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm1_bp2;
371 429 }
372 430 else if (lfrMode == LFR_MODE_SBM2)
373 431 {
374 432 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm2_bp1;
375 433 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm2_bp2;
376 434 }
377 435 else if (lfrMode == LFR_MODE_BURST)
378 436 {
379 437 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
380 438 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
381 439 }
382 440 else
383 441 {
384 442 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
385 443 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
386 444 }
387 445 }
446
447 void init_k_coefficients_f0( void )
448 {
449 init_k_coefficients( k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0 );
450 init_k_coefficients( k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_SBM_F0);
451 }
452
453 void test_TCH( void )
454 {
455 #define NB_BINS_COMPRESSED_MATRIX_TCH 1
456
457 unsigned char LFR_BP1_f0[NB_BINS_COMPRESSED_MATRIX_TCH*NB_BYTES_BP1];
458 unsigned char LFR_BP2_f0[NB_BINS_COMPRESSED_MATRIX_TCH*NB_BYTES_BP2];
459 float k_coefficients[NB_BINS_COMPRESSED_MATRIX_TCH * NB_K_COEFF_PER_BIN];
460
461 float compressed_spectral_matrix_TCH[ NB_BINS_COMPRESSED_MATRIX_TCH * NB_VALUES_PER_SPECTRAL_MATRIX ] = {
462 1.02217712e+06,
463 -8.58216250e+04,
464 -3.22199043e+04,
465 1.01597820e+05,
466 8.10333875e+05,
467 1.19030141e+05,
468 -8.69636688e+05,
469 5.01504031e+05,
470 -1.01948547e+05,
471 1.35475020e+04,
472 -3.67825469e+04,
473 -1.10950273e+05,
474 2.10715000e+04,
475 4.49727383e+04,
476 -4.37282031e+04,
477 3.83337695e+03,
478 1.05317175e+06,
479 -4.04155312e+05,
480 -1.32987891e+05,
481 1.49277250e+05,
482 -4.39122625e+05,
483 9.46006250e+05,
484 2.64386625e+05,
485 3.71843125e+05,
486 3.39770000e+05
487 };
488
489 init_k_coefficients( k_coefficients, NB_BINS_COMPRESSED_MATRIX_TCH );
490
491 printf("\n");
492
493 BP1_set(compressed_spectral_matrix_TCH, k_coefficients, NB_BINS_COMPRESSED_MATRIX_TCH, LFR_BP1_f0);
494
495 printf("\n");
496
497 BP2_set(compressed_spectral_matrix_TCH, NB_BINS_COMPRESSED_MATRIX_TCH, LFR_BP2_f0);
498 }
Show file before | Show file after | Hide comments
@@ -1,367 +1,379
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf1_prc1.h"
11 11
12 12 nb_sm_before_bp_asm_f1 nb_sm_before_f1;
13 13
14 extern ring_node sm_ring_f1[ ];
15
14 16 //***
15 17 // F1
16 18 ring_node_asm asm_ring_norm_f1 [ NB_RING_NODES_ASM_NORM_F1 ];
17 19 ring_node_asm asm_ring_burst_sbm_f1 [ NB_RING_NODES_ASM_BURST_SBM_F1 ];
18 20
19 21 ring_node ring_to_send_asm_f1 [ NB_RING_NODES_ASM_F1 ];
20 22 int buffer_asm_f1 [ NB_RING_NODES_ASM_F1 * TOTAL_SIZE_SM ];
21 23
22 24 float asm_f1_reorganized [ TOTAL_SIZE_SM ];
23 25 char asm_f1_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ];
24 26 float compressed_sm_norm_f1[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F1];
25 27 float compressed_sm_sbm_f1 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 ];
26 28
29 float k_coeff_intercalib_f1_norm[ NB_BINS_COMPRESSED_SM_F1 * NB_K_COEFF_PER_BIN ]; // 13 * 32 = 416
30 float k_coeff_intercalib_f1_sbm[ NB_BINS_COMPRESSED_SM_SBM_F1 * NB_K_COEFF_PER_BIN ]; // 26 * 32 = 832
31
27 32 //************
28 33 // RTEMS TASKS
29 34
30 35 rtems_task avf1_task( rtems_task_argument lfrRequestedMode )
31 36 {
32 37 int i;
33 38
34 39 rtems_event_set event_out;
35 40 rtems_status_code status;
36 41 rtems_id queue_id_prc1;
37 42 asm_msg msgForMATR;
38 ring_node *ring_node_tab[8];
43 ring_node *nodeForAveraging;
44 ring_node *ring_node_tab[NB_SM_BEFORE_AVF0];
39 45 ring_node_asm *current_ring_node_asm_burst_sbm_f1;
40 46 ring_node_asm *current_ring_node_asm_norm_f1;
41 47
42 48 unsigned int nb_norm_bp1;
43 49 unsigned int nb_norm_bp2;
44 50 unsigned int nb_norm_asm;
45 51 unsigned int nb_sbm_bp1;
46 52 unsigned int nb_sbm_bp2;
47 53
48 54 nb_norm_bp1 = 0;
49 55 nb_norm_bp2 = 0;
50 56 nb_norm_asm = 0;
51 57 nb_sbm_bp1 = 0;
52 58 nb_sbm_bp2 = 0;
53 59
54 60 reset_nb_sm_f1( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
55 61 ASM_generic_init_ring( asm_ring_norm_f1, NB_RING_NODES_ASM_NORM_F1 );
56 62 ASM_generic_init_ring( asm_ring_burst_sbm_f1, NB_RING_NODES_ASM_BURST_SBM_F1 );
57 63 current_ring_node_asm_norm_f1 = asm_ring_norm_f1;
58 64 current_ring_node_asm_burst_sbm_f1 = asm_ring_burst_sbm_f1;
59 65
60 66 BOOT_PRINTF1("in AVF1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
61 67
62 68 status = get_message_queue_id_prc1( &queue_id_prc1 );
63 69 if (status != RTEMS_SUCCESSFUL)
64 70 {
65 71 PRINTF1("in AVF1 *** ERR get_message_queue_id_prc1 %d\n", status)
66 72 }
67 73
68 74 while(1){
69 75 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
70 76
71 77 //****************************************
72 78 // initialize the mesage for the MATR task
73 79 msgForMATR.norm = current_ring_node_asm_norm_f1;
74 80 msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f1;
75 81 msgForMATR.event = 0x00; // this composite event will be sent to the PRC1 task
76 msgForMATR.coarseTime = ring_node_for_averaging_sm_f1->coarseTime;
77 msgForMATR.fineTime = ring_node_for_averaging_sm_f1->fineTime;
78 82 //
79 83 //****************************************
80 84
81 ring_node_tab[NB_SM_BEFORE_AVF1-1] = ring_node_for_averaging_sm_f1;
85 nodeForAveraging = getRingNodeForAveraging( 1 );
86
87 ring_node_tab[NB_SM_BEFORE_AVF1-1] = nodeForAveraging;
82 88 for ( i = 2; i < (NB_SM_BEFORE_AVF1+1); i++ )
83 89 {
84 ring_node_for_averaging_sm_f1 = ring_node_for_averaging_sm_f1->previous;
85 ring_node_tab[NB_SM_BEFORE_AVF1-i] = ring_node_for_averaging_sm_f1;
90 nodeForAveraging = nodeForAveraging->previous;
91 ring_node_tab[NB_SM_BEFORE_AVF1-i] = nodeForAveraging;
86 92 }
87 93
88 94 // compute the average and store it in the averaged_sm_f1 buffer
89 95 SM_average( current_ring_node_asm_norm_f1->matrix,
90 96 current_ring_node_asm_burst_sbm_f1->matrix,
91 97 ring_node_tab,
92 nb_norm_bp1, nb_sbm_bp1 );
98 nb_norm_bp1, nb_sbm_bp1,
99 &msgForMATR );
93 100
94 101 // update nb_average
95 102 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF1;
96 103 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF1;
97 104 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF1;
98 105 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF1;
99 106 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF1;
100 107
101 108 if (nb_sbm_bp1 == nb_sm_before_f1.burst_sbm_bp1)
102 109 {
103 110 nb_sbm_bp1 = 0;
104 111 // set another ring for the ASM storage
105 112 current_ring_node_asm_burst_sbm_f1 = current_ring_node_asm_burst_sbm_f1->next;
106 113 if ( lfrCurrentMode == LFR_MODE_BURST )
107 114 {
108 115 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP1_F1;
109 116 }
110 117 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
111 118 {
112 119 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP1_F1;
113 120 }
114 121 }
115 122
116 123 if (nb_sbm_bp2 == nb_sm_before_f1.burst_sbm_bp2)
117 124 {
118 125 nb_sbm_bp2 = 0;
119 126 if ( lfrCurrentMode == LFR_MODE_BURST )
120 127 {
121 128 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP2_F1;
122 129 }
123 130 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
124 131 {
125 132 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP2_F1;
126 133 }
127 134 }
128 135
129 136 if (nb_norm_bp1 == nb_sm_before_f1.norm_bp1)
130 137 {
131 138 nb_norm_bp1 = 0;
132 139 // set another ring for the ASM storage
133 140 current_ring_node_asm_norm_f1 = current_ring_node_asm_norm_f1->next;
134 141 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
135 142 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
136 143 {
137 144 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F1;
138 145 }
139 146 }
140 147
141 148 if (nb_norm_bp2 == nb_sm_before_f1.norm_bp2)
142 149 {
143 150 nb_norm_bp2 = 0;
144 151 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
145 152 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
146 153 {
147 154 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F1;
148 155 }
149 156 }
150 157
151 158 if (nb_norm_asm == nb_sm_before_f1.norm_asm)
152 159 {
153 160 nb_norm_asm = 0;
154 161 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
155 162 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
156 163 {
157 164 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F1;
158 165 }
159 166 }
160 167
161 168 //*************************
162 169 // send the message to MATR
163 170 if (msgForMATR.event != 0x00)
164 171 {
165 172 status = rtems_message_queue_send( queue_id_prc1, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC1);
166 173 }
167 174
168 175 if (status != RTEMS_SUCCESSFUL) {
169 176 printf("in AVF1 *** Error sending message to PRC1, code %d\n", status);
170 177 }
171 178 }
172 179 }
173 180
174 181 rtems_task prc1_task( rtems_task_argument lfrRequestedMode )
175 182 {
176 183 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
177 184 size_t size; // size of the incoming TC packet
178 185 asm_msg *incomingMsg;
179 186 //
180 187 unsigned char sid;
181 188 rtems_status_code status;
182 189 rtems_id queue_id_send;
183 190 rtems_id queue_id_q_p1;
184 191 bp_packet_with_spare packet_norm_bp1;
185 192 bp_packet packet_norm_bp2;
186 193 bp_packet packet_sbm_bp1;
187 194 bp_packet packet_sbm_bp2;
188 195 ring_node *current_ring_node_to_send_asm_f1;
189 196
190 197 unsigned long long int localTime;
191 198
192 199 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
193 200 init_ring( ring_to_send_asm_f1, NB_RING_NODES_ASM_F1, (volatile int*) buffer_asm_f1, TOTAL_SIZE_SM );
194 201 current_ring_node_to_send_asm_f1 = ring_to_send_asm_f1;
195 202
196 203 //*************
197 204 // NORM headers
198 205 BP_init_header_with_spare( &packet_norm_bp1.header,
199 206 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F1,
200 207 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1, NB_BINS_COMPRESSED_SM_F1 );
201 BP_init_header( &packet_norm_bp2.header,
208 BP_init_header( &packet_norm_bp2,
202 209 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F1,
203 210 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1, NB_BINS_COMPRESSED_SM_F1);
204 211
205 212 //***********************
206 213 // BURST and SBM2 headers
207 214 if ( lfrRequestedMode == LFR_MODE_BURST )
208 215 {
209 BP_init_header( &packet_sbm_bp1.header,
216 BP_init_header( &packet_sbm_bp1,
210 217 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F1,
211 218 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
212 BP_init_header( &packet_sbm_bp2.header,
219 BP_init_header( &packet_sbm_bp2,
213 220 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F1,
214 221 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
215 222 }
216 223 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
217 224 {
218 BP_init_header( &packet_sbm_bp1.header,
225 BP_init_header( &packet_sbm_bp1,
219 226 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F1,
220 227 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
221 BP_init_header( &packet_sbm_bp2.header,
228 BP_init_header( &packet_sbm_bp2,
222 229 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F1,
223 230 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
224 231 }
225 232 else
226 233 {
227 234 PRINTF1("in PRC1 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
228 235 }
229 236
230 237 status = get_message_queue_id_send( &queue_id_send );
231 238 if (status != RTEMS_SUCCESSFUL)
232 239 {
233 240 PRINTF1("in PRC1 *** ERR get_message_queue_id_send %d\n", status)
234 241 }
235 242 status = get_message_queue_id_prc1( &queue_id_q_p1);
236 243 if (status != RTEMS_SUCCESSFUL)
237 244 {
238 245 PRINTF1("in PRC1 *** ERR get_message_queue_id_prc1 %d\n", status)
239 246 }
240 247
241 248 BOOT_PRINTF1("in PRC1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
242 249
243 250 while(1){
244 251 status = rtems_message_queue_receive( queue_id_q_p1, incomingData, &size, //************************************
245 252 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
246 253
247 254 incomingMsg = (asm_msg*) incomingData;
248 255
249 256 localTime = getTimeAsUnsignedLongLongInt( );
250 257 //***********
251 258 //***********
252 259 // BURST SBM2
253 260 //***********
254 261 //***********
255 262 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F1) )
256 263 {
257 264 sid = getSID( incomingMsg->event );
258 265 // 1) compress the matrix for Basic Parameters calculation
259 266 ASM_compress_reorganize_and_divide( incomingMsg->burst_sbm->matrix, compressed_sm_sbm_f1,
260 267 nb_sm_before_f1.burst_sbm_bp1,
261 268 NB_BINS_COMPRESSED_SM_SBM_F1, NB_BINS_TO_AVERAGE_ASM_SBM_F1,
262 269 ASM_F1_INDICE_START);
263 270 // 2) compute the BP1 set
264
271 BP1_set( compressed_sm_sbm_f1, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp1.data );
265 272 // 3) send the BP1 set
266 set_time( packet_sbm_bp1.header.time, (unsigned char *) &incomingMsg->coarseTime );
267 set_time( packet_sbm_bp1.header.acquisitionTime, (unsigned char *) &incomingMsg->coarseTime );
273 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
274 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
268 275 BP_send( (char *) &packet_sbm_bp1, queue_id_send,
269 276 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 + PACKET_LENGTH_DELTA,
270 277 sid );
271 278 // 4) compute the BP2 set if needed
272 279 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F1) )
273 280 {
274 281 // 1) compute the BP2 set
275
282 BP2_set( compressed_sm_sbm_f1, NB_BINS_COMPRESSED_SM_SBM_F1, packet_norm_bp2.data );
276 283 // 2) send the BP2 set
277 set_time( packet_sbm_bp2.header.time, (unsigned char *) &incomingMsg->coarseTime );
278 set_time( packet_sbm_bp2.header.acquisitionTime, (unsigned char *) &incomingMsg->coarseTime );
284 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
285 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
279 286 BP_send( (char *) &packet_sbm_bp2, queue_id_send,
280 287 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1 + PACKET_LENGTH_DELTA,
281 288 sid );
282 289 }
283 290 }
284 291
285 292 //*****
286 293 //*****
287 294 // NORM
288 295 //*****
289 296 //*****
290 297 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F1)
291 298 {
292 299 // 1) compress the matrix for Basic Parameters calculation
293 300 ASM_compress_reorganize_and_divide( incomingMsg->norm->matrix, compressed_sm_norm_f1,
294 301 nb_sm_before_f1.norm_bp1,
295 302 NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0,
296 303 ASM_F0_INDICE_START );
297 304 // 2) compute the BP1 set
298
305 BP1_set( compressed_sm_norm_f1, k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp1.data );
299 306 // 3) send the BP1 set
300 set_time( packet_norm_bp1.header.time, (unsigned char *) &incomingMsg->coarseTime );
301 set_time( packet_norm_bp1.header.acquisitionTime, (unsigned char *) &incomingMsg->coarseTime );
307 set_time( packet_norm_bp1.header.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
308 set_time( packet_norm_bp1.header.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
302 309 BP_send( (char *) &packet_norm_bp1, queue_id_send,
303 310 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 + PACKET_LENGTH_DELTA,
304 311 SID_NORM_BP1_F1 );
305 312 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F1)
306 313 {
307 314 // 1) compute the BP2 set
308
315 BP2_set( compressed_sm_norm_f1, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp2.data );
309 316 // 2) send the BP2 set
310 set_time( packet_norm_bp2.header.time, (unsigned char *) &incomingMsg->coarseTime );
311 set_time( packet_norm_bp2.header.acquisitionTime, (unsigned char *) &incomingMsg->coarseTime );
317 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
318 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
312 319 BP_send( (char *) &packet_norm_bp2, queue_id_send,
313 320 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 + PACKET_LENGTH_DELTA,
314 321 SID_NORM_BP2_F1 );
315 322 }
316 323 }
317 324
318 325 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F1)
319 326 {
320 327 // 1) reorganize the ASM and divide
321 328 ASM_reorganize_and_divide( incomingMsg->norm->matrix,
322 329 asm_f1_reorganized,
323 330 nb_sm_before_f1.norm_bp1 );
324 331 // 2) convert the float array in a char array
325 332 ASM_convert( asm_f1_reorganized, (char*) current_ring_node_to_send_asm_f1->buffer_address );
326 current_ring_node_to_send_asm_f1->coarseTime = incomingMsg->coarseTime;
327 current_ring_node_to_send_asm_f1->fineTime = incomingMsg->fineTime;
333 current_ring_node_to_send_asm_f1->coarseTime = incomingMsg->coarseTimeNORM;
334 current_ring_node_to_send_asm_f1->fineTime = incomingMsg->fineTimeNORM;
328 335 current_ring_node_to_send_asm_f1->sid = SID_NORM_ASM_F1;
329 336 // 3) send the spectral matrix packets
330 337 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f1, sizeof( ring_node* ) );
331 338 // change asm ring node
332 339 current_ring_node_to_send_asm_f1 = current_ring_node_to_send_asm_f1->next;
333 340 }
334 341
335 342 }
336 343 }
337 344
338 345 //**********
339 346 // FUNCTIONS
340 347
341 348 void reset_nb_sm_f1( unsigned char lfrMode )
342 349 {
343 350 nb_sm_before_f1.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 16;
344 351 nb_sm_before_f1.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 16;
345 352 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;
346 353 nb_sm_before_f1.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 16;
347 354 nb_sm_before_f1.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 16;
348 355 nb_sm_before_f1.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 16;
349 356 nb_sm_before_f1.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 16;
350 357
351 358 if (lfrMode == LFR_MODE_SBM2)
352 359 {
353 360 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.sbm2_bp1;
354 361 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.sbm2_bp2;
355 362 }
356 363 else if (lfrMode == LFR_MODE_BURST)
357 364 {
358 365 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
359 366 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
360 367 }
361 368 else
362 369 {
363 370 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
364 371 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
365 372 }
366 373 }
367 374
375 void init_k_coefficients_f1( void )
376 {
377 init_k_coefficients( k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1 );
378 init_k_coefficients( k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1);
379 }
Show file before | Show file after | Hide comments
@@ -1,262 +1,287
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf2_prc2.h"
11 11
12 12 nb_sm_before_bp_asm_f2 nb_sm_before_f2;
13 13
14 extern ring_node sm_ring_f2[ ];
15
14 16 //***
15 17 // F2
16 18 ring_node_asm asm_ring_norm_f2 [ NB_RING_NODES_ASM_NORM_F2 ];
17 19 ring_node_asm asm_ring_burst_sbm_f2[ NB_RING_NODES_ASM_BURST_SBM_F2 ];
18 20
19 21 ring_node ring_to_send_asm_f2 [ NB_RING_NODES_ASM_F2 ];
20 22 int buffer_asm_f2 [ NB_RING_NODES_ASM_F2 * TOTAL_SIZE_SM ];
21 23
22 24 float asm_f2_reorganized [ TOTAL_SIZE_SM ];
23 25 char asm_f2_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ];
24 26 float compressed_sm_norm_f2[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F2];
25 27 float compressed_sm_sbm_f2 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F2 ];
26 28
29 float k_coeff_intercalib_f2[ NB_BINS_COMPRESSED_SM_F2 * NB_K_COEFF_PER_BIN ]; // 12 * 32 = 384
30
27 31 //************
28 32 // RTEMS TASKS
29 33
30 34 //***
31 35 // F2
32 36 rtems_task avf2_task( rtems_task_argument argument )
33 37 {
34 38 rtems_event_set event_out;
35 39 rtems_status_code status;
36 40 rtems_id queue_id_prc2;
37 41 asm_msg msgForMATR;
42 ring_node *nodeForAveraging;
38 43 ring_node_asm *current_ring_node_asm_norm_f2;
39 44
40 45 unsigned int nb_norm_bp1;
41 46 unsigned int nb_norm_bp2;
42 47 unsigned int nb_norm_asm;
43 48
44 49 nb_norm_bp1 = 0;
45 50 nb_norm_bp2 = 0;
46 51 nb_norm_asm = 0;
47 52
48 53 reset_nb_sm_f2( ); // reset the sm counters that drive the BP and ASM computations / transmissions
49 54 ASM_generic_init_ring( asm_ring_norm_f2, NB_RING_NODES_ASM_NORM_F2 );
50 55 current_ring_node_asm_norm_f2 = asm_ring_norm_f2;
51 56
52 57 BOOT_PRINTF("in AVF2 ***\n")
53 58
54 59 status = get_message_queue_id_prc2( &queue_id_prc2 );
55 60 if (status != RTEMS_SUCCESSFUL)
56 61 {
57 62 PRINTF1("in AVF2 *** ERR get_message_queue_id_prc2 %d\n", status)
58 63 }
59 64
60 65 while(1){
61 66 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
62 67
63 68 //****************************************
64 69 // initialize the mesage for the MATR task
65 70 msgForMATR.norm = current_ring_node_asm_norm_f2;
66 71 msgForMATR.burst_sbm = NULL;
67 72 msgForMATR.event = 0x00; // this composite event will be sent to the PRC2 task
68 msgForMATR.coarseTime = ring_node_for_averaging_sm_f2->coarseTime;
69 msgForMATR.fineTime = ring_node_for_averaging_sm_f2->fineTime;
70 73 //
71 74 //****************************************
72 75
76 nodeForAveraging = getRingNodeForAveraging( 2 );
77
78 // printf(" **0** %x . %x", sm_ring_f2[0].coarseTime, sm_ring_f2[0].fineTime);
79 // printf(" **1** %x . %x", sm_ring_f2[1].coarseTime, sm_ring_f2[1].fineTime);
80 // printf(" **2** %x . %x", sm_ring_f2[2].coarseTime, sm_ring_f2[2].fineTime);
81 // printf(" **3** %x . %x", sm_ring_f2[3].coarseTime, sm_ring_f2[3].fineTime);
82 // printf(" **4** %x . %x", sm_ring_f2[4].coarseTime, sm_ring_f2[4].fineTime);
83 // printf(" **5** %x . %x", sm_ring_f2[5].coarseTime, sm_ring_f2[5].fineTime);
84 // printf(" **6** %x . %x", sm_ring_f2[6].coarseTime, sm_ring_f2[6].fineTime);
85 // printf(" **7** %x . %x", sm_ring_f2[7].coarseTime, sm_ring_f2[7].fineTime);
86 // printf(" **8** %x . %x", sm_ring_f2[8].coarseTime, sm_ring_f2[8].fineTime);
87 // printf(" **9** %x . %x", sm_ring_f2[9].coarseTime, sm_ring_f2[9].fineTime);
88 // printf(" **10** %x . %x\n", sm_ring_f2[10].coarseTime, sm_ring_f2[10].fineTime);
89
73 90 // compute the average and store it in the averaged_sm_f2 buffer
74 91 SM_average_f2( current_ring_node_asm_norm_f2->matrix,
75 ring_node_for_averaging_sm_f2,
76 nb_norm_bp1 );
92 nodeForAveraging,
93 nb_norm_bp1,
94 &msgForMATR );
77 95
78 96 // update nb_average
79 97 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF2;
80 98 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF2;
81 99 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF2;
82 100
83 101 if (nb_norm_bp1 == nb_sm_before_f2.norm_bp1)
84 102 {
85 103 nb_norm_bp1 = 0;
86 104 // set another ring for the ASM storage
87 105 current_ring_node_asm_norm_f2 = current_ring_node_asm_norm_f2->next;
88 106 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
89 107 || (lfrCurrentMode == LFR_MODE_SBM2) )
90 108 {
91 109 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F2;
92 110 }
93 111 }
94 112
95 113 if (nb_norm_bp2 == nb_sm_before_f2.norm_bp2)
96 114 {
97 115 nb_norm_bp2 = 0;
98 116 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
99 117 || (lfrCurrentMode == LFR_MODE_SBM2) )
100 118 {
101 119 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F2;
102 120 }
103 121 }
104 122
105 123 if (nb_norm_asm == nb_sm_before_f2.norm_asm)
106 124 {
107 125 nb_norm_asm = 0;
108 126 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
109 127 || (lfrCurrentMode == LFR_MODE_SBM2) )
110 128 {
111 // PRINTF1("%lld\n", localTime)
112 129 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F2;
113 130 }
114 131 }
115 132
116 133 //*************************
117 134 // send the message to MATR
118 135 if (msgForMATR.event != 0x00)
119 136 {
120 137 status = rtems_message_queue_send( queue_id_prc2, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC0);
121 138 }
122 139
123 140 if (status != RTEMS_SUCCESSFUL) {
124 141 printf("in AVF2 *** Error sending message to MATR, code %d\n", status);
125 142 }
126 143 }
127 144 }
128 145
129 146 rtems_task prc2_task( rtems_task_argument argument )
130 147 {
131 148 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
132 149 size_t size; // size of the incoming TC packet
133 150 asm_msg *incomingMsg;
134 151 //
135 152 rtems_status_code status;
136 153 rtems_id queue_id;
137 154 rtems_id queue_id_q_p2;
138 bp_packet packet_norm_bp1_f2;
139 bp_packet packet_norm_bp2_f2;
155 bp_packet packet_norm_bp1;
156 bp_packet packet_norm_bp2;
140 157 ring_node *current_ring_node_to_send_asm_f2;
141 158
142 159 unsigned long long int localTime;
143 160
144 161 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
145 162 init_ring( ring_to_send_asm_f2, NB_RING_NODES_ASM_F2, (volatile int*) buffer_asm_f2, TOTAL_SIZE_SM );
146 163 current_ring_node_to_send_asm_f2 = ring_to_send_asm_f2;
147 164
148 165 incomingMsg = NULL;
149 166
150 167 //*************
151 168 // NORM headers
152 BP_init_header( &packet_norm_bp1_f2.header,
169 BP_init_header( &packet_norm_bp1,
153 170 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F2,
154 171 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2, NB_BINS_COMPRESSED_SM_F2 );
155 BP_init_header( &packet_norm_bp2_f2.header,
172 BP_init_header( &packet_norm_bp2,
156 173 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F2,
157 174 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2, NB_BINS_COMPRESSED_SM_F2 );
158 175
159 176 status = get_message_queue_id_send( &queue_id );
160 177 if (status != RTEMS_SUCCESSFUL)
161 178 {
162 179 PRINTF1("in PRC2 *** ERR get_message_queue_id_send %d\n", status)
163 180 }
164 181 status = get_message_queue_id_prc2( &queue_id_q_p2);
165 182 if (status != RTEMS_SUCCESSFUL)
166 183 {
167 184 PRINTF1("in PRC2 *** ERR get_message_queue_id_prc2 %d\n", status)
168 185 }
169 186
170 187 BOOT_PRINTF("in PRC2 ***\n")
171 188
172 189 while(1){
173 190 status = rtems_message_queue_receive( queue_id_q_p2, incomingData, &size, //************************************
174 191 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
175 192
176 193 incomingMsg = (asm_msg*) incomingData;
177 194
178 195 localTime = getTimeAsUnsignedLongLongInt( );
179 196
180 197 //*****
181 198 //*****
182 199 // NORM
183 200 //*****
184 201 //*****
185 202 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F2)
186 203 {
187 204 // 1) compress the matrix for Basic Parameters calculation
188 205 ASM_compress_reorganize_and_divide( incomingMsg->norm->matrix, compressed_sm_norm_f2,
189 206 nb_sm_before_f2.norm_bp1,
190 207 NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2,
191 208 ASM_F2_INDICE_START );
192 209 // 2) compute the BP1 set
193
210 BP1_set( compressed_sm_norm_f2, k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp1.data );
194 211 // 3) send the BP1 set
195 set_time( packet_norm_bp1_f2.header.time, (unsigned char *) &incomingMsg->coarseTime );
196 set_time( packet_norm_bp1_f2.header.acquisitionTime, (unsigned char *) &incomingMsg->coarseTime );
197 BP_send( (char *) &packet_norm_bp1_f2, queue_id,
212 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
213 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
214 BP_send( (char *) &packet_norm_bp1, queue_id,
198 215 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2 + PACKET_LENGTH_DELTA,
199 216 SID_NORM_BP1_F2 );
200 217 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F2)
201 218 {
202 219 // 1) compute the BP2 set using the same ASM as the one used for BP1
203
220 BP2_set( compressed_sm_norm_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp2.data );
204 221 // 2) send the BP2 set
205 set_time( packet_norm_bp2_f2.header.time, (unsigned char *) &incomingMsg->coarseTime );
206 set_time( packet_norm_bp2_f2.header.acquisitionTime, (unsigned char *) &incomingMsg->coarseTime );
207 BP_send( (char *) &packet_norm_bp2_f2, queue_id,
222 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
223 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
224 BP_send( (char *) &packet_norm_bp2, queue_id,
208 225 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2 + PACKET_LENGTH_DELTA,
209 226 SID_NORM_BP2_F2 );
210 227 }
211 228 }
212 229
213 230 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F2)
214 231 {
215 232 // 1) reorganize the ASM and divide
216 233 ASM_reorganize_and_divide( incomingMsg->norm->matrix,
217 234 asm_f2_reorganized,
218 235 nb_sm_before_f2.norm_bp1 );
219 236 // 2) convert the float array in a char array
220 237 ASM_convert( asm_f2_reorganized, (char*) current_ring_node_to_send_asm_f2->buffer_address );
221 current_ring_node_to_send_asm_f2->coarseTime = incomingMsg->coarseTime;
222 current_ring_node_to_send_asm_f2->fineTime = incomingMsg->fineTime;
238 current_ring_node_to_send_asm_f2->coarseTime = incomingMsg->coarseTimeNORM;
239 current_ring_node_to_send_asm_f2->fineTime = incomingMsg->fineTimeNORM;
223 240 current_ring_node_to_send_asm_f2->sid = SID_NORM_ASM_F2;
224 241 // 3) send the spectral matrix packets
225 242 status = rtems_message_queue_send( queue_id, &current_ring_node_to_send_asm_f2, sizeof( ring_node* ) );
226 243 // change asm ring node
227 244 current_ring_node_to_send_asm_f2 = current_ring_node_to_send_asm_f2->next;
228 245 }
229 246
230 247 }
231 248 }
232 249
233 250 //**********
234 251 // FUNCTIONS
235 252
236 253 void reset_nb_sm_f2( void )
237 254 {
238 255 nb_sm_before_f2.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0;
239 256 nb_sm_before_f2.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1;
240 257 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];
241 258 }
242 259
243 260 void SM_average_f2( float *averaged_spec_mat_f2,
244 261 ring_node *ring_node,
245 unsigned int nbAverageNormF2 )
262 unsigned int nbAverageNormF2,
263 asm_msg *msgForMATR )
246 264 {
247 265 float sum;
248 266 unsigned int i;
249 267
250 268 for(i=0; i<TOTAL_SIZE_SM; i++)
251 269 {
252 270 sum = ( (int *) (ring_node->buffer_address) ) [ i ];
253 271 if ( (nbAverageNormF2 == 0) )
254 272 {
255 273 averaged_spec_mat_f2[ i ] = sum;
274 msgForMATR->coarseTimeNORM = ring_node->coarseTime;
275 msgForMATR->fineTimeNORM = ring_node->fineTime;
256 276 }
257 277 else
258 278 {
259 279 averaged_spec_mat_f2[ i ] = ( averaged_spec_mat_f2[ i ] + sum );
260 280 }
261 281 }
262 282 }
283
284 void init_k_coefficients_f2( void )
285 {
286 init_k_coefficients( k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2);
287 }
Show file before | Show file after | Hide comments
@@ -1,523 +1,532
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "fsw_processing.h"
11 11 #include "fsw_processing_globals.c"
12 12
13 13 unsigned int nb_sm_f0;
14 14 unsigned int nb_sm_f0_aux_f1;
15 15 unsigned int nb_sm_f1;
16 16 unsigned int nb_sm_f0_aux_f2;
17 17
18 18 //************************
19 19 // spectral matrices rings
20 20 ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ];
21 21 ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ];
22 22 ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ];
23 23 ring_node *current_ring_node_sm_f0;
24 24 ring_node *current_ring_node_sm_f1;
25 25 ring_node *current_ring_node_sm_f2;
26 26 ring_node *ring_node_for_averaging_sm_f0;
27 27 ring_node *ring_node_for_averaging_sm_f1;
28 28 ring_node *ring_node_for_averaging_sm_f2;
29 29
30 //
31 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
32 {
33 ring_node *node;
34
35 node = NULL;
36 switch ( frequencyChannel ) {
37 case 0:
38 node = ring_node_for_averaging_sm_f0;
39 break;
40 case 1:
41 node = ring_node_for_averaging_sm_f1;
42 break;
43 case 2:
44 node = ring_node_for_averaging_sm_f2;
45 break;
46 default:
47 break;
48 }
49
50 return node;
51 }
52
30 53 //***********************************************************
31 54 // Interrupt Service Routine for spectral matrices processing
32 55
33 56 void spectral_matrices_isr_f0( void )
34 57 {
35 58 unsigned char status;
36 unsigned long long int time_0;
37 unsigned long long int time_1;
38 unsigned long long int syncBit0;
39 unsigned long long int syncBit1;
59 rtems_status_code status_code;
40 60
41 61 status = spectral_matrix_regs->status & 0x03; // [0011] get the status_ready_matrix_f0_x bits
42 62
43 time_0 = get_acquisition_time( (unsigned char *) &spectral_matrix_regs->f0_0_coarse_time );
44 time_1 = get_acquisition_time( (unsigned char *) &spectral_matrix_regs->f0_1_coarse_time );
45 syncBit0 = ( (unsigned long long int) (spectral_matrix_regs->f0_0_coarse_time & 0x80000000) ) << 16;
46 syncBit1 = ( (unsigned long long int) (spectral_matrix_regs->f0_1_coarse_time & 0x80000000) ) << 16;
47
48 63 switch(status)
49 64 {
50 65 case 0:
51 66 break;
52 67 case 3:
53 // send a message if two buffers are ready
54 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_6 );
55 if ( time_0 < time_1 )
56 {
57 close_matrix_actions( &nb_sm_f0, NB_SM_BEFORE_AVF0, Task_id[TASKID_AVF0],
58 ring_node_for_averaging_sm_f0, current_ring_node_sm_f0, time_0 | syncBit0);
59 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
60 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
61 close_matrix_actions( &nb_sm_f0, NB_SM_BEFORE_AVF0, Task_id[TASKID_AVF0],
62 ring_node_for_averaging_sm_f0, current_ring_node_sm_f0, time_1 | syncBit1);
63 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
64 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
65 }
66 else
67 {
68 close_matrix_actions( &nb_sm_f0, NB_SM_BEFORE_AVF0, Task_id[TASKID_AVF0],
69 ring_node_for_averaging_sm_f0, current_ring_node_sm_f0, time_1 | syncBit1);
70 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
71 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
72 close_matrix_actions( &nb_sm_f0, NB_SM_BEFORE_AVF0, Task_id[TASKID_AVF0],
73 ring_node_for_averaging_sm_f0, current_ring_node_sm_f0, time_0 | syncBit0);
74 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
75 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
76 }
68 // UNEXPECTED VALUE
77 69 spectral_matrix_regs->status = 0x03; // [0011]
70 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
78 71 break;
79 72 case 1:
80 close_matrix_actions( &nb_sm_f0, NB_SM_BEFORE_AVF0, Task_id[TASKID_AVF0],
81 ring_node_for_averaging_sm_f0, current_ring_node_sm_f0, time_0 | syncBit0);
73 ring_node_for_averaging_sm_f0 = current_ring_node_sm_f0->previous;
82 74 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
75 ring_node_for_averaging_sm_f0->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
76 ring_node_for_averaging_sm_f0->fineTime = spectral_matrix_regs->f0_0_fine_time;
83 77 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
84 spectral_matrix_regs->status = 0x01; // [0001]
78 spectral_matrix_regs->status = 0x01; // [0000 0001]
79 // if there are enough ring nodes ready, wake up an AVFx task
80 nb_sm_f0 = nb_sm_f0 + 1;
81 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
82 {
83 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
84 {
85 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
86 }
87 nb_sm_f0 = 0;
88 }
85 89 break;
86 90 case 2:
87 close_matrix_actions( &nb_sm_f0, NB_SM_BEFORE_AVF0, Task_id[TASKID_AVF0],
88 ring_node_for_averaging_sm_f0, current_ring_node_sm_f0, time_1 | syncBit1);
91 ring_node_for_averaging_sm_f0 = current_ring_node_sm_f0->previous;
89 92 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
90 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
91 spectral_matrix_regs->status = 0x02; // [0010]
93 ring_node_for_averaging_sm_f0->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
94 ring_node_for_averaging_sm_f0->fineTime = spectral_matrix_regs->f0_1_fine_time;
95 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
96 spectral_matrix_regs->status = 0x02; // [0000 0010]
97 // if there are enough ring nodes ready, wake up an AVFx task
98 nb_sm_f0 = nb_sm_f0 + 1;
99 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
100 {
101 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
102 {
103 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
104 }
105 nb_sm_f0 = 0;
106 }
92 107 break;
93 108 }
94 109 }
95 110
96 111 void spectral_matrices_isr_f1( void )
97 112 {
113 rtems_status_code status_code;
98 114 unsigned char status;
99 unsigned long long int time;
100 unsigned long long int syncBit;
101 rtems_status_code status_code;
102 115
103 116 status = (spectral_matrix_regs->status & 0x0c) >> 2; // [1100] get the status_ready_matrix_f0_x bits
104 117
105 118 switch(status)
106 119 {
107 120 case 0:
108 121 break;
109 122 case 3:
110 123 // UNEXPECTED VALUE
111 124 spectral_matrix_regs->status = 0xc0; // [1100]
112 125 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
113 126 break;
114 127 case 1:
115 time = get_acquisition_time( (unsigned char *) &spectral_matrix_regs->f1_0_coarse_time );
116 syncBit = ( (unsigned long long int) (spectral_matrix_regs->f1_0_coarse_time & 0x80000000) ) << 16;
117 close_matrix_actions( &nb_sm_f1, NB_SM_BEFORE_AVF1, Task_id[TASKID_AVF1],
118 ring_node_for_averaging_sm_f1, current_ring_node_sm_f1, time | syncBit);
128 ring_node_for_averaging_sm_f1 = current_ring_node_sm_f1->previous;
119 129 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
130 ring_node_for_averaging_sm_f1->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
131 ring_node_for_averaging_sm_f1->fineTime = spectral_matrix_regs->f1_0_fine_time;
120 132 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
121 spectral_matrix_regs->status = 0x04; // [0100]
133 spectral_matrix_regs->status = 0x04; // [0000 0100]
134 // if there are enough ring nodes ready, wake up an AVFx task
135 nb_sm_f1 = nb_sm_f1 + 1;
136 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
137 {
138 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
139 {
140 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
141 }
142 nb_sm_f1 = 0;
143 }
122 144 break;
123 145 case 2:
124 time = get_acquisition_time( (unsigned char *) &spectral_matrix_regs->f1_1_coarse_time );
125 syncBit = ( (unsigned long long int) (spectral_matrix_regs->f1_1_coarse_time & 0x80000000) ) << 16;
126 close_matrix_actions( &nb_sm_f1, NB_SM_BEFORE_AVF1, Task_id[TASKID_AVF1],
127 ring_node_for_averaging_sm_f1, current_ring_node_sm_f1, time | syncBit);
146 ring_node_for_averaging_sm_f1 = current_ring_node_sm_f1->previous;
128 147 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
148 ring_node_for_averaging_sm_f1->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
149 ring_node_for_averaging_sm_f1->fineTime = spectral_matrix_regs->f1_1_fine_time;
129 150 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
130 spectral_matrix_regs->status = 0x08; // [1000]
151 spectral_matrix_regs->status = 0x08; // [1000 0000]
152 // if there are enough ring nodes ready, wake up an AVFx task
153 nb_sm_f1 = nb_sm_f1 + 1;
154 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
155 {
156 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
157 {
158 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
159 }
160 nb_sm_f1 = 0;
161 }
131 162 break;
132 163 }
133 164 }
134 165
135 166 void spectral_matrices_isr_f2( void )
136 167 {
137 168 unsigned char status;
138 169 rtems_status_code status_code;
139 170
140 171 status = (spectral_matrix_regs->status & 0x30) >> 4; // [0011 0000] get the status_ready_matrix_f0_x bits
141 172
142 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2;
143
144 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
145
146 173 switch(status)
147 174 {
148 175 case 0:
149 176 break;
150 177 case 3:
151 178 // UNEXPECTED VALUE
152 179 spectral_matrix_regs->status = 0x30; // [0011 0000]
153 180 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
154 181 break;
155 182 case 1:
183 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
184 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
156 185 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
157 186 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
158 187 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
159 188 spectral_matrix_regs->status = 0x10; // [0001 0000]
160 189 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
161 190 {
162 191 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
163 192 }
164 193 break;
165 194 case 2:
195 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
196 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
166 197 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
167 198 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
168 199 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
169 200 spectral_matrix_regs->status = 0x20; // [0010 0000]
170 201 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
171 202 {
172 203 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
173 204 }
174 205 break;
175 206 }
176 207 }
177 208
178 209 void spectral_matrix_isr_error_handler( void )
179 210 {
180 // rtems_status_code status_code;
211 rtems_status_code status_code;
181 212
182 // if (spectral_matrix_regs->status & 0x7c0) // [0111 1100 0000]
183 // {
184 // status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
185 // }
213 if (spectral_matrix_regs->status & 0x7c0) // [0111 1100 0000]
214 {
215 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
216 }
186 217
187 // spectral_matrix_regs->status = spectral_matrix_regs->status & 0x7c0;
218 spectral_matrix_regs->status = spectral_matrix_regs->status & 0x7c0;
188 219 }
189 220
190 221 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
191 222 {
192 223 // STATUS REGISTER
193 224 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
194 225 // 10 9 8
195 226 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
196 227 // 7 6 5 4 3 2 1 0
197 228
198 229 spectral_matrices_isr_f0();
199 230
200 231 spectral_matrices_isr_f1();
201 232
202 233 spectral_matrices_isr_f2();
203 234
204 235 spectral_matrix_isr_error_handler();
205 236 }
206 237
207 238 rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector )
208 239 {
209 240 rtems_status_code status_code;
210 241
211 242 //***
212 243 // F0
213 244 nb_sm_f0 = nb_sm_f0 + 1;
214 245 if (nb_sm_f0 == NB_SM_BEFORE_AVF0 )
215 246 {
216 247 ring_node_for_averaging_sm_f0 = current_ring_node_sm_f0;
217 248 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
218 249 {
219 250 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
220 251 }
221 252 nb_sm_f0 = 0;
222 253 }
223 254
224 255 //***
225 256 // F1
226 257 nb_sm_f0_aux_f1 = nb_sm_f0_aux_f1 + 1;
227 258 if (nb_sm_f0_aux_f1 == 6)
228 259 {
229 260 nb_sm_f0_aux_f1 = 0;
230 261 nb_sm_f1 = nb_sm_f1 + 1;
231 262 }
232 263 if (nb_sm_f1 == NB_SM_BEFORE_AVF1 )
233 264 {
234 265 ring_node_for_averaging_sm_f1 = current_ring_node_sm_f1;
235 266 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
236 267 {
237 268 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
238 269 }
239 270 nb_sm_f1 = 0;
240 271 }
241 272
242 273 //***
243 274 // F2
244 275 nb_sm_f0_aux_f2 = nb_sm_f0_aux_f2 + 1;
245 276 if (nb_sm_f0_aux_f2 == 96)
246 277 {
247 278 nb_sm_f0_aux_f2 = 0;
248 279 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2;
249 280 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
250 281 {
251 282 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
252 283 }
253 284 }
254 285 }
255 286
256 287 //******************
257 288 // Spectral Matrices
258 289
259 290 void reset_nb_sm( void )
260 291 {
261 292 nb_sm_f0 = 0;
262 293 nb_sm_f0_aux_f1 = 0;
263 294 nb_sm_f0_aux_f2 = 0;
264 295
265 296 nb_sm_f1 = 0;
266 297 }
267 298
268 299 void SM_init_rings( void )
269 300 {
270 301 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
271 302 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
272 303 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
273 304
274 305 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
275 306 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
276 307 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
277 308 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
278 309 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
279 310 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
280 311 }
281 312
282 313 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
283 314 {
284 315 unsigned char i;
285 316
286 317 ring[ nbNodes - 1 ].next
287 318 = (ring_node_asm*) &ring[ 0 ];
288 319
289 320 for(i=0; i<nbNodes-1; i++)
290 321 {
291 322 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
292 323 }
293 324 }
294 325
295 326 void SM_reset_current_ring_nodes( void )
296 327 {
297 328 current_ring_node_sm_f0 = sm_ring_f0[0].next;
298 329 current_ring_node_sm_f1 = sm_ring_f1[0].next;
299 330 current_ring_node_sm_f2 = sm_ring_f2[0].next;
300 331
301 332 ring_node_for_averaging_sm_f0 = sm_ring_f0;
302 333 ring_node_for_averaging_sm_f1 = sm_ring_f1;
303 334 ring_node_for_averaging_sm_f2 = sm_ring_f2;
304 335 }
305 336
306 337 //*****************
307 338 // Basic Parameters
308 339
309 void BP_init_header( Header_TM_LFR_SCIENCE_BP_t *header,
340 void BP_init_header( bp_packet *header,
310 341 unsigned int apid, unsigned char sid,
311 342 unsigned int packetLength, unsigned char blkNr )
312 343 {
313 344 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
314 345 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
315 346 header->reserved = 0x00;
316 347 header->userApplication = CCSDS_USER_APP;
317 348 header->packetID[0] = (unsigned char) (apid >> 8);
318 349 header->packetID[1] = (unsigned char) (apid);
319 350 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
320 351 header->packetSequenceControl[1] = 0x00;
321 352 header->packetLength[0] = (unsigned char) (packetLength >> 8);
322 353 header->packetLength[1] = (unsigned char) (packetLength);
323 354 // DATA FIELD HEADER
324 355 header->spare1_pusVersion_spare2 = 0x10;
325 356 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
326 357 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
327 358 header->destinationID = TM_DESTINATION_ID_GROUND;
359 header->time[0] = 0x00;
360 header->time[1] = 0x00;
361 header->time[2] = 0x00;
362 header->time[3] = 0x00;
363 header->time[4] = 0x00;
364 header->time[5] = 0x00;
328 365 // AUXILIARY DATA HEADER
329 366 header->sid = sid;
330 367 header->biaStatusInfo = 0x00;
331 header->time[0] = 0x00;
332 header->time[0] = 0x00;
333 header->time[0] = 0x00;
334 header->time[0] = 0x00;
335 header->time[0] = 0x00;
336 header->time[0] = 0x00;
368 header->acquisitionTime[0] = 0x00;
369 header->acquisitionTime[1] = 0x00;
370 header->acquisitionTime[2] = 0x00;
371 header->acquisitionTime[3] = 0x00;
372 header->acquisitionTime[4] = 0x00;
373 header->acquisitionTime[5] = 0x00;
337 374 header->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
338 375 header->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
339 376 }
340 377
341 378 void BP_init_header_with_spare(Header_TM_LFR_SCIENCE_BP_with_spare_t *header,
342 379 unsigned int apid, unsigned char sid,
343 380 unsigned int packetLength , unsigned char blkNr)
344 381 {
345 382 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
346 383 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
347 384 header->reserved = 0x00;
348 385 header->userApplication = CCSDS_USER_APP;
349 386 header->packetID[0] = (unsigned char) (apid >> 8);
350 387 header->packetID[1] = (unsigned char) (apid);
351 388 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
352 389 header->packetSequenceControl[1] = 0x00;
353 390 header->packetLength[0] = (unsigned char) (packetLength >> 8);
354 391 header->packetLength[1] = (unsigned char) (packetLength);
355 392 // DATA FIELD HEADER
356 393 header->spare1_pusVersion_spare2 = 0x10;
357 394 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
358 395 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
359 396 header->destinationID = TM_DESTINATION_ID_GROUND;
360 397 // AUXILIARY DATA HEADER
361 398 header->sid = sid;
362 399 header->biaStatusInfo = 0x00;
363 400 header->time[0] = 0x00;
364 401 header->time[0] = 0x00;
365 402 header->time[0] = 0x00;
366 403 header->time[0] = 0x00;
367 404 header->time[0] = 0x00;
368 405 header->time[0] = 0x00;
369 406 header->source_data_spare = 0x00;
370 407 header->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
371 408 header->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
372 409 }
373 410
374 411 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
375 412 {
376 413 rtems_status_code status;
377 414
378 415 // SET THE SEQUENCE_CNT PARAMETER
379 416 increment_seq_counter_source_id( (unsigned char*) &data[ PACKET_POS_SEQUENCE_CNT ], sid );
380 417 // SEND PACKET
381 418 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
382 419 if (status != RTEMS_SUCCESSFUL)
383 420 {
384 421 printf("ERR *** in BP_send *** ERR %d\n", (int) status);
385 422 }
386 423 }
387 424
388 425 //******************
389 426 // general functions
390 427
391 428 void reset_sm_status( void )
392 429 {
393 430 // error
394 431 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
395 432 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
396 433 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
397 434 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
398 435
399 436 spectral_matrix_regs->status = 0x7ff; // [0111 1111 1111]
400 437 }
401 438
402 439 void reset_spectral_matrix_regs( void )
403 440 {
404 441 /** This function resets the spectral matrices module registers.
405 442 *
406 443 * The registers affected by this function are located at the following offset addresses:
407 444 *
408 445 * - 0x00 config
409 446 * - 0x04 status
410 447 * - 0x08 matrixF0_Address0
411 448 * - 0x10 matrixFO_Address1
412 449 * - 0x14 matrixF1_Address
413 450 * - 0x18 matrixF2_Address
414 451 *
415 452 */
416 453
417 454 set_sm_irq_onError( 0 );
418 455
419 456 set_sm_irq_onNewMatrix( 0 );
420 457
421 458 reset_sm_status();
422 459
460 // F1
423 461 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
424 462 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
463 // F2
425 464 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
426 465 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
466 // F3
427 467 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
428 468 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
429 469
430 470 spectral_matrix_regs->matrix_length = 0xc8; // 25 * 128 / 16 = 200 = 0xc8
431 471 }
432 472
433 473 void set_time( unsigned char *time, unsigned char * timeInBuffer )
434 474 {
435 475 time[0] = timeInBuffer[0];
436 476 time[1] = timeInBuffer[1];
437 477 time[2] = timeInBuffer[2];
438 478 time[3] = timeInBuffer[3];
439 479 time[4] = timeInBuffer[6];
440 480 time[5] = timeInBuffer[7];
441 481 }
442 482
443 483 unsigned long long int get_acquisition_time( unsigned char *timePtr )
444 484 {
445 485 unsigned long long int acquisitionTimeAslong;
446 486 acquisitionTimeAslong = 0x00;
447 487 acquisitionTimeAslong = ( (unsigned long long int) (timePtr[0] & 0x7f) << 40 ) // [0111 1111] mask the synchronization bit
448 488 + ( (unsigned long long int) timePtr[1] << 32 )
449 489 + ( (unsigned long long int) timePtr[2] << 24 )
450 490 + ( (unsigned long long int) timePtr[3] << 16 )
451 491 + ( (unsigned long long int) timePtr[6] << 8 )
452 492 + ( (unsigned long long int) timePtr[7] );
453 493 return acquisitionTimeAslong;
454 494 }
455 495
456 void close_matrix_actions(unsigned int *nb_sm, unsigned int nb_sm_before_avf, rtems_id avf_task_id,
457 ring_node *node_for_averaging, ring_node *ringNode,
458 unsigned long long int time )
459 {
460 unsigned char *timePtr;
461 unsigned char *coarseTimePtr;
462 unsigned char *fineTimePtr;
463 rtems_status_code status_code;
464
465 timePtr = (unsigned char *) &time;
466 coarseTimePtr = (unsigned char *) &node_for_averaging->coarseTime;
467 fineTimePtr = (unsigned char *) &node_for_averaging->fineTime;
468
469 *nb_sm = *nb_sm + 1;
470 if (*nb_sm == nb_sm_before_avf)
471 {
472 node_for_averaging = ringNode;
473 coarseTimePtr[0] = timePtr[2];
474 coarseTimePtr[1] = timePtr[3];
475 coarseTimePtr[2] = timePtr[4];
476 coarseTimePtr[3] = timePtr[5];
477 fineTimePtr[2] = timePtr[6];
478 fineTimePtr[3] = timePtr[7];
479 if (rtems_event_send( avf_task_id, RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
480 {
481 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
482 }
483 *nb_sm = 0;
484 }
485 }
486
487 496 unsigned char getSID( rtems_event_set event )
488 497 {
489 498 unsigned char sid;
490 499
491 500 rtems_event_set eventSetBURST;
492 501 rtems_event_set eventSetSBM;
493 502
494 503 //******
495 504 // BURST
496 505 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
497 506 | RTEMS_EVENT_BURST_BP1_F1
498 507 | RTEMS_EVENT_BURST_BP2_F0
499 508 | RTEMS_EVENT_BURST_BP2_F1;
500 509
501 510 //****
502 511 // SBM
503 512 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
504 513 | RTEMS_EVENT_SBM_BP1_F1
505 514 | RTEMS_EVENT_SBM_BP2_F0
506 515 | RTEMS_EVENT_SBM_BP2_F1;
507 516
508 517 if (event & eventSetBURST)
509 518 {
510 519 sid = SID_BURST_BP1_F0;
511 520 }
512 521 else if (event & eventSetSBM)
513 522 {
514 523 sid = SID_SBM1_BP1_F0;
515 524 }
516 525 else
517 526 {
518 527 sid = 0;
519 528 }
520 529
521 530 return sid;
522 531 }
523 532
Show file before | Show file after | Hide comments
@@ -1,971 +1,971
1 1 /** Functions and tasks related to TeleCommand handling.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TeleCommands:\n
7 7 * action launching\n
8 8 * TC parsing\n
9 9 * ...
10 10 *
11 11 */
12 12
13 13 #include "tc_handler.h"
14 14
15 15 //***********
16 16 // RTEMS TASK
17 17
18 18 rtems_task actn_task( rtems_task_argument unused )
19 19 {
20 20 /** This RTEMS task is responsible for launching actions upton the reception of valid TeleCommands.
21 21 *
22 22 * @param unused is the starting argument of the RTEMS task
23 23 *
24 24 * The ACTN task waits for data coming from an RTEMS msesage queue. When data arrives, it launches specific actions depending
25 25 * on the incoming TeleCommand.
26 26 *
27 27 */
28 28
29 29 int result;
30 30 rtems_status_code status; // RTEMS status code
31 31 ccsdsTelecommandPacket_t TC; // TC sent to the ACTN task
32 32 size_t size; // size of the incoming TC packet
33 33 unsigned char subtype; // subtype of the current TC packet
34 34 unsigned char time[6];
35 35 rtems_id queue_rcv_id;
36 36 rtems_id queue_snd_id;
37 37
38 38 status = get_message_queue_id_recv( &queue_rcv_id );
39 39 if (status != RTEMS_SUCCESSFUL)
40 40 {
41 41 PRINTF1("in ACTN *** ERR get_message_queue_id_recv %d\n", status)
42 42 }
43 43
44 44 status = get_message_queue_id_send( &queue_snd_id );
45 45 if (status != RTEMS_SUCCESSFUL)
46 46 {
47 47 PRINTF1("in ACTN *** ERR get_message_queue_id_send %d\n", status)
48 48 }
49 49
50 50 result = LFR_SUCCESSFUL;
51 51 subtype = 0; // subtype of the current TC packet
52 52
53 53 BOOT_PRINTF("in ACTN *** \n")
54 54
55 55 while(1)
56 56 {
57 57 status = rtems_message_queue_receive( queue_rcv_id, (char*) &TC, &size,
58 58 RTEMS_WAIT, RTEMS_NO_TIMEOUT);
59 59 getTime( time ); // set time to the current time
60 60 if (status!=RTEMS_SUCCESSFUL)
61 61 {
62 62 PRINTF1("ERR *** in task ACTN *** error receiving a message, code %d \n", status)
63 63 }
64 64 else
65 65 {
66 66 subtype = TC.serviceSubType;
67 67 switch(subtype)
68 68 {
69 69 case TC_SUBTYPE_RESET:
70 70 result = action_reset( &TC, queue_snd_id, time );
71 71 close_action( &TC, result, queue_snd_id );
72 72 break;
73 73 //
74 74 case TC_SUBTYPE_LOAD_COMM:
75 75 result = action_load_common_par( &TC );
76 76 close_action( &TC, result, queue_snd_id );
77 77 break;
78 78 //
79 79 case TC_SUBTYPE_LOAD_NORM:
80 80 result = action_load_normal_par( &TC, queue_snd_id, time );
81 81 close_action( &TC, result, queue_snd_id );
82 82 break;
83 83 //
84 84 case TC_SUBTYPE_LOAD_BURST:
85 85 result = action_load_burst_par( &TC, queue_snd_id, time );
86 86 close_action( &TC, result, queue_snd_id );
87 87 break;
88 88 //
89 89 case TC_SUBTYPE_LOAD_SBM1:
90 90 result = action_load_sbm1_par( &TC, queue_snd_id, time );
91 91 close_action( &TC, result, queue_snd_id );
92 92 break;
93 93 //
94 94 case TC_SUBTYPE_LOAD_SBM2:
95 95 result = action_load_sbm2_par( &TC, queue_snd_id, time );
96 96 close_action( &TC, result, queue_snd_id );
97 97 break;
98 98 //
99 99 case TC_SUBTYPE_DUMP:
100 100 result = action_dump_par( queue_snd_id );
101 101 close_action( &TC, result, queue_snd_id );
102 102 break;
103 103 //
104 104 case TC_SUBTYPE_ENTER:
105 105 result = action_enter_mode( &TC, queue_snd_id );
106 106 close_action( &TC, result, queue_snd_id );
107 107 break;
108 108 //
109 109 case TC_SUBTYPE_UPDT_INFO:
110 110 result = action_update_info( &TC, queue_snd_id );
111 111 close_action( &TC, result, queue_snd_id );
112 112 break;
113 113 //
114 114 case TC_SUBTYPE_EN_CAL:
115 115 result = action_enable_calibration( &TC, queue_snd_id, time );
116 116 close_action( &TC, result, queue_snd_id );
117 117 break;
118 118 //
119 119 case TC_SUBTYPE_DIS_CAL:
120 120 result = action_disable_calibration( &TC, queue_snd_id, time );
121 121 close_action( &TC, result, queue_snd_id );
122 122 break;
123 123 //
124 124 case TC_SUBTYPE_UPDT_TIME:
125 125 result = action_update_time( &TC );
126 126 close_action( &TC, result, queue_snd_id );
127 127 break;
128 128 //
129 129 default:
130 130 break;
131 131 }
132 132 }
133 133 }
134 134 }
135 135
136 136 //***********
137 137 // TC ACTIONS
138 138
139 139 int action_reset(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
140 140 {
141 141 /** This function executes specific actions when a TC_LFR_RESET TeleCommand has been received.
142 142 *
143 143 * @param TC points to the TeleCommand packet that is being processed
144 144 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
145 145 *
146 146 */
147 147
148 148 printf("this is the end!!!\n");
149 149 exit(0);
150 150 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
151 151 return LFR_DEFAULT;
152 152 }
153 153
154 154 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
155 155 {
156 156 /** This function executes specific actions when a TC_LFR_ENTER_MODE TeleCommand has been received.
157 157 *
158 158 * @param TC points to the TeleCommand packet that is being processed
159 159 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
160 160 *
161 161 */
162 162
163 163 rtems_status_code status;
164 164 unsigned char requestedMode;
165 165 unsigned int *transitionCoarseTime_ptr;
166 166 unsigned int transitionCoarseTime;
167 167 unsigned char * bytePosPtr;
168 168
169 169 bytePosPtr = (unsigned char *) &TC->packetID;
170 170
171 171 requestedMode = bytePosPtr[ BYTE_POS_CP_MODE_LFR_SET ];
172 172 transitionCoarseTime_ptr = (unsigned int *) ( &bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME ] );
173 173 transitionCoarseTime = (*transitionCoarseTime_ptr) & 0x7fffffff;
174 174
175 175 status = check_mode_value( requestedMode );
176 176
177 177 if ( status != LFR_SUCCESSFUL ) // the mode value is inconsistent
178 178 {
179 179 send_tm_lfr_tc_exe_inconsistent( TC, queue_id, BYTE_POS_CP_MODE_LFR_SET, requestedMode );
180 180 }
181 181 else // the mode value is consistent, check the transition
182 182 {
183 183 status = check_mode_transition(requestedMode);
184 184 if (status != LFR_SUCCESSFUL)
185 185 {
186 186 PRINTF("ERR *** in action_enter_mode *** check_mode_transition\n")
187 187 send_tm_lfr_tc_exe_not_executable( TC, queue_id );
188 188 }
189 189 }
190 190
191 191 if ( status == LFR_SUCCESSFUL ) // the transition is valid, enter the mode
192 192 {
193 193 status = check_transition_date( transitionCoarseTime );
194 194 if (status != LFR_SUCCESSFUL)
195 195 {
196 196 PRINTF("ERR *** in action_enter_mode *** check_transition_date\n")
197 197 send_tm_lfr_tc_exe_inconsistent( TC, queue_id,
198 198 BYTE_POS_CP_LFR_ENTER_MODE_TIME,
199 199 bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME + 3 ] );
200 200 }
201 201 }
202 202
203 203 if ( status == LFR_SUCCESSFUL ) // the date is valid, enter the mode
204 204 {
205 205 PRINTF1("OK *** in action_enter_mode *** enter mode %d\n", requestedMode);
206 206 status = enter_mode( requestedMode, transitionCoarseTime );
207 207 }
208 208
209 209 return status;
210 210 }
211 211
212 212 int action_update_info(ccsdsTelecommandPacket_t *TC, rtems_id queue_id)
213 213 {
214 214 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
215 215 *
216 216 * @param TC points to the TeleCommand packet that is being processed
217 217 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
218 218 *
219 219 * @return LFR directive status code:
220 220 * - LFR_DEFAULT
221 221 * - LFR_SUCCESSFUL
222 222 *
223 223 */
224 224
225 225 unsigned int val;
226 226 int result;
227 227 unsigned int status;
228 228 unsigned char mode;
229 229 unsigned char * bytePosPtr;
230 230
231 231 bytePosPtr = (unsigned char *) &TC->packetID;
232 232
233 233 // check LFR mode
234 234 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET5 ] & 0x1e) >> 1;
235 235 status = check_update_info_hk_lfr_mode( mode );
236 236 if (status == LFR_SUCCESSFUL) // check TDS mode
237 237 {
238 238 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0xf0) >> 4;
239 239 status = check_update_info_hk_tds_mode( mode );
240 240 }
241 241 if (status == LFR_SUCCESSFUL) // check THR mode
242 242 {
243 243 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0x0f);
244 244 status = check_update_info_hk_thr_mode( mode );
245 245 }
246 246 if (status == LFR_SUCCESSFUL) // if the parameter check is successful
247 247 {
248 248 val = housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * 256
249 249 + housekeeping_packet.hk_lfr_update_info_tc_cnt[1];
250 250 val++;
251 251 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> 8);
252 252 housekeeping_packet.hk_lfr_update_info_tc_cnt[1] = (unsigned char) (val);
253 253 }
254 254
255 255 result = status;
256 256
257 257 return result;
258 258 }
259 259
260 260 int action_enable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
261 261 {
262 262 /** This function executes specific actions when a TC_LFR_ENABLE_CALIBRATION TeleCommand has been received.
263 263 *
264 264 * @param TC points to the TeleCommand packet that is being processed
265 265 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
266 266 *
267 267 */
268 268
269 269 int result;
270 270 unsigned char lfrMode;
271 271
272 272 result = LFR_DEFAULT;
273 273 lfrMode = (housekeeping_packet.lfr_status_word[0] & 0xf0) >> 4;
274 274
275 275 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
276 276 result = LFR_DEFAULT;
277 277
278 278 return result;
279 279 }
280 280
281 281 int action_disable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
282 282 {
283 283 /** This function executes specific actions when a TC_LFR_DISABLE_CALIBRATION TeleCommand has been received.
284 284 *
285 285 * @param TC points to the TeleCommand packet that is being processed
286 286 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
287 287 *
288 288 */
289 289
290 290 int result;
291 291 unsigned char lfrMode;
292 292
293 293 result = LFR_DEFAULT;
294 294 lfrMode = (housekeeping_packet.lfr_status_word[0] & 0xf0) >> 4;
295 295
296 296 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
297 297 result = LFR_DEFAULT;
298 298
299 299 return result;
300 300 }
301 301
302 302 int action_update_time(ccsdsTelecommandPacket_t *TC)
303 303 {
304 304 /** This function executes specific actions when a TC_LFR_UPDATE_TIME TeleCommand has been received.
305 305 *
306 306 * @param TC points to the TeleCommand packet that is being processed
307 307 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
308 308 *
309 309 * @return LFR_SUCCESSFUL
310 310 *
311 311 */
312 312
313 313 unsigned int val;
314 314
315 315 time_management_regs->coarse_time_load = (TC->dataAndCRC[0] << 24)
316 316 + (TC->dataAndCRC[1] << 16)
317 317 + (TC->dataAndCRC[2] << 8)
318 318 + TC->dataAndCRC[3];
319 319
320 320 val = housekeeping_packet.hk_lfr_update_time_tc_cnt[0] * 256
321 321 + housekeeping_packet.hk_lfr_update_time_tc_cnt[1];
322 322 val++;
323 323 housekeeping_packet.hk_lfr_update_time_tc_cnt[0] = (unsigned char) (val >> 8);
324 324 housekeeping_packet.hk_lfr_update_time_tc_cnt[1] = (unsigned char) (val);
325 325
326 326 return LFR_SUCCESSFUL;
327 327 }
328 328
329 329 //*******************
330 330 // ENTERING THE MODES
331 331 int check_mode_value( unsigned char requestedMode )
332 332 {
333 333 int status;
334 334
335 335 if ( (requestedMode != LFR_MODE_STANDBY)
336 336 && (requestedMode != LFR_MODE_NORMAL) && (requestedMode != LFR_MODE_BURST)
337 337 && (requestedMode != LFR_MODE_SBM1) && (requestedMode != LFR_MODE_SBM2) )
338 338 {
339 339 status = LFR_DEFAULT;
340 340 }
341 341 else
342 342 {
343 343 status = LFR_SUCCESSFUL;
344 344 }
345 345
346 346 return status;
347 347 }
348 348
349 349 int check_mode_transition( unsigned char requestedMode )
350 350 {
351 351 /** This function checks the validity of the transition requested by the TC_LFR_ENTER_MODE.
352 352 *
353 353 * @param requestedMode is the mode requested by the TC_LFR_ENTER_MODE
354 354 *
355 355 * @return LFR directive status codes:
356 356 * - LFR_SUCCESSFUL - the transition is authorized
357 357 * - LFR_DEFAULT - the transition is not authorized
358 358 *
359 359 */
360 360
361 361 int status;
362 362
363 363 switch (requestedMode)
364 364 {
365 365 case LFR_MODE_STANDBY:
366 366 if ( lfrCurrentMode == LFR_MODE_STANDBY ) {
367 367 status = LFR_DEFAULT;
368 368 }
369 369 else
370 370 {
371 371 status = LFR_SUCCESSFUL;
372 372 }
373 373 break;
374 374 case LFR_MODE_NORMAL:
375 375 if ( lfrCurrentMode == LFR_MODE_NORMAL ) {
376 376 status = LFR_DEFAULT;
377 377 }
378 378 else {
379 379 status = LFR_SUCCESSFUL;
380 380 }
381 381 break;
382 382 case LFR_MODE_BURST:
383 383 if ( lfrCurrentMode == LFR_MODE_BURST ) {
384 384 status = LFR_DEFAULT;
385 385 }
386 386 else {
387 387 status = LFR_SUCCESSFUL;
388 388 }
389 389 break;
390 390 case LFR_MODE_SBM1:
391 391 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
392 392 status = LFR_DEFAULT;
393 393 }
394 394 else {
395 395 status = LFR_SUCCESSFUL;
396 396 }
397 397 break;
398 398 case LFR_MODE_SBM2:
399 399 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
400 400 status = LFR_DEFAULT;
401 401 }
402 402 else {
403 403 status = LFR_SUCCESSFUL;
404 404 }
405 405 break;
406 406 default:
407 407 status = LFR_DEFAULT;
408 408 break;
409 409 }
410 410
411 411 return status;
412 412 }
413 413
414 414 int check_transition_date( unsigned int transitionCoarseTime )
415 415 {
416 416 int status;
417 417 unsigned int localCoarseTime;
418 418 unsigned int deltaCoarseTime;
419 419
420 420 status = LFR_SUCCESSFUL;
421 421
422 422 if (transitionCoarseTime == 0) // transition time = 0 means an instant transition
423 423 {
424 424 status = LFR_SUCCESSFUL;
425 425 }
426 426 else
427 427 {
428 428 localCoarseTime = time_management_regs->coarse_time & 0x7fffffff;
429 429
430 430 if ( transitionCoarseTime <= localCoarseTime ) // SSS-CP-EQS-322
431 431 {
432 432 status = LFR_DEFAULT;
433 433 PRINTF2("ERR *** in check_transition_date *** transition = %x, local = %x\n", transitionCoarseTime, localCoarseTime)
434 434 }
435 435
436 436 if (status == LFR_SUCCESSFUL)
437 437 {
438 438 deltaCoarseTime = transitionCoarseTime - localCoarseTime;
439 439 if ( deltaCoarseTime > 3 ) // SSS-CP-EQS-323
440 440 {
441 441 status = LFR_DEFAULT;
442 442 PRINTF1("ERR *** in check_transition_date *** deltaCoarseTime = %x\n", deltaCoarseTime)
443 443 }
444 444 }
445 445 }
446 446
447 447 return status;
448 448 }
449 449
450 450 int stop_current_mode( void )
451 451 {
452 452 /** This function stops the current mode by masking interrupt lines and suspending science tasks.
453 453 *
454 454 * @return RTEMS directive status codes:
455 455 * - RTEMS_SUCCESSFUL - task restarted successfully
456 456 * - RTEMS_INVALID_ID - task id invalid
457 457 * - RTEMS_ALREADY_SUSPENDED - task already suspended
458 458 *
459 459 */
460 460
461 461 rtems_status_code status;
462 462
463 463 status = RTEMS_SUCCESSFUL;
464 464
465 465 // (1) mask interruptions
466 466 LEON_Mask_interrupt( IRQ_WAVEFORM_PICKER ); // mask waveform picker interrupt
467 467 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
468 468
469 // reset lfr VHDL module
470 reset_lfr();
471
472 469 // (2) reset waveform picker registers
473 470 reset_wfp_burst_enable(); // reset burst and enable bits
474 471 reset_wfp_status(); // reset all the status bits
475 472
476 473 // (3) reset spectral matrices registers
477 474 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
478 475 reset_sm_status();
479 476
477 // reset lfr VHDL module
478 reset_lfr();
479
480 480 reset_extractSWF(); // reset the extractSWF flag to false
481 481
482 482 // (4) clear interruptions
483 483 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER ); // clear waveform picker interrupt
484 484 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
485 485
486 486 // <Spectral Matrices simulator>
487 487 LEON_Mask_interrupt( IRQ_SM_SIMULATOR ); // mask spectral matrix interrupt simulator
488 488 timer_stop( (gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR );
489 489 LEON_Clear_interrupt( IRQ_SM_SIMULATOR ); // clear spectral matrix interrupt simulator
490 490 // </Spectral Matrices simulator>
491 491
492 492 // suspend several tasks
493 493 if (lfrCurrentMode != LFR_MODE_STANDBY) {
494 494 status = suspend_science_tasks();
495 495 }
496 496
497 497 if (status != RTEMS_SUCCESSFUL)
498 498 {
499 499 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
500 500 }
501 501
502 502 return status;
503 503 }
504 504
505 505 int enter_mode( unsigned char mode, unsigned int transitionCoarseTime )
506 506 {
507 507 /** This function is launched after a mode transition validation.
508 508 *
509 509 * @param mode is the mode in which LFR will be put.
510 510 *
511 511 * @return RTEMS directive status codes:
512 512 * - RTEMS_SUCCESSFUL - the mode has been entered successfully
513 513 * - RTEMS_NOT_SATISFIED - the mode has not been entered successfully
514 514 *
515 515 */
516 516
517 517 rtems_status_code status;
518 518
519 519 //**********************
520 520 // STOP THE CURRENT MODE
521 521 status = stop_current_mode();
522 522 if (status != RTEMS_SUCCESSFUL)
523 523 {
524 524 PRINTF1("ERR *** in enter_mode *** stop_current_mode with mode = %d\n", mode)
525 525 }
526 526
527 527 //*************************
528 528 // ENTER THE REQUESTED MODE
529 529 if ( (mode == LFR_MODE_NORMAL) || (mode == LFR_MODE_BURST)
530 530 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2) )
531 531 {
532 532 #ifdef PRINT_TASK_STATISTICS
533 533 rtems_cpu_usage_reset();
534 534 maxCount = 0;
535 535 #endif
536 536 status = restart_science_tasks( mode );
537 537 launch_spectral_matrix( );
538 538 launch_waveform_picker( mode, transitionCoarseTime );
539 539 // launch_spectral_matrix_simu( );
540 540 }
541 541 else if ( mode == LFR_MODE_STANDBY )
542 542 {
543 543 #ifdef PRINT_TASK_STATISTICS
544 544 rtems_cpu_usage_report();
545 545 #endif
546 546
547 547 #ifdef PRINT_STACK_REPORT
548 548 PRINTF("stack report selected\n")
549 549 rtems_stack_checker_report_usage();
550 550 #endif
551 551 PRINTF1("maxCount = %d\n", maxCount)
552 552 }
553 553 else
554 554 {
555 555 status = RTEMS_UNSATISFIED;
556 556 }
557 557
558 558 if (status != RTEMS_SUCCESSFUL)
559 559 {
560 560 PRINTF1("ERR *** in enter_mode *** status = %d\n", status)
561 561 status = RTEMS_UNSATISFIED;
562 562 }
563 563
564 564 return status;
565 565 }
566 566
567 567 int restart_science_tasks(unsigned char lfrRequestedMode )
568 568 {
569 569 /** This function is used to restart all science tasks.
570 570 *
571 571 * @return RTEMS directive status codes:
572 572 * - RTEMS_SUCCESSFUL - task restarted successfully
573 573 * - RTEMS_INVALID_ID - task id invalid
574 574 * - RTEMS_INCORRECT_STATE - task never started
575 575 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
576 576 *
577 577 * Science tasks are AVF0, PRC0, WFRM, CWF3, CW2, CWF1
578 578 *
579 579 */
580 580
581 581 rtems_status_code status[10];
582 582 rtems_status_code ret;
583 583
584 584 ret = RTEMS_SUCCESSFUL;
585 585
586 586 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
587 587 if (status[0] != RTEMS_SUCCESSFUL)
588 588 {
589 589 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
590 590 }
591 591
592 592 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
593 593 if (status[1] != RTEMS_SUCCESSFUL)
594 594 {
595 595 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
596 596 }
597 597
598 598 status[2] = rtems_task_restart( Task_id[TASKID_WFRM],1 );
599 599 if (status[2] != RTEMS_SUCCESSFUL)
600 600 {
601 601 PRINTF1("in restart_science_task *** WFRM ERR %d\n", status[2])
602 602 }
603 603
604 604 status[3] = rtems_task_restart( Task_id[TASKID_CWF3],1 );
605 605 if (status[3] != RTEMS_SUCCESSFUL)
606 606 {
607 607 PRINTF1("in restart_science_task *** CWF3 ERR %d\n", status[3])
608 608 }
609 609
610 610 status[4] = rtems_task_restart( Task_id[TASKID_CWF2],1 );
611 611 if (status[4] != RTEMS_SUCCESSFUL)
612 612 {
613 613 PRINTF1("in restart_science_task *** CWF2 ERR %d\n", status[4])
614 614 }
615 615
616 616 status[5] = rtems_task_restart( Task_id[TASKID_CWF1],1 );
617 617 if (status[5] != RTEMS_SUCCESSFUL)
618 618 {
619 619 PRINTF1("in restart_science_task *** CWF1 ERR %d\n", status[5])
620 620 }
621 621
622 622 status[6] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
623 623 if (status[6] != RTEMS_SUCCESSFUL)
624 624 {
625 625 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[6])
626 626 }
627 627
628 628 status[7] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
629 629 if (status[7] != RTEMS_SUCCESSFUL)
630 630 {
631 631 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[7])
632 632 }
633 633
634 634 status[8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
635 635 if (status[8] != RTEMS_SUCCESSFUL)
636 636 {
637 637 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[8])
638 638 }
639 639
640 640 status[9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
641 641 if (status[9] != RTEMS_SUCCESSFUL)
642 642 {
643 643 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[9])
644 644 }
645 645
646 646 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
647 647 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
648 648 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) ||
649 649 (status[6] != RTEMS_SUCCESSFUL) || (status[7] != RTEMS_SUCCESSFUL) ||
650 650 (status[8] != RTEMS_SUCCESSFUL) || (status[9] != RTEMS_SUCCESSFUL) )
651 651 {
652 652 ret = RTEMS_UNSATISFIED;
653 653 }
654 654
655 655 return ret;
656 656 }
657 657
658 658 int suspend_science_tasks()
659 659 {
660 660 /** This function suspends the science tasks.
661 661 *
662 662 * @return RTEMS directive status codes:
663 663 * - RTEMS_SUCCESSFUL - task restarted successfully
664 664 * - RTEMS_INVALID_ID - task id invalid
665 665 * - RTEMS_ALREADY_SUSPENDED - task already suspended
666 666 *
667 667 */
668 668
669 669 rtems_status_code status;
670 670
671 671 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
672 672 if (status != RTEMS_SUCCESSFUL)
673 673 {
674 674 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
675 675 }
676 676 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
677 677 {
678 678 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
679 679 if (status != RTEMS_SUCCESSFUL)
680 680 {
681 681 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
682 682 }
683 683 }
684 684 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
685 685 {
686 686 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
687 687 if (status != RTEMS_SUCCESSFUL)
688 688 {
689 689 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
690 690 }
691 691 }
692 692 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
693 693 {
694 694 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
695 695 if (status != RTEMS_SUCCESSFUL)
696 696 {
697 697 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
698 698 }
699 699 }
700 700 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
701 701 {
702 702 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
703 703 if (status != RTEMS_SUCCESSFUL)
704 704 {
705 705 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
706 706 }
707 707 }
708 708 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
709 709 {
710 710 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
711 711 if (status != RTEMS_SUCCESSFUL)
712 712 {
713 713 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
714 714 }
715 715 }
716 716 if (status == RTEMS_SUCCESSFUL) // suspend WFRM
717 717 {
718 718 status = rtems_task_suspend( Task_id[TASKID_WFRM] );
719 719 if (status != RTEMS_SUCCESSFUL)
720 720 {
721 721 PRINTF1("in suspend_science_task *** WFRM ERR %d\n", status)
722 722 }
723 723 }
724 724 if (status == RTEMS_SUCCESSFUL) // suspend CWF3
725 725 {
726 726 status = rtems_task_suspend( Task_id[TASKID_CWF3] );
727 727 if (status != RTEMS_SUCCESSFUL)
728 728 {
729 729 PRINTF1("in suspend_science_task *** CWF3 ERR %d\n", status)
730 730 }
731 731 }
732 732 if (status == RTEMS_SUCCESSFUL) // suspend CWF2
733 733 {
734 734 status = rtems_task_suspend( Task_id[TASKID_CWF2] );
735 735 if (status != RTEMS_SUCCESSFUL)
736 736 {
737 737 PRINTF1("in suspend_science_task *** CWF2 ERR %d\n", status)
738 738 }
739 739 }
740 740 if (status == RTEMS_SUCCESSFUL) // suspend CWF1
741 741 {
742 742 status = rtems_task_suspend( Task_id[TASKID_CWF1] );
743 743 if (status != RTEMS_SUCCESSFUL)
744 744 {
745 745 PRINTF1("in suspend_science_task *** CWF1 ERR %d\n", status)
746 746 }
747 747 }
748 748
749 749 return status;
750 750 }
751 751
752 752 void launch_waveform_picker( unsigned char mode, unsigned int transitionCoarseTime )
753 753 {
754 754 WFP_reset_current_ring_nodes();
755 755
756 756 reset_waveform_picker_regs();
757 757
758 758 set_wfp_burst_enable_register( mode );
759 759
760 760 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER );
761 761 LEON_Unmask_interrupt( IRQ_WAVEFORM_PICKER );
762 762
763 763 if (transitionCoarseTime == 0)
764 764 {
765 765 waveform_picker_regs->start_date = time_management_regs->coarse_time;
766 766 }
767 767 else
768 768 {
769 769 waveform_picker_regs->start_date = transitionCoarseTime;
770 770 }
771 771
772 PRINTF1("commutation coarse time = %d\n", transitionCoarseTime)
772 PRINTF1("commutation coarse time = %x\n", transitionCoarseTime)
773 773 }
774 774
775 775 void launch_spectral_matrix( void )
776 776 {
777 777 SM_reset_current_ring_nodes();
778 778
779 779 reset_spectral_matrix_regs();
780 780
781 781 reset_nb_sm();
782 782
783 783 set_sm_irq_onNewMatrix( 1 );
784 784
785 785 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX );
786 786 LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRIX );
787 787
788 788 }
789 789
790 790 void launch_spectral_matrix_simu( void )
791 791 {
792 792 SM_reset_current_ring_nodes();
793 793 reset_spectral_matrix_regs();
794 794 reset_nb_sm();
795 795
796 796 // Spectral Matrices simulator
797 797 timer_start( (gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR );
798 798 LEON_Clear_interrupt( IRQ_SM_SIMULATOR );
799 799 LEON_Unmask_interrupt( IRQ_SM_SIMULATOR );
800 800 }
801 801
802 802 void set_sm_irq_onNewMatrix( unsigned char value )
803 803 {
804 804 if (value == 1)
805 805 {
806 806 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x01;
807 807 }
808 808 else
809 809 {
810 810 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffe; // 1110
811 811 }
812 812 }
813 813
814 814 void set_sm_irq_onError( unsigned char value )
815 815 {
816 816 if (value == 1)
817 817 {
818 818 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x02;
819 819 }
820 820 else
821 821 {
822 822 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffd; // 1101
823 823 }
824 824 }
825 825
826 826 //****************
827 827 // CLOSING ACTIONS
828 828 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC, unsigned char * time )
829 829 {
830 830 /** This function is used to update the HK packets statistics after a successful TC execution.
831 831 *
832 832 * @param TC points to the TC being processed
833 833 * @param time is the time used to date the TC execution
834 834 *
835 835 */
836 836
837 837 unsigned int val;
838 838
839 839 housekeeping_packet.hk_lfr_last_exe_tc_id[0] = TC->packetID[0];
840 840 housekeeping_packet.hk_lfr_last_exe_tc_id[1] = TC->packetID[1];
841 841 housekeeping_packet.hk_lfr_last_exe_tc_type[0] = 0x00;
842 842 housekeeping_packet.hk_lfr_last_exe_tc_type[1] = TC->serviceType;
843 843 housekeeping_packet.hk_lfr_last_exe_tc_subtype[0] = 0x00;
844 844 housekeeping_packet.hk_lfr_last_exe_tc_subtype[1] = TC->serviceSubType;
845 845 housekeeping_packet.hk_lfr_last_exe_tc_time[0] = time[0];
846 846 housekeeping_packet.hk_lfr_last_exe_tc_time[1] = time[1];
847 847 housekeeping_packet.hk_lfr_last_exe_tc_time[2] = time[2];
848 848 housekeeping_packet.hk_lfr_last_exe_tc_time[3] = time[3];
849 849 housekeeping_packet.hk_lfr_last_exe_tc_time[4] = time[4];
850 850 housekeeping_packet.hk_lfr_last_exe_tc_time[5] = time[5];
851 851
852 852 val = housekeeping_packet.hk_lfr_exe_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_exe_tc_cnt[1];
853 853 val++;
854 854 housekeeping_packet.hk_lfr_exe_tc_cnt[0] = (unsigned char) (val >> 8);
855 855 housekeeping_packet.hk_lfr_exe_tc_cnt[1] = (unsigned char) (val);
856 856 }
857 857
858 858 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC, unsigned char * time )
859 859 {
860 860 /** This function is used to update the HK packets statistics after a TC rejection.
861 861 *
862 862 * @param TC points to the TC being processed
863 863 * @param time is the time used to date the TC rejection
864 864 *
865 865 */
866 866
867 867 unsigned int val;
868 868
869 869 housekeeping_packet.hk_lfr_last_rej_tc_id[0] = TC->packetID[0];
870 870 housekeeping_packet.hk_lfr_last_rej_tc_id[1] = TC->packetID[1];
871 871 housekeeping_packet.hk_lfr_last_rej_tc_type[0] = 0x00;
872 872 housekeeping_packet.hk_lfr_last_rej_tc_type[1] = TC->serviceType;
873 873 housekeeping_packet.hk_lfr_last_rej_tc_subtype[0] = 0x00;
874 874 housekeeping_packet.hk_lfr_last_rej_tc_subtype[1] = TC->serviceSubType;
875 875 housekeeping_packet.hk_lfr_last_rej_tc_time[0] = time[0];
876 876 housekeeping_packet.hk_lfr_last_rej_tc_time[1] = time[1];
877 877 housekeeping_packet.hk_lfr_last_rej_tc_time[2] = time[2];
878 878 housekeeping_packet.hk_lfr_last_rej_tc_time[3] = time[3];
879 879 housekeeping_packet.hk_lfr_last_rej_tc_time[4] = time[4];
880 880 housekeeping_packet.hk_lfr_last_rej_tc_time[5] = time[5];
881 881
882 882 val = housekeeping_packet.hk_lfr_rej_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_rej_tc_cnt[1];
883 883 val++;
884 884 housekeeping_packet.hk_lfr_rej_tc_cnt[0] = (unsigned char) (val >> 8);
885 885 housekeeping_packet.hk_lfr_rej_tc_cnt[1] = (unsigned char) (val);
886 886 }
887 887
888 888 void close_action(ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id )
889 889 {
890 890 /** This function is the last step of the TC execution workflow.
891 891 *
892 892 * @param TC points to the TC being processed
893 893 * @param result is the result of the TC execution (LFR_SUCCESSFUL / LFR_DEFAULT)
894 894 * @param queue_id is the id of the RTEMS message queue used to send TM packets
895 895 * @param time is the time used to date the TC execution
896 896 *
897 897 */
898 898
899 899 unsigned char requestedMode;
900 900
901 901 if (result == LFR_SUCCESSFUL)
902 902 {
903 903 if ( !( (TC->serviceType==TC_TYPE_TIME) & (TC->serviceSubType==TC_SUBTYPE_UPDT_TIME) )
904 904 &
905 905 !( (TC->serviceType==TC_TYPE_GEN) & (TC->serviceSubType==TC_SUBTYPE_UPDT_INFO))
906 906 )
907 907 {
908 908 send_tm_lfr_tc_exe_success( TC, queue_id );
909 909 }
910 910 if ( (TC->serviceType == TC_TYPE_GEN) & (TC->serviceSubType == TC_SUBTYPE_ENTER) )
911 911 {
912 912 //**********************************
913 913 // UPDATE THE LFRMODE LOCAL VARIABLE
914 914 requestedMode = TC->dataAndCRC[1];
915 915 housekeeping_packet.lfr_status_word[0] = (unsigned char) ((requestedMode << 4) + 0x0d);
916 916 updateLFRCurrentMode();
917 917 }
918 918 }
919 919 else if (result == LFR_EXE_ERROR)
920 920 {
921 921 send_tm_lfr_tc_exe_error( TC, queue_id );
922 922 }
923 923 }
924 924
925 925 //***************************
926 926 // Interrupt Service Routines
927 927 rtems_isr commutation_isr1( rtems_vector_number vector )
928 928 {
929 929 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
930 930 printf("In commutation_isr1 *** Error sending event to DUMB\n");
931 931 }
932 932 }
933 933
934 934 rtems_isr commutation_isr2( rtems_vector_number vector )
935 935 {
936 936 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
937 937 printf("In commutation_isr2 *** Error sending event to DUMB\n");
938 938 }
939 939 }
940 940
941 941 //****************
942 942 // OTHER FUNCTIONS
943 943 void updateLFRCurrentMode()
944 944 {
945 945 /** This function updates the value of the global variable lfrCurrentMode.
946 946 *
947 947 * lfrCurrentMode is a parameter used by several functions to know in which mode LFR is running.
948 948 *
949 949 */
950 950 // update the local value of lfrCurrentMode with the value contained in the housekeeping_packet structure
951 951 lfrCurrentMode = (housekeeping_packet.lfr_status_word[0] & 0xf0) >> 4;
952 952 }
953 953
954 954 void set_lfr_soft_reset( unsigned char value )
955 955 {
956 956 if (value == 1)
957 957 {
958 958 time_management_regs->ctrl = time_management_regs->ctrl | 0x00000004; // [0100]
959 959 }
960 960 else
961 961 {
962 962 time_management_regs->ctrl = time_management_regs->ctrl & 0xfffffffb; // [1011]
963 963 }
964 964 }
965 965
966 966 void reset_lfr( void )
967 967 {
968 968 set_lfr_soft_reset( 1 );
969 969
970 970 set_lfr_soft_reset( 0 );
971 971 }
Show file before | Show file after | Hide comments
@@ -1,1347 +1,1402
1 1 /** Functions and tasks related to waveform packet generation.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle waveforms, in snapshot or continuous format.\n
7 7 *
8 8 */
9 9
10 10 #include "wf_handler.h"
11 11
12 12 //***************
13 13 // waveform rings
14 14 // F0
15 15 ring_node waveform_ring_f0[NB_RING_NODES_F0];
16 16 ring_node *current_ring_node_f0;
17 17 ring_node *ring_node_to_send_swf_f0;
18 18 // F1
19 19 ring_node waveform_ring_f1[NB_RING_NODES_F1];
20 20 ring_node *current_ring_node_f1;
21 21 ring_node *ring_node_to_send_swf_f1;
22 22 ring_node *ring_node_to_send_cwf_f1;
23 23 // F2
24 24 ring_node waveform_ring_f2[NB_RING_NODES_F2];
25 25 ring_node *current_ring_node_f2;
26 26 ring_node *ring_node_to_send_swf_f2;
27 27 ring_node *ring_node_to_send_cwf_f2;
28 28 // F3
29 29 ring_node waveform_ring_f3[NB_RING_NODES_F3];
30 30 ring_node *current_ring_node_f3;
31 31 ring_node *ring_node_to_send_cwf_f3;
32 32
33 33 bool extractSWF = false;
34 34 bool swf_f0_ready = false;
35 35 bool swf_f1_ready = false;
36 36 bool swf_f2_ready = false;
37 37
38 38 int wf_snap_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ];
39 39 ring_node ring_node_wf_snap_extracted;
40 40
41 41 //*********************
42 42 // Interrupt SubRoutine
43 43
44 ring_node * getRingNodeToSendCWF( unsigned char frequencyChannel)
45 {
46 ring_node *node;
47
48 node = NULL;
49 switch ( frequencyChannel ) {
50 case 1:
51 node = ring_node_to_send_cwf_f1;
52 break;
53 case 2:
54 node = ring_node_to_send_cwf_f2;
55 break;
56 case 3:
57 node = ring_node_to_send_cwf_f3;
58 break;
59 default:
60 break;
61 }
62
63 return node;
64 }
65
66 ring_node * getRingNodeToSendSWF( unsigned char frequencyChannel)
67 {
68 ring_node *node;
69
70 node = NULL;
71 switch ( frequencyChannel ) {
72 case 0:
73 node = ring_node_to_send_swf_f0;
74 break;
75 case 1:
76 node = ring_node_to_send_swf_f1;
77 break;
78 case 2:
79 node = ring_node_to_send_swf_f2;
80 break;
81 default:
82 break;
83 }
84
85 return node;
86 }
87
44 88 void reset_extractSWF( void )
45 89 {
46 90 extractSWF = false;
47 91 swf_f0_ready = false;
48 92 swf_f1_ready = false;
49 93 swf_f2_ready = false;
50 94 }
51 95
52 96 inline void waveforms_isr_f3( void )
53 97 {
54 98 rtems_status_code spare_status;
55 99
56 100 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_BURST) // in BURST the data are used to place v, e1 and e2 in the HK packet
57 101 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
58 102 { // in modes other than STANDBY and BURST, send the CWF_F3 data
59 103 //***
60 104 // F3
61 105 if ( (waveform_picker_regs->status & 0xc0) != 0x00 ) { // [1100 0000] check the f3 full bits
62 106 ring_node_to_send_cwf_f3 = current_ring_node_f3->previous;
63 107 current_ring_node_f3 = current_ring_node_f3->next;
64 108 if ((waveform_picker_regs->status & 0x40) == 0x40){ // [0100 0000] f3 buffer 0 is full
65 109 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_0_coarse_time;
66 110 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_0_fine_time;
67 111 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->buffer_address;
68 112 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008840; // [1000 1000 0100 0000]
69 113 }
70 114 else if ((waveform_picker_regs->status & 0x80) == 0x80){ // [1000 0000] f3 buffer 1 is full
71 115 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_1_coarse_time;
72 116 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_1_fine_time;
73 117 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address;
74 118 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008880; // [1000 1000 1000 0000]
75 119 }
76 120 if (rtems_event_send( Task_id[TASKID_CWF3], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
77 121 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
78 122 }
79 123 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_2);
80 124 }
81 125 }
82 126 }
83 127
84 128 inline void waveforms_isr_normal( void )
85 129 {
86 130 rtems_status_code status;
87 131
88 132 if ( ( (waveform_picker_regs->status & 0x30) != 0x00 ) // [0011 0000] check the f2 full bits
89 133 && ( (waveform_picker_regs->status & 0x0c) != 0x00 ) // [0000 1100] check the f1 full bits
90 134 && ( (waveform_picker_regs->status & 0x03) != 0x00 )) // [0000 0011] check the f0 full bits
91 135 {
92 136 //***
93 137 // F0
94 138 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
95 139 current_ring_node_f0 = current_ring_node_f0->next;
96 140 if ( (waveform_picker_regs->status & 0x01) == 0x01)
97 141 {
98 142
99 143 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
100 144 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
101 145 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
102 146 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
103 147 }
104 148 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
105 149 {
106 150 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
107 151 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
108 152 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
109 153 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
110 154 }
111 155
112 156 //***
113 157 // F1
114 158 ring_node_to_send_swf_f1 = current_ring_node_f1->previous;
115 159 current_ring_node_f1 = current_ring_node_f1->next;
116 160 if ( (waveform_picker_regs->status & 0x04) == 0x04)
117 161 {
118 162 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
119 163 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
120 164 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
121 165 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
122 166 }
123 167 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
124 168 {
125 169 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
126 170 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
127 171 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
128 172 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
129 173 }
130 174
131 175 //***
132 176 // F2
133 177 ring_node_to_send_swf_f2 = current_ring_node_f2->previous;
134 178 current_ring_node_f2 = current_ring_node_f2->next;
135 179 if ( (waveform_picker_regs->status & 0x10) == 0x10)
136 180 {
137 181 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
138 182 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
139 183 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
140 184 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
141 185 }
142 186 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
143 187 {
144 188 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
145 189 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
146 190 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
147 191 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
148 192 }
149 193 //
150 194 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_NORMAL );
151 195 if ( status != RTEMS_SUCCESSFUL)
152 196 {
153 197 status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
154 198 }
155 199 }
156 200 }
157 201
158 202 inline void waveforms_isr_burst( void )
159 203 {
204 unsigned char status;
160 205 rtems_status_code spare_status;
161 206
162 if ( (waveform_picker_regs->status & 0x30) != 0 ){ // [0100] check the f2 full bit
163 // (1) change the receiving buffer for the waveform picker
207 status = (waveform_picker_regs->status & 0x30) >> 4; // [0011 0000] get the status_ready_matrix_f0_x bits
208
209 switch(status)
210 {
211 case 1:
164 212 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
165 213 current_ring_node_f2 = current_ring_node_f2->next;
166 if ( (waveform_picker_regs->status & 0x10) == 0x10)
167 {
168 214 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
169 215 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
170 216 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
171 217 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
218 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
219 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
172 220 }
173 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
174 {
221 break;
222 case 2:
223 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
224 current_ring_node_f2 = current_ring_node_f2->next;
175 225 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
176 226 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
177 227 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
178 228 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
179 }
180 // (2) send an event for the waveforms transmission
181 229 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
182 230 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
183 231 }
232 break;
233 default:
234 break;
184 235 }
185 236 }
186 237
187 238 inline void waveforms_isr_sbm1( void )
188 239 {
189 240 rtems_status_code status;
190 241
191 242 //***
192 243 // F1
193 244 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bits
194 245 // (1) change the receiving buffer for the waveform picker
195 246 ring_node_to_send_cwf_f1 = current_ring_node_f1->previous;
196 247 current_ring_node_f1 = current_ring_node_f1->next;
197 248 if ( (waveform_picker_regs->status & 0x04) == 0x04)
198 249 {
199 250 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
200 251 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
201 252 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
202 253 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
203 254 }
204 255 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
205 256 {
206 257 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
207 258 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
208 259 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
209 260 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
210 261 }
211 262 // (2) send an event for the the CWF1 task for transmission (and snapshot extraction if needed)
212 263 status = rtems_event_send( Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_SBM1 );
213 264 }
214 265
215 266 //***
216 267 // F0
217 268 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) { // [0000 0011] check the f0 full bits
218 269 swf_f0_ready = true;
219 270 // change f0 buffer
220 271 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
221 272 current_ring_node_f0 = current_ring_node_f0->next;
222 273 if ( (waveform_picker_regs->status & 0x01) == 0x01)
223 274 {
224 275
225 276 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
226 277 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
227 278 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
228 279 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
229 280 }
230 281 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
231 282 {
232 283 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
233 284 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
234 285 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
235 286 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
236 287 }
237 288 }
238 289
239 290 //***
240 291 // F2
241 292 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bits
242 293 swf_f2_ready = true;
243 294 // change f2 buffer
244 295 ring_node_to_send_swf_f2 = current_ring_node_f2->previous;
245 296 current_ring_node_f2 = current_ring_node_f2->next;
246 297 if ( (waveform_picker_regs->status & 0x10) == 0x10)
247 298 {
248 299 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
249 300 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
250 301 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
251 302 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
252 303 }
253 304 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
254 305 {
255 306 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
256 307 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
257 308 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
258 309 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
259 310 }
260 311 }
261 312 }
262 313
263 314 inline void waveforms_isr_sbm2( void )
264 315 {
265 316 rtems_status_code status;
266 317
267 318 //***
268 319 // F2
269 320 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bit
270 321 // (1) change the receiving buffer for the waveform picker
271 322 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
272 323 current_ring_node_f2 = current_ring_node_f2->next;
273 324 if ( (waveform_picker_regs->status & 0x10) == 0x10)
274 325 {
275 326 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
276 327 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
277 328 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
278 329 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
279 330 }
280 331 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
281 332 {
282 333 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
283 334 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
284 335 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
285 336 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
286 337 }
287 338 // (2) send an event for the waveforms transmission
288 339 status = rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_SBM2 );
289 340 }
290 341
291 342 //***
292 343 // F0
293 344 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) { // [0000 0011] check the f0 full bit
294 345 swf_f0_ready = true;
295 346 // change f0 buffer
296 347 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
297 348 current_ring_node_f0 = current_ring_node_f0->next;
298 349 if ( (waveform_picker_regs->status & 0x01) == 0x01)
299 350 {
300 351
301 352 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
302 353 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
303 354 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
304 355 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
305 356 }
306 357 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
307 358 {
308 359 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
309 360 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
310 361 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
311 362 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
312 363 }
313 364 }
314 365
315 366 //***
316 367 // F1
317 368 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bit
318 369 swf_f1_ready = true;
319 370 ring_node_to_send_swf_f1 = current_ring_node_f1->previous;
320 371 current_ring_node_f1 = current_ring_node_f1->next;
321 372 if ( (waveform_picker_regs->status & 0x04) == 0x04)
322 373 {
323 374 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
324 375 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
325 376 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
326 377 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
327 378 }
328 379 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
329 380 {
330 381 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
331 382 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
332 383 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
333 384 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
334 385 }
335 386 }
336 387 }
337 388
338 389 rtems_isr waveforms_isr( rtems_vector_number vector )
339 390 {
340 391 /** This is the interrupt sub routine called by the waveform picker core.
341 392 *
342 393 * This ISR launch different actions depending mainly on two pieces of information:
343 394 * 1. the values read in the registers of the waveform picker.
344 395 * 2. the current LFR mode.
345 396 *
346 397 */
347 398
348 399 // STATUS
349 400 // new error error buffer full
350 401 // 15 14 13 12 11 10 9 8
351 402 // f3 f2 f1 f0 f3 f2 f1 f0
352 403 //
353 404 // ready buffer
354 405 // 7 6 5 4 3 2 1 0
355 406 // f3_1 f3_0 f2_1 f2_0 f1_1 f1_0 f0_1 f0_0
356 407
357 408 rtems_status_code spare_status;
358 409
359 410 waveforms_isr_f3();
360 411
361 412 if ( (waveform_picker_regs->status & 0xff00) != 0x00) // [1111 1111 0000 0000] check the error bits
362 413 {
363 414 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_10 );
364 415 }
365 416
366 417 switch(lfrCurrentMode)
367 418 {
368 419 //********
369 420 // STANDBY
370 421 case(LFR_MODE_STANDBY):
371 422 break;
372 423
373 424 //******
374 425 // NORMAL
375 426 case(LFR_MODE_NORMAL):
376 427 waveforms_isr_normal();
377 428 break;
378 429
379 430 //******
380 431 // BURST
381 432 case(LFR_MODE_BURST):
382 433 waveforms_isr_burst();
383 434 break;
384 435
385 436 //*****
386 437 // SBM1
387 438 case(LFR_MODE_SBM1):
388 439 waveforms_isr_sbm1();
389 440 break;
390 441
391 442 //*****
392 443 // SBM2
393 444 case(LFR_MODE_SBM2):
394 445 waveforms_isr_sbm2();
395 446 break;
396 447
397 448 //********
398 449 // DEFAULT
399 450 default:
400 451 break;
401 452 }
402 453 }
403 454
404 455 //************
405 456 // RTEMS TASKS
406 457
407 458 rtems_task wfrm_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
408 459 {
409 460 /** This RTEMS task is dedicated to the transmission of snapshots of the NORMAL mode.
410 461 *
411 462 * @param unused is the starting argument of the RTEMS task
412 463 *
413 464 * The following data packets are sent by this task:
414 465 * - TM_LFR_SCIENCE_NORMAL_SWF_F0
415 466 * - TM_LFR_SCIENCE_NORMAL_SWF_F1
416 467 * - TM_LFR_SCIENCE_NORMAL_SWF_F2
417 468 *
418 469 */
419 470
420 471 rtems_event_set event_out;
421 472 rtems_id queue_id;
422 473 rtems_status_code status;
423 474 bool resynchronisationEngaged;
424 475 ring_node *ring_node_wf_snap_extracted_ptr;
425 476
426 477 ring_node_wf_snap_extracted_ptr = (ring_node *) &ring_node_wf_snap_extracted;
427 478
428 479 resynchronisationEngaged = false;
429 480
430 481 status = get_message_queue_id_send( &queue_id );
431 482 if (status != RTEMS_SUCCESSFUL)
432 483 {
433 484 PRINTF1("in WFRM *** ERR get_message_queue_id_send %d\n", status)
434 485 }
435 486
436 487 BOOT_PRINTF("in WFRM ***\n")
437 488
438 489 while(1){
439 490 // wait for an RTEMS_EVENT
440 491 rtems_event_receive(RTEMS_EVENT_MODE_NORMAL | RTEMS_EVENT_MODE_SBM1
441 492 | RTEMS_EVENT_MODE_SBM2 | RTEMS_EVENT_MODE_SBM2_WFRM,
442 493 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
443 494 if(resynchronisationEngaged == false)
444 495 { // engage resynchronisation
445 // snapshot_resynchronization( (unsigned char *) ring_node_to_send_swf_f0->coarseTime );
496 snapshot_resynchronization( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
446 497 resynchronisationEngaged = true;
447 498 }
448 499 else
449 500 { // reset delta_snapshot to the nominal value
450 501 PRINTF("no resynchronisation, reset delta_snapshot to the nominal value\n")
451 // set_wfp_delta_snapshot();
502 set_wfp_delta_snapshot();
452 503 resynchronisationEngaged = false;
453 504 }
454 505 //
455 506
456 507 if (event_out == RTEMS_EVENT_MODE_NORMAL)
457 508 {
458 509 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_NORMAL\n")
459 510 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
460 511 ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1;
461 512 ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2;
462 513 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
463 514 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) );
464 515 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) );
465 516 }
466 517 if (event_out == RTEMS_EVENT_MODE_SBM1)
467 518 {
468 519 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM1\n")
469 520 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
470 521 ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F1;
471 522 ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2;
472 523 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
473 524 status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) );
474 525 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) );
475 526 }
476 527 if (event_out == RTEMS_EVENT_MODE_SBM2)
477 528 {
478 529 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM2\n")
479 530 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
480 531 ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1;
481 532 ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F2;
482 533 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
483 534 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) );
484 535 status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) );
485 536 }
486 537 }
487 538 }
488 539
489 540 rtems_task cwf3_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
490 541 {
491 542 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f3.
492 543 *
493 544 * @param unused is the starting argument of the RTEMS task
494 545 *
495 546 * The following data packet is sent by this task:
496 547 * - TM_LFR_SCIENCE_NORMAL_CWF_F3
497 548 *
498 549 */
499 550
500 551 rtems_event_set event_out;
501 552 rtems_id queue_id;
502 553 rtems_status_code status;
503 554 ring_node ring_node_cwf3_light;
504 555
505 556 status = get_message_queue_id_send( &queue_id );
506 557 if (status != RTEMS_SUCCESSFUL)
507 558 {
508 559 PRINTF1("in CWF3 *** ERR get_message_queue_id_send %d\n", status)
509 560 }
510 561
511 562 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
512 563
513 564 // init the ring_node_cwf3_light structure
514 565 ring_node_cwf3_light.buffer_address = (int) wf_cont_f3_light;
515 566 ring_node_cwf3_light.coarseTime = 0x00;
516 567 ring_node_cwf3_light.fineTime = 0x00;
517 568 ring_node_cwf3_light.next = NULL;
518 569 ring_node_cwf3_light.previous = NULL;
519 570 ring_node_cwf3_light.sid = SID_NORM_CWF_F3;
520 571 ring_node_cwf3_light.status = 0x00;
521 572
522 573 BOOT_PRINTF("in CWF3 ***\n")
523 574
524 575 while(1){
525 576 // wait for an RTEMS_EVENT
526 577 rtems_event_receive( RTEMS_EVENT_0,
527 578 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
528 579 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
529 580 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode==LFR_MODE_SBM2) )
530 581 {
531 582 if ( (parameter_dump_packet.sy_lfr_n_cwf_long_f3 & 0x01) == 0x01)
532 583 {
533 584 PRINTF("send CWF_LONG_F3\n")
534 585 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
535 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf_f2, sizeof( ring_node* ) );
586 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf_f3, sizeof( ring_node* ) );
536 587 }
537 588 else
538 589 {
539 590 PRINTF("send CWF_F3 (light)\n")
540 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_F3;
541 591 send_waveform_CWF3_light( ring_node_to_send_cwf_f3, &ring_node_cwf3_light, queue_id );
542 592 }
543 593
544 594 }
545 595 else
546 596 {
547 597 PRINTF1("in CWF3 *** lfrCurrentMode is %d, no data will be sent\n", lfrCurrentMode)
548 598 }
549 599 }
550 600 }
551 601
552 602 rtems_task cwf2_task(rtems_task_argument argument) // ONLY USED IN BURST AND SBM2
553 603 {
554 604 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f2.
555 605 *
556 606 * @param unused is the starting argument of the RTEMS task
557 607 *
558 608 * The following data packet is sent by this function:
559 609 * - TM_LFR_SCIENCE_BURST_CWF_F2
560 610 * - TM_LFR_SCIENCE_SBM2_CWF_F2
561 611 *
562 612 */
563 613
564 614 rtems_event_set event_out;
565 615 rtems_id queue_id;
566 616 rtems_status_code status;
617 ring_node *ring_node_to_send;
567 618
568 619 status = get_message_queue_id_send( &queue_id );
569 620 if (status != RTEMS_SUCCESSFUL)
570 621 {
571 622 PRINTF1("in CWF2 *** ERR get_message_queue_id_send %d\n", status)
572 623 }
573 624
574 625 BOOT_PRINTF("in CWF2 ***\n")
575 626
576 627 while(1){
577 628 // wait for an RTEMS_EVENT
578 629 rtems_event_receive( RTEMS_EVENT_MODE_BURST | RTEMS_EVENT_MODE_SBM2,
579 630 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
631 ring_node_to_send = getRingNodeToSendCWF( 2 );
632 printf("ring_node_to_send_cwf === coarse = %x, fine = %x\n", ring_node_to_send->coarseTime, ring_node_to_send->fineTime);
633 printf("**0** %x . %x", waveform_ring_f2[0].coarseTime, waveform_ring_f2[0].fineTime);
634 printf(" **1** %x . %x", waveform_ring_f2[1].coarseTime, waveform_ring_f2[1].fineTime);
635 printf(" **2** %x . %x", waveform_ring_f2[2].coarseTime, waveform_ring_f2[2].fineTime);
636 printf(" **3** %x . %x", waveform_ring_f2[3].coarseTime, waveform_ring_f2[3].fineTime);
637 printf(" **4** %x . %x\n", waveform_ring_f2[4].coarseTime, waveform_ring_f2[4].fineTime);
580 638 if (event_out == RTEMS_EVENT_MODE_BURST)
581 639 {
582 // send_waveform_CWF( ring_node_to_send_cwf_f2, SID_BURST_CWF_F2, headerCWF_F2_BURST, queue_id );
583 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
584 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf_f2, sizeof( ring_node* ) );
640 ring_node_to_send->sid = SID_BURST_CWF_F2;
641 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
585 642 }
586 643 if (event_out == RTEMS_EVENT_MODE_SBM2)
587 644 {
588 // send_waveform_CWF( ring_node_to_send_cwf_f2, SID_SBM2_CWF_F2, headerCWF_F2_SBM2, queue_id );
589 ring_node_to_send_cwf_f2->sid = SID_SBM2_CWF_F2;
590 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf_f2, sizeof( ring_node* ) );
645 ring_node_to_send->sid = SID_SBM2_CWF_F2;
646 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
591 647 // launch snapshot extraction if needed
592 648 if (extractSWF == true)
593 649 {
594 ring_node_to_send_swf_f2 = ring_node_to_send_cwf_f2;
650 ring_node_to_send_swf_f2 = ring_node_to_send;
595 651 // extract the snapshot
596 652 build_snapshot_from_ring( ring_node_to_send_swf_f2, 2 );
597 653 // send the snapshot when built
598 654 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM2 );
599 655 extractSWF = false;
600 656 }
601 657 if (swf_f0_ready && swf_f1_ready)
602 658 {
603 659 extractSWF = true;
604 660 swf_f0_ready = false;
605 661 swf_f1_ready = false;
606 662 }
607 663 }
608 664 }
609 665 }
610 666
611 667 rtems_task cwf1_task(rtems_task_argument argument) // ONLY USED IN SBM1
612 668 {
613 669 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f1.
614 670 *
615 671 * @param unused is the starting argument of the RTEMS task
616 672 *
617 673 * The following data packet is sent by this function:
618 674 * - TM_LFR_SCIENCE_SBM1_CWF_F1
619 675 *
620 676 */
621 677
622 678 rtems_event_set event_out;
623 679 rtems_id queue_id;
624 680 rtems_status_code status;
625 681
626 // init_header_continuous_wf_table( SID_SBM1_CWF_F1, headerCWF_F1 );
682 ring_node * ring_node_to_send_cwf;
627 683
628 684 status = get_message_queue_id_send( &queue_id );
629 685 if (status != RTEMS_SUCCESSFUL)
630 686 {
631 687 PRINTF1("in CWF1 *** ERR get_message_queue_id_send %d\n", status)
632 688 }
633 689
634 690 BOOT_PRINTF("in CWF1 ***\n")
635 691
636 692 while(1){
637 693 // wait for an RTEMS_EVENT
638 694 rtems_event_receive( RTEMS_EVENT_MODE_SBM1,
639 695 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
696 ring_node_to_send_cwf = getRingNodeToSendCWF( 1 );
697 printf("ring_node_to_send_cwf === coarse = %x, fine = %x\n", ring_node_to_send_cwf->coarseTime, ring_node_to_send_cwf->fineTime);
698 printf("**0** %x . %x", waveform_ring_f1[0].coarseTime, waveform_ring_f1[0].fineTime);
699 printf(" **1** %x . %x", waveform_ring_f1[1].coarseTime, waveform_ring_f1[1].fineTime);
700 printf(" **2** %x . %x", waveform_ring_f1[2].coarseTime, waveform_ring_f1[2].fineTime);
701 printf(" **3** %x . %x", waveform_ring_f1[3].coarseTime, waveform_ring_f1[3].fineTime);
702 printf(" **4** %x . %x\n\n", waveform_ring_f1[4].coarseTime, waveform_ring_f1[4].fineTime);
640 703 ring_node_to_send_cwf_f1->sid = SID_SBM1_CWF_F1;
641 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf_f1, sizeof( ring_node* ) );
704 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
642 705 // launch snapshot extraction if needed
643 706 if (extractSWF == true)
644 707 {
645 ring_node_to_send_swf_f1 = ring_node_to_send_cwf_f1;
708 ring_node_to_send_swf_f1 = ring_node_to_send_cwf;
646 709 // launch the snapshot extraction
647 710 status = rtems_event_send( Task_id[TASKID_SWBD], RTEMS_EVENT_MODE_SBM1 );
648 711 extractSWF = false;
649 712 }
650 713 if (swf_f0_ready == true)
651 714 {
652 715 extractSWF = true;
653 716 swf_f0_ready = false; // this step shall be executed only one time
654 717 }
655 718 if ((swf_f1_ready == true) && (swf_f2_ready == true)) // swf_f1 is ready after the extraction
656 719 {
657 720 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM1 );
658 721 swf_f1_ready = false;
659 722 swf_f2_ready = false;
660 723 }
661 724 }
662 725 }
663 726
664 727 rtems_task swbd_task(rtems_task_argument argument)
665 728 {
666 729 /** This RTEMS task is dedicated to the building of snapshots from different continuous waveforms buffers.
667 730 *
668 731 * @param unused is the starting argument of the RTEMS task
669 732 *
670 733 */
671 734
672 735 rtems_event_set event_out;
673 736
674 737 BOOT_PRINTF("in SWBD ***\n")
675 738
676 739 while(1){
677 740 // wait for an RTEMS_EVENT
678 741 rtems_event_receive( RTEMS_EVENT_MODE_SBM1 | RTEMS_EVENT_MODE_SBM2,
679 742 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
680 743 if (event_out == RTEMS_EVENT_MODE_SBM1)
681 744 {
682 745 build_snapshot_from_ring( ring_node_to_send_swf_f1, 1 );
683 746 swf_f1_ready = true; // the snapshot has been extracted and is ready to be sent
684 747 }
685 748 else
686 749 {
687 750 PRINTF1("in SWBD *** unexpected rtems event received %x\n", (int) event_out)
688 751 }
689 752 }
690 753 }
691 754
692 755 //******************
693 756 // general functions
694 757
695 758 void WFP_init_rings( void )
696 759 {
697 760 // F0 RING
698 761 init_ring( waveform_ring_f0, NB_RING_NODES_F0, wf_buffer_f0, WFRM_BUFFER );
699 762 // F1 RING
700 763 init_ring( waveform_ring_f1, NB_RING_NODES_F1, wf_buffer_f1, WFRM_BUFFER );
701 764 // F2 RING
702 765 init_ring( waveform_ring_f2, NB_RING_NODES_F2, wf_buffer_f2, WFRM_BUFFER );
703 766 // F3 RING
704 767 init_ring( waveform_ring_f3, NB_RING_NODES_F3, wf_buffer_f3, WFRM_BUFFER );
705 768
706 769 ring_node_wf_snap_extracted.buffer_address = (int) wf_snap_extracted;
707 770
708 771 DEBUG_PRINTF1("waveform_ring_f0 @%x\n", (unsigned int) waveform_ring_f0)
709 772 DEBUG_PRINTF1("waveform_ring_f1 @%x\n", (unsigned int) waveform_ring_f1)
710 773 DEBUG_PRINTF1("waveform_ring_f2 @%x\n", (unsigned int) waveform_ring_f2)
711 774 DEBUG_PRINTF1("waveform_ring_f3 @%x\n", (unsigned int) waveform_ring_f3)
712 775 DEBUG_PRINTF1("wf_buffer_f0 @%x\n", (unsigned int) wf_buffer_f0)
713 776 DEBUG_PRINTF1("wf_buffer_f1 @%x\n", (unsigned int) wf_buffer_f1)
714 777 DEBUG_PRINTF1("wf_buffer_f2 @%x\n", (unsigned int) wf_buffer_f2)
715 778 DEBUG_PRINTF1("wf_buffer_f3 @%x\n", (unsigned int) wf_buffer_f3)
716 779
717 780 }
718 781
719 782 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
720 783 {
721 784 unsigned char i;
722 785
723 786 //***************
724 787 // BUFFER ADDRESS
725 788 for(i=0; i<nbNodes; i++)
726 789 {
727 790 ring[i].coarseTime = 0x00;
728 791 ring[i].fineTime = 0x00;
729 792 ring[i].sid = 0x00;
730 793 ring[i].status = 0x00;
731 794 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
732 795 }
733 796
734 797 //*****
735 798 // NEXT
736 799 ring[nbNodes-1].next = (ring_node*) &ring[ 0 ];
737 800 for(i=0; i<nbNodes-1; i++)
738 801 {
739 802 ring[i].next = (ring_node*) &ring[ i + 1 ];
740 803 }
741 804
742 805 //*********
743 806 // PREVIOUS
744 807 ring[0].previous = (ring_node*) &ring[ nbNodes - 1 ];
745 808 for(i=1; i<nbNodes; i++)
746 809 {
747 810 ring[i].previous = (ring_node*) &ring[ i - 1 ];
748 811 }
749 812 }
750 813
751 814 void WFP_reset_current_ring_nodes( void )
752 815 {
753 current_ring_node_f0 = waveform_ring_f0;
754 ring_node_to_send_swf_f0 = waveform_ring_f0;
816 current_ring_node_f0 = waveform_ring_f0[0].next;
817 current_ring_node_f1 = waveform_ring_f1[0].next;
818 current_ring_node_f2 = waveform_ring_f2[0].next;
819 current_ring_node_f3 = waveform_ring_f3[0].next;
755 820
756 current_ring_node_f1 = waveform_ring_f1;
757 ring_node_to_send_cwf_f1 = waveform_ring_f1;
821 ring_node_to_send_swf_f0 = waveform_ring_f0;
758 822 ring_node_to_send_swf_f1 = waveform_ring_f1;
759
760 current_ring_node_f2 = waveform_ring_f2;
761 ring_node_to_send_cwf_f2 = waveform_ring_f2;
762 823 ring_node_to_send_swf_f2 = waveform_ring_f2;
763 824
764 current_ring_node_f3 = waveform_ring_f3;
825 ring_node_to_send_cwf_f1 = waveform_ring_f1;
826 ring_node_to_send_cwf_f2 = waveform_ring_f2;
765 827 ring_node_to_send_cwf_f3 = waveform_ring_f3;
766 828 }
767 829
768 830 int send_waveform_CWF3_light( ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id )
769 831 {
770 832 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
771 833 *
772 834 * @param waveform points to the buffer containing the data that will be send.
773 835 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
774 836 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
775 837 * contain information to setup the transmission of the data packets.
776 838 *
777 839 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
778 840 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
779 841 *
780 842 */
781 843
782 844 unsigned int i;
783 845 int ret;
784 846 rtems_status_code status;
785 spw_ioctl_pkt_send spw_ioctl_send_CWF;
847
786 848 char *sample;
787 849 int *dataPtr;
788 850
789 spw_ioctl_send_CWF.hlen = TM_HEADER_LEN + 4 + 10; // + 4 is for the protocole extra header, + 10 is for the auxiliary header
790 spw_ioctl_send_CWF.options = 0;
791
792 851 ret = LFR_DEFAULT;
793 852
794 853 dataPtr = (int*) ring_node_to_send->buffer_address;
795 854
796 855 ring_node_cwf3_light->coarseTime = ring_node_to_send->coarseTime;
797 856 ring_node_cwf3_light->fineTime = ring_node_to_send->fineTime;
798 857
799 858 //**********************
800 859 // BUILD CWF3_light DATA
801 860 for ( i=0; i< NB_SAMPLES_PER_SNAPSHOT; i++)
802 861 {
803 862 sample = (char*) &dataPtr[ (i * NB_WORDS_SWF_BLK) ];
804 863 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) ] = sample[ 0 ];
805 864 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 1 ] = sample[ 1 ];
806 865 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 2 ] = sample[ 2 ];
807 866 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 3 ] = sample[ 3 ];
808 867 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 4 ] = sample[ 4 ];
809 868 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 5 ] = sample[ 5 ];
810 869 }
811 870
812 871 // SEND PACKET
813 872 status = rtems_message_queue_send( queue_id, &ring_node_cwf3_light, sizeof( ring_node* ) );
814 873 if (status != RTEMS_SUCCESSFUL) {
815 874 printf("%d-%d, ERR %d\n", SID_NORM_CWF_F3, i, (int) status);
816 875 ret = LFR_DEFAULT;
817 876 }
818 877
819 878 return ret;
820 879 }
821 880
822 881 void compute_acquisition_time( unsigned int coarseTime, unsigned int fineTime,
823 882 unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char * acquisitionTime )
824 883 {
825 884 unsigned long long int acquisitionTimeAsLong;
826 885 unsigned char localAcquisitionTime[6];
827 886 double deltaT;
828 887
829 888 deltaT = 0.;
830 889
831 890 localAcquisitionTime[0] = (unsigned char) ( coarseTime >> 24 );
832 891 localAcquisitionTime[1] = (unsigned char) ( coarseTime >> 16 );
833 892 localAcquisitionTime[2] = (unsigned char) ( coarseTime >> 8 );
834 893 localAcquisitionTime[3] = (unsigned char) ( coarseTime );
835 894 localAcquisitionTime[4] = (unsigned char) ( fineTime >> 8 );
836 895 localAcquisitionTime[5] = (unsigned char) ( fineTime );
837 896
838 897 acquisitionTimeAsLong = ( (unsigned long long int) localAcquisitionTime[0] << 40 )
839 898 + ( (unsigned long long int) localAcquisitionTime[1] << 32 )
840 899 + ( (unsigned long long int) localAcquisitionTime[2] << 24 )
841 900 + ( (unsigned long long int) localAcquisitionTime[3] << 16 )
842 901 + ( (unsigned long long int) localAcquisitionTime[4] << 8 )
843 902 + ( (unsigned long long int) localAcquisitionTime[5] );
844 903
845 904 switch( sid )
846 905 {
847 906 case SID_NORM_SWF_F0:
848 907 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 24576. ;
849 908 break;
850 909
851 910 case SID_NORM_SWF_F1:
852 911 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 4096. ;
853 912 break;
854 913
855 914 case SID_NORM_SWF_F2:
856 915 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 256. ;
857 916 break;
858 917
859 918 case SID_SBM1_CWF_F1:
860 919 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 4096. ;
861 920 break;
862 921
863 922 case SID_SBM2_CWF_F2:
864 923 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
865 924 break;
866 925
867 926 case SID_BURST_CWF_F2:
868 927 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
869 928 break;
870 929
871 930 case SID_NORM_CWF_F3:
872 931 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF_SHORT_F3 * 65536. / 16. ;
873 932 break;
874 933
875 934 case SID_NORM_CWF_LONG_F3:
876 935 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 16. ;
877 936 break;
878 937
879 938 default:
880 939 PRINTF1("in compute_acquisition_time *** ERR unexpected sid %d\n", sid)
881 940 deltaT = 0.;
882 941 break;
883 942 }
884 943
885 944 acquisitionTimeAsLong = acquisitionTimeAsLong + (unsigned long long int) deltaT;
886 945 //
887 946 acquisitionTime[0] = (unsigned char) (acquisitionTimeAsLong >> 40);
888 947 acquisitionTime[1] = (unsigned char) (acquisitionTimeAsLong >> 32);
889 948 acquisitionTime[2] = (unsigned char) (acquisitionTimeAsLong >> 24);
890 949 acquisitionTime[3] = (unsigned char) (acquisitionTimeAsLong >> 16);
891 950 acquisitionTime[4] = (unsigned char) (acquisitionTimeAsLong >> 8 );
892 951 acquisitionTime[5] = (unsigned char) (acquisitionTimeAsLong );
893 952
894 953 }
895 954
896 955 void build_snapshot_from_ring( ring_node *ring_node_to_send, unsigned char frequencyChannel )
897 956 {
898 957 unsigned int i;
899 958 unsigned long long int centerTime_asLong;
900 959 unsigned long long int acquisitionTimeF0_asLong;
901 960 unsigned long long int acquisitionTime_asLong;
902 961 unsigned long long int bufferAcquisitionTime_asLong;
903 962 unsigned char *ptr1;
904 963 unsigned char *ptr2;
905 964 unsigned char *timeCharPtr;
906 965 unsigned char nb_ring_nodes;
907 966 unsigned long long int frequency_asLong;
908 967 unsigned long long int nbTicksPerSample_asLong;
909 968 unsigned long long int nbSamplesPart1_asLong;
910 969 unsigned long long int sampleOffset_asLong;
911 970
912 971 unsigned int deltaT_F0;
913 972 unsigned int deltaT_F1;
914 973 unsigned long long int deltaT_F2;
915 974
916 975 deltaT_F0 = 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
917 976 deltaT_F1 = 16384; // (2048. / 4096. / 2.) * 65536. = 16384;
918 977 deltaT_F2 = 262144; // (2048. / 256. / 2.) * 65536. = 262144;
919 978 sampleOffset_asLong = 0x00;
920 979
921 980 // (1) get the f0 acquisition time
922 981 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send->coarseTime );
923 982
924 983 // (2) compute the central reference time
925 984 centerTime_asLong = acquisitionTimeF0_asLong + deltaT_F0;
926 985
927 986 // (3) compute the acquisition time of the current snapshot
928 987 switch(frequencyChannel)
929 988 {
930 989 case 1: // 1 is for F1 = 4096 Hz
931 990 acquisitionTime_asLong = centerTime_asLong - deltaT_F1;
932 991 nb_ring_nodes = NB_RING_NODES_F1;
933 992 frequency_asLong = 4096;
934 993 nbTicksPerSample_asLong = 16; // 65536 / 4096;
935 994 break;
936 995 case 2: // 2 is for F2 = 256 Hz
937 996 acquisitionTime_asLong = centerTime_asLong - deltaT_F2;
938 997 nb_ring_nodes = NB_RING_NODES_F2;
939 998 frequency_asLong = 256;
940 999 nbTicksPerSample_asLong = 256; // 65536 / 256;
941 1000 break;
942 1001 default:
943 1002 acquisitionTime_asLong = centerTime_asLong;
944 1003 frequency_asLong = 256;
945 1004 nbTicksPerSample_asLong = 256;
946 1005 break;
947 1006 }
948 1007
949 1008 //****************************************************************************
950 1009 // (4) search the ring_node with the acquisition time <= acquisitionTime_asLong
951 1010 for (i=0; i<nb_ring_nodes; i++)
952 1011 {
953 1012 PRINTF1("%d ... ", i)
954 1013 bufferAcquisitionTime_asLong = get_acquisition_time( (unsigned char *) ring_node_to_send->coarseTime );
955 1014 if (bufferAcquisitionTime_asLong <= acquisitionTime_asLong)
956 1015 {
957 1016 PRINTF1("buffer found with acquisition time = %llx\n", bufferAcquisitionTime_asLong)
958 1017 break;
959 1018 }
960 1019 ring_node_to_send = ring_node_to_send->previous;
961 1020 }
962 1021
963 1022 // (5) compute the number of samples to take in the current buffer
964 1023 sampleOffset_asLong = ((acquisitionTime_asLong - bufferAcquisitionTime_asLong) * frequency_asLong ) >> 16;
965 1024 nbSamplesPart1_asLong = NB_SAMPLES_PER_SNAPSHOT - sampleOffset_asLong;
966 1025 PRINTF2("sampleOffset_asLong = %llx, nbSamplesPart1_asLong = %llx\n", sampleOffset_asLong, nbSamplesPart1_asLong)
967 1026
968 1027 // (6) compute the final acquisition time
969 1028 acquisitionTime_asLong = bufferAcquisitionTime_asLong +
970 1029 sampleOffset_asLong * nbTicksPerSample_asLong;
971 1030
972 1031 // (7) copy the acquisition time at the beginning of the extrated snapshot
973 1032 ptr1 = (unsigned char*) &acquisitionTime_asLong;
974 1033 // fine time
975 1034 ptr2 = (unsigned char*) &ring_node_wf_snap_extracted.fineTime;
976 1035 ptr2[2] = ptr1[ 4 + 2 ];
977 1036 ptr2[3] = ptr1[ 5 + 2 ];
978 1037 // coarse time
979 1038 ptr2 = (unsigned char*) &ring_node_wf_snap_extracted.coarseTime;
980 1039 ptr2[0] = ptr1[ 0 + 2 ];
981 1040 ptr2[1] = ptr1[ 1 + 2 ];
982 1041 ptr2[2] = ptr1[ 2 + 2 ];
983 1042 ptr2[3] = ptr1[ 3 + 2 ];
984 1043
985 1044 // re set the synchronization bit
986 1045 timeCharPtr = (unsigned char*) &ring_node_to_send->coarseTime;
987 1046 ptr2[0] = ptr2[0] | (timeCharPtr[0] & 0x80); // [1000 0000]
988 1047
989 1048 if ( (nbSamplesPart1_asLong >= NB_SAMPLES_PER_SNAPSHOT) | (nbSamplesPart1_asLong < 0) )
990 1049 {
991 1050 nbSamplesPart1_asLong = 0;
992 1051 }
993 1052 // copy the part 1 of the snapshot in the extracted buffer
994 1053 for ( i = 0; i < (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i++ )
995 1054 {
996 1055 wf_snap_extracted[i] =
997 1056 ((int*) ring_node_to_send->buffer_address)[ i + (sampleOffset_asLong * NB_WORDS_SWF_BLK) ];
998 1057 }
999 1058 // copy the part 2 of the snapshot in the extracted buffer
1000 1059 ring_node_to_send = ring_node_to_send->next;
1001 1060 for ( i = (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i < (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK); i++ )
1002 1061 {
1003 1062 wf_snap_extracted[i] =
1004 1063 ((int*) ring_node_to_send->buffer_address)[ (i-(nbSamplesPart1_asLong * NB_WORDS_SWF_BLK)) ];
1005 1064 }
1006 1065 }
1007 1066
1008 1067 void snapshot_resynchronization( unsigned char *timePtr )
1009 1068 {
1010 1069 unsigned long long int acquisitionTime;
1011 1070 unsigned long long int centerTime;
1012 1071 unsigned long long int previousTick;
1013 1072 unsigned long long int nextTick;
1014 1073 unsigned long long int deltaPreviousTick;
1015 1074 unsigned long long int deltaNextTick;
1016 1075 unsigned int deltaTickInF2;
1017 1076 double deltaPrevious;
1018 1077 double deltaNext;
1019 1078
1020 1079 acquisitionTime = get_acquisition_time( timePtr );
1021 1080
1022 1081 // compute center time
1023 1082 centerTime = acquisitionTime + 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
1024 1083 previousTick = centerTime - (centerTime & 0xffff);
1025 1084 nextTick = previousTick + 65536;
1026 1085
1027 1086 deltaPreviousTick = centerTime - previousTick;
1028 1087 deltaNextTick = nextTick - centerTime;
1029 1088
1030 1089 deltaPrevious = ((double) deltaPreviousTick) / 65536. * 1000.;
1031 1090 deltaNext = ((double) deltaNextTick) / 65536. * 1000.;
1032 1091
1033 1092 printf("delta previous = %f ms, delta next = %f ms\n", deltaPrevious, deltaNext);
1034 1093 printf("delta previous = %llu, delta next = %llu\n", deltaPreviousTick, deltaNextTick);
1035 1094
1036 1095 // which tick is the closest
1037 1096 if (deltaPreviousTick > deltaNextTick)
1038 1097 {
1039 1098 deltaTickInF2 = floor( (deltaNext * 256. / 1000.) ); // the division by 2 is important here
1040 1099 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot + deltaTickInF2;
1041 1100 printf("correction of = + %u\n", deltaTickInF2);
1042 1101 }
1043 1102 else
1044 1103 {
1045 1104 deltaTickInF2 = floor( (deltaPrevious * 256. / 1000.) ); // the division by 2 is important here
1046 1105 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot - deltaTickInF2;
1047 1106 printf("correction of = - %u\n", deltaTickInF2);
1048 1107 }
1049 1108 }
1050 1109
1051 1110 //**************
1052 1111 // wfp registers
1053 1112 void reset_wfp_burst_enable( void )
1054 1113 {
1055 1114 /** This function resets the waveform picker burst_enable register.
1056 1115 *
1057 1116 * The burst bits [f2 f1 f0] and the enable bits [f3 f2 f1 f0] are set to 0.
1058 1117 *
1059 1118 */
1060 1119
1061 1120 // [1000 000] burst f2, f1, f0 enable f3, f2, f1, f0
1062 1121 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable & 0x80;
1063 1122 }
1064 1123
1065 1124 void reset_wfp_status( void )
1066 1125 {
1067 1126 /** This function resets the waveform picker status register.
1068 1127 *
1069 1128 * All status bits are set to 0 [new_err full_err full].
1070 1129 *
1071 1130 */
1072 1131
1073 1132 waveform_picker_regs->status = 0xffff;
1074 1133 }
1075 1134
1076 1135 void reset_wfp_buffer_addresses( void )
1077 1136 {
1078 1137 // F0
1079 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address; // 0x08
1080 current_ring_node_f0 = current_ring_node_f0->next;
1138 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->previous->buffer_address; // 0x08
1081 1139 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address; // 0x0c
1082 1140 // F1
1083 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address; // 0x10
1084 current_ring_node_f1 = current_ring_node_f1->next;
1141 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->previous->buffer_address; // 0x10
1085 1142 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address; // 0x14
1086 1143 // F2
1087 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address; // 0x18
1088 current_ring_node_f2 = current_ring_node_f2->next;
1144 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->previous->buffer_address; // 0x18
1089 1145 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; // 0x1c
1090 1146 // F3
1091 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->buffer_address; // 0x20
1092 current_ring_node_f3 = current_ring_node_f3->next;
1147 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->previous->buffer_address; // 0x20
1093 1148 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address; // 0x24
1094 1149 }
1095 1150
1096 1151 void reset_waveform_picker_regs( void )
1097 1152 {
1098 1153 /** This function resets the waveform picker module registers.
1099 1154 *
1100 1155 * The registers affected by this function are located at the following offset addresses:
1101 1156 * - 0x00 data_shaping
1102 1157 * - 0x04 run_burst_enable
1103 1158 * - 0x08 addr_data_f0
1104 1159 * - 0x0C addr_data_f1
1105 1160 * - 0x10 addr_data_f2
1106 1161 * - 0x14 addr_data_f3
1107 1162 * - 0x18 status
1108 1163 * - 0x1C delta_snapshot
1109 1164 * - 0x20 delta_f0
1110 1165 * - 0x24 delta_f0_2
1111 1166 * - 0x28 delta_f1
1112 1167 * - 0x2c delta_f2
1113 1168 * - 0x30 nb_data_by_buffer
1114 1169 * - 0x34 nb_snapshot_param
1115 1170 * - 0x38 start_date
1116 1171 * - 0x3c nb_word_in_buffer
1117 1172 *
1118 1173 */
1119 1174
1120 1175 set_wfp_data_shaping(); // 0x00 *** R1 R0 SP1 SP0 BW
1121 1176
1122 1177 reset_wfp_burst_enable(); // 0x04 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
1123 1178
1124 1179 reset_wfp_buffer_addresses();
1125 1180
1126 1181 reset_wfp_status(); // 0x18
1127 1182
1128 1183 set_wfp_delta_snapshot(); // 0x1c *** 300 s => 0x12bff
1129 1184
1130 1185 set_wfp_delta_f0_f0_2(); // 0x20, 0x24
1131 1186
1132 1187 set_wfp_delta_f1(); // 0x28
1133 1188
1134 1189 set_wfp_delta_f2(); // 0x2c
1135 1190
1136 1191 DEBUG_PRINTF1("delta_snapshot %x\n", waveform_picker_regs->delta_snapshot)
1137 1192 DEBUG_PRINTF1("delta_f0 %x\n", waveform_picker_regs->delta_f0)
1138 1193 DEBUG_PRINTF1("delta_f0_2 %x\n", waveform_picker_regs->delta_f0_2)
1139 1194 DEBUG_PRINTF1("delta_f1 %x\n", waveform_picker_regs->delta_f1)
1140 1195 DEBUG_PRINTF1("delta_f2 %x\n", waveform_picker_regs->delta_f2)
1141 1196 // 2688 = 8 * 336
1142 1197 waveform_picker_regs->nb_data_by_buffer = 0xa7f; // 0x30 *** 2688 - 1 => nb samples -1
1143 1198 waveform_picker_regs->snapshot_param = 0xa80; // 0x34 *** 2688 => nb samples
1144 1199 waveform_picker_regs->start_date = 0x7fffffff; // 0x38
1145 1200 //
1146 1201 // coarse time and fine time registers are not initialized, they are volatile
1147 1202 //
1148 1203 waveform_picker_regs->buffer_length = 0x1f8;// buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
1149 1204 }
1150 1205
1151 1206 void set_wfp_data_shaping( void )
1152 1207 {
1153 1208 /** This function sets the data_shaping register of the waveform picker module.
1154 1209 *
1155 1210 * The value is read from one field of the parameter_dump_packet structure:\n
1156 1211 * bw_sp0_sp1_r0_r1
1157 1212 *
1158 1213 */
1159 1214
1160 1215 unsigned char data_shaping;
1161 1216
1162 1217 // get the parameters for the data shaping [BW SP0 SP1 R0 R1] in sy_lfr_common1 and configure the register
1163 1218 // waveform picker : [R1 R0 SP1 SP0 BW]
1164 1219
1165 1220 data_shaping = parameter_dump_packet.bw_sp0_sp1_r0_r1;
1166 1221
1167 1222 waveform_picker_regs->data_shaping =
1168 1223 ( (data_shaping & 0x10) >> 4 ) // BW
1169 1224 + ( (data_shaping & 0x08) >> 2 ) // SP0
1170 1225 + ( (data_shaping & 0x04) ) // SP1
1171 1226 + ( (data_shaping & 0x02) << 2 ) // R0
1172 1227 + ( (data_shaping & 0x01) << 4 ); // R1
1173 1228 }
1174 1229
1175 1230 void set_wfp_burst_enable_register( unsigned char mode )
1176 1231 {
1177 1232 /** This function sets the waveform picker burst_enable register depending on the mode.
1178 1233 *
1179 1234 * @param mode is the LFR mode to launch.
1180 1235 *
1181 1236 * The burst bits shall be before the enable bits.
1182 1237 *
1183 1238 */
1184 1239
1185 1240 // [0000 0000] burst f2, f1, f0 enable f3 f2 f1 f0
1186 1241 // the burst bits shall be set first, before the enable bits
1187 1242 switch(mode) {
1188 1243 case(LFR_MODE_NORMAL):
1189 1244 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enable
1190 1245 waveform_picker_regs->run_burst_enable = 0x0f; // [0000 1111] enable f3 f2 f1 f0
1191 1246 break;
1192 1247 case(LFR_MODE_BURST):
1193 1248 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1194 1249 // waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x04; // [0100] enable f2
1195 1250 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0c; // [1100] enable f3 AND f2
1196 1251 break;
1197 1252 case(LFR_MODE_SBM1):
1198 1253 waveform_picker_regs->run_burst_enable = 0x20; // [0010 0000] f1 burst enabled
1199 1254 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1200 1255 break;
1201 1256 case(LFR_MODE_SBM2):
1202 1257 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1203 1258 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1204 1259 break;
1205 1260 default:
1206 1261 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enabled, no waveform enabled
1207 1262 break;
1208 1263 }
1209 1264 }
1210 1265
1211 1266 void set_wfp_delta_snapshot( void )
1212 1267 {
1213 1268 /** This function sets the delta_snapshot register of the waveform picker module.
1214 1269 *
1215 1270 * The value is read from two (unsigned char) of the parameter_dump_packet structure:
1216 1271 * - sy_lfr_n_swf_p[0]
1217 1272 * - sy_lfr_n_swf_p[1]
1218 1273 *
1219 1274 */
1220 1275
1221 1276 unsigned int delta_snapshot;
1222 1277 unsigned int delta_snapshot_in_T2;
1223 1278
1224 1279 delta_snapshot = parameter_dump_packet.sy_lfr_n_swf_p[0]*256
1225 1280 + parameter_dump_packet.sy_lfr_n_swf_p[1];
1226 1281
1227 1282 delta_snapshot_in_T2 = delta_snapshot * 256;
1228 1283 waveform_picker_regs->delta_snapshot = delta_snapshot_in_T2 - 1; // max 4 bytes
1229 1284 }
1230 1285
1231 1286 void set_wfp_delta_f0_f0_2( void )
1232 1287 {
1233 1288 unsigned int delta_snapshot;
1234 1289 unsigned int nb_samples_per_snapshot;
1235 1290 float delta_f0_in_float;
1236 1291
1237 1292 delta_snapshot = waveform_picker_regs->delta_snapshot;
1238 1293 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1239 1294 delta_f0_in_float =nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 24576.) * 256.;
1240 1295
1241 1296 waveform_picker_regs->delta_f0 = delta_snapshot - floor( delta_f0_in_float );
1242 1297 waveform_picker_regs->delta_f0_2 = 0x7; // max 7 bits
1243 1298 }
1244 1299
1245 1300 void set_wfp_delta_f1( void )
1246 1301 {
1247 1302 unsigned int delta_snapshot;
1248 1303 unsigned int nb_samples_per_snapshot;
1249 1304 float delta_f1_in_float;
1250 1305
1251 1306 delta_snapshot = waveform_picker_regs->delta_snapshot;
1252 1307 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1253 1308 delta_f1_in_float = nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 4096.) * 256.;
1254 1309
1255 1310 waveform_picker_regs->delta_f1 = delta_snapshot - floor( delta_f1_in_float );
1256 1311 }
1257 1312
1258 1313 void set_wfp_delta_f2()
1259 1314 {
1260 1315 unsigned int delta_snapshot;
1261 1316 unsigned int nb_samples_per_snapshot;
1262 1317
1263 1318 delta_snapshot = waveform_picker_regs->delta_snapshot;
1264 1319 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1265 1320
1266 1321 waveform_picker_regs->delta_f2 = delta_snapshot - nb_samples_per_snapshot / 2;
1267 1322 }
1268 1323
1269 1324 //*****************
1270 1325 // local parameters
1271 1326
1272 1327 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid )
1273 1328 {
1274 1329 /** This function increments the parameter "sequence_cnt" depending on the sid passed in argument.
1275 1330 *
1276 1331 * @param packet_sequence_control is a pointer toward the parameter sequence_cnt to update.
1277 1332 * @param sid is the source identifier of the packet being updated.
1278 1333 *
1279 1334 * REQ-LFR-SRS-5240 / SSS-CP-FS-590
1280 1335 * The sequence counters shall wrap around from 2^14 to zero.
1281 1336 * The sequence counter shall start at zero at startup.
1282 1337 *
1283 1338 * REQ-LFR-SRS-5239 / SSS-CP-FS-580
1284 1339 * All TM_LFR_SCIENCE_ packets are sent to ground, i.e. destination id = 0
1285 1340 *
1286 1341 */
1287 1342
1288 1343 unsigned short *sequence_cnt;
1289 1344 unsigned short segmentation_grouping_flag;
1290 1345 unsigned short new_packet_sequence_control;
1291 1346 rtems_mode initial_mode_set;
1292 1347 rtems_mode current_mode_set;
1293 1348 rtems_status_code status;
1294 1349
1295 1350 //******************************************
1296 1351 // CHANGE THE MODE OF THE CALLING RTEMS TASK
1297 1352 status = rtems_task_mode( RTEMS_NO_PREEMPT, RTEMS_PREEMPT_MASK, &initial_mode_set );
1298 1353
1299 1354 if ( (sid == SID_NORM_SWF_F0) || (sid == SID_NORM_SWF_F1) || (sid == SID_NORM_SWF_F2)
1300 1355 || (sid == SID_NORM_CWF_F3) || (sid == SID_NORM_CWF_LONG_F3)
1301 1356 || (sid == SID_BURST_CWF_F2)
1302 1357 || (sid == SID_NORM_ASM_F0) || (sid == SID_NORM_ASM_F1) || (sid == SID_NORM_ASM_F2)
1303 1358 || (sid == SID_NORM_BP1_F0) || (sid == SID_NORM_BP1_F1) || (sid == SID_NORM_BP1_F2)
1304 1359 || (sid == SID_NORM_BP2_F0) || (sid == SID_NORM_BP2_F1) || (sid == SID_NORM_BP2_F2)
1305 1360 || (sid == SID_BURST_BP1_F0) || (sid == SID_BURST_BP2_F0)
1306 1361 || (sid == SID_BURST_BP1_F1) || (sid == SID_BURST_BP2_F1) )
1307 1362 {
1308 1363 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_NORMAL_BURST;
1309 1364 }
1310 1365 else if ( (sid ==SID_SBM1_CWF_F1) || (sid ==SID_SBM2_CWF_F2)
1311 1366 || (sid == SID_SBM1_BP1_F0) || (sid == SID_SBM1_BP2_F0)
1312 1367 || (sid == SID_SBM2_BP1_F0) || (sid == SID_SBM2_BP2_F0)
1313 1368 || (sid == SID_SBM2_BP1_F1) || (sid == SID_SBM2_BP2_F1) )
1314 1369 {
1315 1370 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_SBM1_SBM2;
1316 1371 }
1317 1372 else
1318 1373 {
1319 1374 sequence_cnt = (unsigned short *) NULL;
1320 1375 PRINTF1("in increment_seq_counter_source_id *** ERR apid_destid %d not known\n", sid)
1321 1376 }
1322 1377
1323 1378 if (sequence_cnt != NULL)
1324 1379 {
1325 1380 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
1326 1381 *sequence_cnt = (*sequence_cnt) & 0x3fff;
1327 1382
1328 1383 new_packet_sequence_control = segmentation_grouping_flag | (*sequence_cnt) ;
1329 1384
1330 1385 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
1331 1386 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1332 1387
1333 1388 // increment the sequence counter
1334 1389 if ( *sequence_cnt < SEQ_CNT_MAX)
1335 1390 {
1336 1391 *sequence_cnt = *sequence_cnt + 1;
1337 1392 }
1338 1393 else
1339 1394 {
1340 1395 *sequence_cnt = 0;
1341 1396 }
1342 1397 }
1343 1398
1344 1399 //***********************************
1345 1400 // RESET THE MODE OF THE CALLING TASK
1346 1401 status = rtems_task_mode( initial_mode_set, RTEMS_PREEMPT_MASK, &current_mode_set );
1347 1402 }
General Comments 0
You need to be logged in to leave comments. Login now