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

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

sync
paul -
r181:59ee969ac7c2 VHDL_0_1_28
parent child
Show More
Show file before | Show file after | Hide comments
@@ -0,0 +1,66
1 #ifndef GSCMEMORY_HPP_
2 #define GSCMEMORY_HPP_
3
4 static unsigned int getCacheControlRegister(){
5
6 #ifndef LEON3
7 #define LEON3
8 #endif
9
10 #ifdef LEON3
11 unsigned int cacheControlRegister = 0;
12 __asm__ __volatile__("lda [%%g0] 2, %0" : "=r"(cacheControlRegister) : );
13 return cacheControlRegister;
14 #endif
15 }
16
17 static void setCacheControlRegister(unsigned int cacheControlRegister){
18
19 #ifdef LEON3
20 __asm__ __volatile__("sta %0, [%%g0] 2" : : "r"(cacheControlRegister));
21 #endif
22 }
23
24
25 /**
26 * Flush the data cache and the instruction cache.
27 *
28 * @return
29 */
30 static inline void flushCache() {
31 asm("flush");
32 }
33
34
35 static void enableInstructionCache() {
36
37 #ifdef LEON3
38 unsigned int cacheControlRegister;
39 cacheControlRegister = getCacheControlRegister();
40 cacheControlRegister = (cacheControlRegister | 0x3);
41 setCacheControlRegister(cacheControlRegister);
42 #endif
43 }
44
45 static void enableDataCache() {
46
47 #ifdef LEON3
48 unsigned int cacheControlRegister;
49 cacheControlRegister = getCacheControlRegister();
50 cacheControlRegister = (cacheControlRegister | 0xc);
51 setCacheControlRegister(cacheControlRegister);
52 #endif
53 }
54
55 static void enableInstructionBurstFetch() {
56
57 #ifdef LEON3
58 unsigned int cacheControlRegister;
59 cacheControlRegister = getCacheControlRegister();
60 // set the bit IB to 1
61 cacheControlRegister = (cacheControlRegister | 0x10000);
62 setCacheControlRegister(cacheControlRegister);
63 #endif
64 }
65
66 #endif /* GSCMEMORY_HPP_ */
Show file before | Show file after | Hide comments
@@ -1,2 +1,2
1 d896e23033404156cdc95f5bf66e038de84de04b LFR_basic-parameters
2 5cfb4f574403f86583ac510d5921709548a9c902 header/lfr_common_headers
1 a0aa2c6f13574ae69c8645af2a2afa5d448e6c76 LFR_basic-parameters
2 a8668a35669295aaba22432d247158626f00a52a header/lfr_common_headers
Show file before | Show file after | Hide comments
@@ -1,111 +1,112
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 CONFIG += console verbose lpp_dpu_destid cpu_usage_report
5 CONFIG += console verbose lpp_dpu_destid cpu_usage_report stack_report
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 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 31 DEFINES += MSB_FIRST_TCH
32 32
33 33 contains( CONFIG, vhdl_dev ) {
34 34 DEFINES += VHDL_DEV
35 35 }
36 36
37 37 contains( CONFIG, verbose ) {
38 38 DEFINES += PRINT_MESSAGES_ON_CONSOLE
39 39 }
40 40
41 41 contains( CONFIG, debug_messages ) {
42 42 DEFINES += DEBUG_MESSAGES
43 43 }
44 44
45 45 contains( CONFIG, cpu_usage_report ) {
46 46 DEFINES += PRINT_TASK_STATISTICS
47 47 }
48 48
49 49 contains( CONFIG, stack_report ) {
50 50 DEFINES += PRINT_STACK_REPORT
51 51 }
52 52
53 53 contains( CONFIG, boot_messages ) {
54 54 DEFINES += BOOT_MESSAGES
55 55 }
56 56
57 57 #doxygen.target = doxygen
58 58 #doxygen.commands = doxygen ../doc/Doxyfile
59 59 #QMAKE_EXTRA_TARGETS += doxygen
60 60
61 61 TARGET = fsw
62 62
63 63 INCLUDEPATH += \
64 64 $${PWD}/../src \
65 65 $${PWD}/../header \
66 66 $${PWD}/../header/lfr_common_headers \
67 67 $${PWD}/../header/processing \
68 68 $${PWD}/../LFR_basic-parameters
69 69
70 70 SOURCES += \
71 71 ../src/wf_handler.c \
72 72 ../src/tc_handler.c \
73 73 ../src/fsw_misc.c \
74 74 ../src/fsw_init.c \
75 75 ../src/fsw_globals.c \
76 76 ../src/fsw_spacewire.c \
77 77 ../src/tc_load_dump_parameters.c \
78 78 ../src/tm_lfr_tc_exe.c \
79 79 ../src/tc_acceptance.c \
80 80 ../src/processing/fsw_processing.c \
81 81 ../src/processing/avf0_prc0.c \
82 82 ../src/processing/avf1_prc1.c \
83 83 ../src/processing/avf2_prc2.c \
84 84 ../src/lfr_cpu_usage_report.c \
85 85 ../LFR_basic-parameters/basic_parameters.c
86 86
87 87 HEADERS += \
88 88 ../header/wf_handler.h \
89 89 ../header/tc_handler.h \
90 90 ../header/grlib_regs.h \
91 91 ../header/fsw_misc.h \
92 92 ../header/fsw_init.h \
93 93 ../header/fsw_spacewire.h \
94 94 ../header/tc_load_dump_parameters.h \
95 95 ../header/tm_lfr_tc_exe.h \
96 96 ../header/tc_acceptance.h \
97 97 ../header/processing/fsw_processing.h \
98 98 ../header/processing/avf0_prc0.h \
99 99 ../header/processing/avf1_prc1.h \
100 100 ../header/processing/avf2_prc2.h \
101 101 ../header/fsw_params_wf_handler.h \
102 102 ../header/lfr_cpu_usage_report.h \
103 103 ../header/lfr_common_headers/ccsds_types.h \
104 104 ../header/lfr_common_headers/fsw_params.h \
105 105 ../header/lfr_common_headers/fsw_params_nb_bytes.h \
106 106 ../header/lfr_common_headers/fsw_params_processing.h \
107 107 ../header/lfr_common_headers/TC_types.h \
108 108 ../header/lfr_common_headers/tm_byte_positions.h \
109 109 ../LFR_basic-parameters/basic_parameters.h \
110 ../LFR_basic-parameters/basic_parameters_params.h
110 ../LFR_basic-parameters/basic_parameters_params.h \
111 ../header/GscMemoryLPP.hpp
111 112
Show file before | Show file after | Hide comments
@@ -1,48 +1,50
1 1 #ifndef FSW_INIT_H_INCLUDED
2 2 #define FSW_INIT_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <leon.h>
6 6
7 7 #include "fsw_params.h"
8 8 #include "fsw_misc.h"
9 9 #include "fsw_processing.h"
10 10
11 11 #include "tc_handler.h"
12 12 #include "wf_handler.h"
13 13 #include "fsw_spacewire.h"
14 14
15 15 #include "avf0_prc0.h"
16 16 #include "avf1_prc1.h"
17 17 #include "avf2_prc2.h"
18 18
19 #include "GscMemoryLPP.hpp"
20
19 21 extern rtems_name Task_name[20]; /* array of task names */
20 22 extern rtems_id Task_id[20]; /* array of task ids */
21 23
22 24 // RTEMS TASKS
23 25 rtems_task Init( rtems_task_argument argument);
24 26
25 27 // OTHER functions
26 28 void create_names( void );
27 29 int create_all_tasks( void );
28 30 int start_all_tasks( void );
29 31 //
30 32 rtems_status_code create_message_queues( void );
31 33 rtems_status_code get_message_queue_id_send( rtems_id *queue_id );
32 34 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id );
33 35 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id );
34 36 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
35 37 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
36 38 //
37 39 int start_recv_send_tasks( void );
38 40 //
39 41 void init_local_mode_parameters( void );
40 42 void reset_local_time( void );
41 43
42 44 extern void rtems_cpu_usage_report( void );
43 45 extern void rtems_cpu_usage_reset( void );
44 46 extern void rtems_stack_checker_report_usage( void );
45 47
46 48 extern int sched_yield( void );
47 49
48 50 #endif // FSW_INIT_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,298 +1,317
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 24 unsigned char targetLogicalAddress;
25 25 unsigned char protocolIdentifier;
26 26 unsigned char reserved;
27 27 unsigned char userApplication;
28 28 unsigned char packetID[2];
29 29 unsigned char packetSequenceControl[2];
30 30 unsigned char packetLength[2];
31 31 // DATA FIELD HEADER
32 32 unsigned char spare1_pusVersion_spare2;
33 33 unsigned char serviceType;
34 34 unsigned char serviceSubType;
35 35 unsigned char destinationID;
36 36 unsigned char time[6];
37 37 // AUXILIARY HEADER
38 38 unsigned char sid;
39 39 unsigned char biaStatusInfo;
40 40 unsigned char acquisitionTime[6];
41 41 unsigned char pa_lfr_bp_blk_nr[2];
42 42 // SOURCE DATA
43 unsigned char data[ 30 * 22 ]; // MAX size is 22 * 30 [TM_LFR_SCIENCE_BURST_BP2_F1]
43 unsigned char data[ 780 ]; // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1]
44 44 } bp_packet;
45 45
46 46 typedef struct
47 47 {
48 Header_TM_LFR_SCIENCE_BP_with_spare_t header;
49 unsigned char data[ 9 * 13 ]; // only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1
48 unsigned char targetLogicalAddress;
49 unsigned char protocolIdentifier;
50 unsigned char reserved;
51 unsigned char userApplication;
52 unsigned char packetID[2];
53 unsigned char packetSequenceControl[2];
54 unsigned char packetLength[2];
55 // DATA FIELD HEADER
56 unsigned char spare1_pusVersion_spare2;
57 unsigned char serviceType;
58 unsigned char serviceSubType;
59 unsigned char destinationID;
60 unsigned char time[6];
61 // AUXILIARY HEADER
62 unsigned char sid;
63 unsigned char biaStatusInfo;
64 unsigned char acquisitionTime[6];
65 unsigned char source_data_spare;
66 unsigned char pa_lfr_bp_blk_nr[2];
67 // SOURCE DATA
68 unsigned char data[ 117 ]; // 13 bins * 9 Bytes only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1
50 69 } bp_packet_with_spare;
51 70
52 71 typedef struct
53 72 {
54 73 ring_node_asm *norm;
55 74 ring_node_asm *burst_sbm;
56 75 rtems_event_set event;
57 76 unsigned int coarseTimeNORM;
58 77 unsigned int fineTimeNORM;
59 78 unsigned int coarseTimeSBM;
60 79 unsigned int fineTimeSBM;
61 80 } asm_msg;
62 81
63 82 extern volatile int sm_f0[ ];
64 83 extern volatile int sm_f1[ ];
65 84 extern volatile int sm_f2[ ];
66 85
67 86 // parameters
68 87 extern struct param_local_str param_local;
69 88
70 89 // registers
71 90 extern time_management_regs_t *time_management_regs;
72 91 extern volatile spectral_matrix_regs_t *spectral_matrix_regs;
73 92
74 93 extern rtems_name misc_name[5];
75 94 extern rtems_id Task_id[20]; /* array of task ids */
76 95
77 96 //
78 97 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel);
79 98 // ISR
80 99 rtems_isr spectral_matrices_isr( rtems_vector_number vector );
81 100 rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector );
82 101
83 102 //******************
84 103 // Spectral Matrices
85 104 void reset_nb_sm( void );
86 105 // SM
87 106 void SM_init_rings( void );
88 107 void SM_reset_current_ring_nodes( void );
89 108 // ASM
90 109 void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes );
91 110
92 111 //*****************
93 112 // Basic Parameters
94 113
95 114 void BP_reset_current_ring_nodes( void );
96 void BP_init_header(bp_packet *header,
115 void BP_init_header(bp_packet *packet,
97 116 unsigned int apid, unsigned char sid,
98 117 unsigned int packetLength , unsigned char blkNr);
99 void BP_init_header_with_spare( Header_TM_LFR_SCIENCE_BP_with_spare_t *header,
118 void BP_init_header_with_spare(bp_packet_with_spare *packet,
100 119 unsigned int apid, unsigned char sid,
101 120 unsigned int packetLength, unsigned char blkNr );
102 121 void BP_send( char *data,
103 122 rtems_id queue_id ,
104 123 unsigned int nbBytesToSend , unsigned int sid );
105 124
106 125 //******************
107 126 // general functions
108 127 void reset_sm_status( void );
109 128 void reset_spectral_matrix_regs( void );
110 129 void set_time(unsigned char *time, unsigned char *timeInBuffer );
111 130 unsigned long long int get_acquisition_time( unsigned char *timePtr );
112 131 unsigned char getSID( rtems_event_set event );
113 132
114 133 extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
115 134 extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
116 135
117 136 //***************************************
118 137 // DEFINITIONS OF STATIC INLINE FUNCTIONS
119 138 static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
120 139 ring_node *ring_node_tab[],
121 140 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
122 141 asm_msg *msgForMATR );
123 142 static inline void SM_average_debug( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
124 143 ring_node *ring_node_tab[],
125 144 unsigned int nbAverageNORM, unsigned int nbAverageSBM );
126 145 static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized,
127 146 float divider );
128 147 static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat,
129 148 float divider,
130 149 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
131 150 static inline void ASM_convert(volatile float *input_matrix, char *output_matrix);
132 151
133 152 void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
134 153 ring_node *ring_node_tab[],
135 154 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
136 155 asm_msg *msgForMATR )
137 156 {
138 157 float sum;
139 158 unsigned int i;
140 159
141 160 for(i=0; i<TOTAL_SIZE_SM; i++)
142 161 {
143 162 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ]
144 163 + ( (int *) (ring_node_tab[1]->buffer_address) ) [ i ]
145 164 + ( (int *) (ring_node_tab[2]->buffer_address) ) [ i ]
146 165 + ( (int *) (ring_node_tab[3]->buffer_address) ) [ i ]
147 166 + ( (int *) (ring_node_tab[4]->buffer_address) ) [ i ]
148 167 + ( (int *) (ring_node_tab[5]->buffer_address) ) [ i ]
149 168 + ( (int *) (ring_node_tab[6]->buffer_address) ) [ i ]
150 169 + ( (int *) (ring_node_tab[7]->buffer_address) ) [ i ];
151 170
152 171 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
153 172 {
154 173 averaged_spec_mat_NORM[ i ] = sum;
155 174 averaged_spec_mat_SBM[ i ] = sum;
156 175 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
157 176 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
158 177 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
159 178 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
160 179 }
161 180 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
162 181 {
163 182 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
164 183 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
165 184 }
166 185 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
167 186 {
168 187 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
169 188 averaged_spec_mat_SBM[ i ] = sum;
170 189 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
171 190 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
172 191 }
173 192 else
174 193 {
175 194 PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
176 195 }
177 196 }
178 197 }
179 198
180 199 void SM_average_debug( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
181 200 ring_node *ring_node_tab[],
182 201 unsigned int nbAverageNORM, unsigned int nbAverageSBM )
183 202 {
184 203 float sum;
185 204 unsigned int i;
186 205
187 206 for(i=0; i<TOTAL_SIZE_SM; i++)
188 207 {
189 208 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ];
190 209
191 210 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
192 211 {
193 212 averaged_spec_mat_NORM[ i ] = sum;
194 213 averaged_spec_mat_SBM[ i ] = sum;
195 214 }
196 215 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
197 216 {
198 217 averaged_spec_mat_NORM[ i ] = sum;
199 218 averaged_spec_mat_SBM[ i ] = sum;
200 219 }
201 220 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
202 221 {
203 222 averaged_spec_mat_NORM[ i ] = sum;
204 223 averaged_spec_mat_SBM[ i ] = sum;
205 224 }
206 225 else
207 226 {
208 227 PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
209 228 }
210 229 }
211 230 }
212 231
213 232 void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
214 233 {
215 234 int frequencyBin;
216 235 int asmComponent;
217 236 unsigned int offsetASM;
218 237 unsigned int offsetASMReorganized;
219 238
220 239 // BUILD DATA
221 240 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
222 241 {
223 242 for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
224 243 {
225 244 offsetASMReorganized =
226 245 frequencyBin * NB_VALUES_PER_SM
227 246 + asmComponent;
228 247 offsetASM =
229 248 asmComponent * NB_BINS_PER_SM
230 249 + frequencyBin;
231 250 averaged_spec_mat_reorganized[offsetASMReorganized ] =
232 251 averaged_spec_mat[ offsetASM ] / divider;
233 252 }
234 253 }
235 254 }
236 255
237 256 void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
238 257 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
239 258 {
240 259 int frequencyBin;
241 260 int asmComponent;
242 261 int offsetASM;
243 262 int offsetCompressed;
244 263 int k;
245 264
246 265 // BUILD DATA
247 266 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
248 267 {
249 268 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
250 269 {
251 270 offsetCompressed = // NO TIME OFFSET
252 271 frequencyBin * NB_VALUES_PER_SM
253 272 + asmComponent;
254 273 offsetASM = // NO TIME OFFSET
255 274 asmComponent * NB_BINS_PER_SM
256 275 + ASMIndexStart
257 276 + frequencyBin * nbBinsToAverage;
258 277 compressed_spec_mat[ offsetCompressed ] = 0;
259 278 for ( k = 0; k < nbBinsToAverage; k++ )
260 279 {
261 280 compressed_spec_mat[offsetCompressed ] =
262 281 ( compressed_spec_mat[ offsetCompressed ]
263 282 + averaged_spec_mat[ offsetASM + k ] );
264 283 }
265 284 compressed_spec_mat[ offsetCompressed ] =
266 285 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
267 286 }
268 287 }
269 288 }
270 289
271 290 void ASM_convert( volatile float *input_matrix, char *output_matrix)
272 291 {
273 292 unsigned int frequencyBin;
274 293 unsigned int asmComponent;
275 294 char * pt_char_input;
276 295 char * pt_char_output;
277 296 unsigned int offsetInput;
278 297 unsigned int offsetOutput;
279 298
280 299 pt_char_input = (char*) &input_matrix;
281 300 pt_char_output = (char*) &output_matrix;
282 301
283 302 // convert all other data
284 303 for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++)
285 304 {
286 305 for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++)
287 306 {
288 307 offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ;
289 308 offsetOutput = 2 * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ;
290 309 pt_char_input = (char*) &input_matrix [ offsetInput ];
291 310 pt_char_output = (char*) &output_matrix[ offsetOutput ];
292 311 pt_char_output[0] = pt_char_input[0]; // bits 31 downto 24 of the float
293 312 pt_char_output[1] = pt_char_input[1]; // bits 23 downto 16 of the float
294 313 }
295 314 }
296 315 }
297 316
298 317 #endif // FSW_PROCESSING_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,88 +1,87
1 1 #ifndef WF_HANDLER_H_INCLUDED
2 2 #define WF_HANDLER_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <grspw.h>
6 6 #include <stdio.h>
7 7 #include <math.h>
8 8 #include <fsw_params.h>
9 9
10 10 #include "fsw_spacewire.h"
11 11 #include "fsw_misc.h"
12 12 #include "fsw_params_wf_handler.h"
13 13
14 14 #define pi 3.1415
15 15
16 16 extern int fdSPW;
17 17
18 18 //*****************
19 19 // waveform buffers
20 20 extern volatile int wf_buffer_f0[ ];
21 21 extern volatile int wf_buffer_f1[ ];
22 22 extern volatile int wf_buffer_f2[ ];
23 23 extern volatile int wf_buffer_f3[ ];
24 extern char wf_cont_f3_light[ ];
25 24
26 25 extern waveform_picker_regs_0_1_18_t *waveform_picker_regs;
27 26 extern time_management_regs_t *time_management_regs;
28 27 extern Packet_TM_LFR_HK_t housekeeping_packet;
29 28 extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
30 29 extern struct param_local_str param_local;
31 30
32 31 extern unsigned short sequenceCounters_SCIENCE_NORMAL_BURST;
33 32 extern unsigned short sequenceCounters_SCIENCE_SBM1_SBM2;
34 33
35 34 extern rtems_id Task_id[20]; /* array of task ids */
36 35
37 36 extern unsigned char lfrCurrentMode;
38 37
39 38 //**********
40 39 // RTEMS_ISR
41 40 void reset_extractSWF( void );
42 41 rtems_isr waveforms_isr( rtems_vector_number vector );
43 42
44 43 //***********
45 44 // RTEMS_TASK
46 45 rtems_task wfrm_task( rtems_task_argument argument );
47 46 rtems_task cwf3_task( rtems_task_argument argument );
48 47 rtems_task cwf2_task( rtems_task_argument argument );
49 48 rtems_task cwf1_task( rtems_task_argument argument );
50 49 rtems_task swbd_task( rtems_task_argument argument );
51 50
52 51 //******************
53 52 // general functions
54 53 void WFP_init_rings( void );
55 54 void init_ring( ring_node ring[], unsigned char nbNodes, volatile int buffer[] , unsigned int bufferSize );
56 55 void WFP_reset_current_ring_nodes( void );
57 56 //
58 57 int init_header_continuous_cwf3_light_table( Header_TM_LFR_SCIENCE_CWF_t *headerCWF );
59 58 //
60 59 int send_waveform_CWF3_light(ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id );
61 60 //
62 61 void compute_acquisition_time(unsigned int coarseTime, unsigned int fineTime,
63 62 unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char *acquisitionTime );
64 void build_snapshot_from_ring(ring_node *ring_node_to_send , unsigned char frequencyChannel );
63 void build_snapshot_from_ring(ring_node *ring_node_to_send, unsigned char frequencyChannel , unsigned long long acquisitionTimeF0_asLong);
65 64 void snapshot_resynchronization( unsigned char *timePtr );
66 65 //
67 66 rtems_id get_pkts_queue_id( void );
68 67
69 68 //**************
70 69 // wfp registers
71 70 // RESET
72 71 void reset_wfp_burst_enable( void );
73 72 void reset_wfp_status( void );
74 73 void reset_wfp_buffer_addresses( void );
75 74 void reset_waveform_picker_regs( void );
76 75 // SET
77 76 void set_wfp_data_shaping(void);
78 77 void set_wfp_burst_enable_register( unsigned char mode );
79 78 void set_wfp_delta_snapshot( void );
80 79 void set_wfp_delta_f0_f0_2( void );
81 80 void set_wfp_delta_f1( void );
82 81 void set_wfp_delta_f2( void );
83 82
84 83 //*****************
85 84 // local parameters
86 85 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid );
87 86
88 87 #endif // WF_HANDLER_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,75 +1,74
1 1 /** Global variables of the LFR flight software.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * Among global variables, there are:
7 7 * - RTEMS names and id.
8 8 * - APB configuration registers.
9 9 * - waveforms global buffers, used by the waveform picker hardware module to store data.
10 10 * - spectral matrices buffesr, used by the hardware module to store data.
11 11 * - variable related to LFR modes parameters.
12 12 * - the global HK packet buffer.
13 13 * - the global dump parameter buffer.
14 14 *
15 15 */
16 16
17 17 #include <rtems.h>
18 18 #include <grspw.h>
19 19
20 20 #include "ccsds_types.h"
21 21 #include "grlib_regs.h"
22 22 #include "fsw_params.h"
23 23 #include "fsw_params_wf_handler.h"
24 24
25 25 // RTEMS GLOBAL VARIABLES
26 26 rtems_name misc_name[5];
27 27 rtems_id misc_id[5];
28 28 rtems_name Task_name[20]; /* array of task names */
29 29 rtems_id Task_id[20]; /* array of task ids */
30 30 unsigned int maxCount;
31 31 int fdSPW = 0;
32 32 int fdUART = 0;
33 33 unsigned char lfrCurrentMode;
34 34
35 35 // WAVEFORMS GLOBAL VARIABLES // 2048 * 3 * 4 + 2 * 4 = 24576 + 8 bytes = 24584
36 36 // 97 * 256 = 24832 => delta = 248 bytes = 62 words
37 37 // WAVEFORMS GLOBAL VARIABLES // 2688 * 3 * 4 + 2 * 4 = 32256 + 8 bytes = 32264
38 38 // 127 * 256 = 32512 => delta = 248 bytes = 62 words
39 39 // F0 F1 F2 F3
40 40 volatile int wf_buffer_f0[ NB_RING_NODES_F0 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
41 41 volatile int wf_buffer_f1[ NB_RING_NODES_F1 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
42 42 volatile int wf_buffer_f2[ NB_RING_NODES_F2 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
43 43 volatile int wf_buffer_f3[ NB_RING_NODES_F3 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
44 char wf_cont_f3_light[ (NB_SAMPLES_PER_SNAPSHOT) * NB_BYTES_CWF3_LIGHT_BLK + TIME_OFFSET_IN_BYTES ] __attribute__((aligned(0x100)));
45 44
46 45 //***********************************
47 46 // SPECTRAL MATRICES GLOBAL VARIABLES
48 47
49 48 // alignment constraints for the spectral matrices buffers => the first data after the time (8 bytes) shall be aligned on 0x00
50 49 volatile int sm_f0[ NB_RING_NODES_SM_F0 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
51 50 volatile int sm_f1[ NB_RING_NODES_SM_F1 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
52 51 volatile int sm_f2[ NB_RING_NODES_SM_F2 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
53 52
54 53 // APB CONFIGURATION REGISTERS
55 54 time_management_regs_t *time_management_regs = (time_management_regs_t*) REGS_ADDR_TIME_MANAGEMENT;
56 55 gptimer_regs_t *gptimer_regs = (gptimer_regs_t *) REGS_ADDR_GPTIMER;
57 56 waveform_picker_regs_0_1_18_t *waveform_picker_regs = (waveform_picker_regs_0_1_18_t*) REGS_ADDR_WAVEFORM_PICKER;
58 57 spectral_matrix_regs_t *spectral_matrix_regs = (spectral_matrix_regs_t*) REGS_ADDR_SPECTRAL_MATRIX;
59 58
60 59 // MODE PARAMETERS
61 60 Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
62 61 struct param_local_str param_local;
63 62
64 63 // HK PACKETS
65 64 Packet_TM_LFR_HK_t housekeeping_packet;
66 65 // sequence counters are incremented by APID (PID + CAT) and destination ID
67 66 unsigned short sequenceCounters_SCIENCE_NORMAL_BURST;
68 67 unsigned short sequenceCounters_SCIENCE_SBM1_SBM2;
69 68 unsigned short sequenceCounters_TC_EXE[SEQ_CNT_NB_DEST_ID];
70 69 unsigned short sequenceCounterHK;
71 70 unsigned short sequenceCounterParameterDump;
72 71 spw_stats spacewire_stats;
73 72 spw_stats spacewire_stats_backup;
74 73
75 74
Show file before | Show file after | Hide comments
@@ -1,787 +1,810
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 void initCache()
65 {
66 // unsigned int cacheControlRegister;
67
68 // cacheControlRegister = getCacheControlRegister();
69 // printf("(0) cacheControlRegister = %x\n", cacheControlRegister);
70
71 enableInstructionCache();
72 enableDataCache();
73 enableInstructionBurstFetch();
74
75 // cacheControlRegister = getCacheControlRegister();
76 // printf("(1) cacheControlRegister = %x\n", cacheControlRegister);
77 }
78
64 79 rtems_task Init( rtems_task_argument ignored )
65 80 {
66 81 /** This is the RTEMS INIT taks, it the first task launched by the system.
67 82 *
68 83 * @param unused is the starting argument of the RTEMS task
69 84 *
70 85 * The INIT task create and run all other RTEMS tasks.
71 86 *
72 87 */
73 88
89 //***********
90 // INIT CACHE
91
74 92 unsigned char *vhdlVersion;
75 93
76 94 reset_lfr();
77 95
78 96 reset_local_time();
79 97
80 98 rtems_cpu_usage_reset();
81 99
82 100 rtems_status_code status;
83 101 rtems_status_code status_spw;
84 102 rtems_isr_entry old_isr_handler;
85 103
86 104 // UART settings
87 105 send_console_outputs_on_apbuart_port();
88 106 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
89 107 enable_apbuart_transmitter();
108
90 109 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
91 110
111
92 112 PRINTF("\n\n\n\n\n")
113
114 initCache();
115
93 116 PRINTF("*************************\n")
94 117 PRINTF("** LFR Flight Software **\n")
95 118 PRINTF1("** %d.", SW_VERSION_N1)
96 119 PRINTF1("%d." , SW_VERSION_N2)
97 120 PRINTF1("%d." , SW_VERSION_N3)
98 121 PRINTF1("%d **\n", SW_VERSION_N4)
99 122
100 123 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
101 124 PRINTF("** VHDL **\n")
102 125 PRINTF1("** %d.", vhdlVersion[1])
103 126 PRINTF1("%d." , vhdlVersion[2])
104 127 PRINTF1("%d **\n", vhdlVersion[3])
105 128 PRINTF("*************************\n")
106 129 PRINTF("\n\n")
107 130
108 131 init_parameter_dump();
109 132 init_local_mode_parameters();
110 133 init_housekeeping_parameters();
111 134 init_k_coefficients_f0();
112 135 init_k_coefficients_f1();
113 136 init_k_coefficients_f2();
114 137
115 138 // waveform picker initialization
116 139 WFP_init_rings(); // initialize the waveform rings
117 140 WFP_reset_current_ring_nodes();
118 141 reset_waveform_picker_regs();
119 142
120 143 // spectral matrices initialization
121 144 SM_init_rings(); // initialize spectral matrices rings
122 145 SM_reset_current_ring_nodes();
123 146 reset_spectral_matrix_regs();
124 147
125 148 updateLFRCurrentMode();
126 149
127 150 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
128 151
129 152 create_names(); // create all names
130 153
131 154 status = create_message_queues(); // create message queues
132 155 if (status != RTEMS_SUCCESSFUL)
133 156 {
134 157 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
135 158 }
136 159
137 160 status = create_all_tasks(); // create all tasks
138 161 if (status != RTEMS_SUCCESSFUL)
139 162 {
140 163 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
141 164 }
142 165
143 166 // **************************
144 167 // <SPACEWIRE INITIALIZATION>
145 168 grspw_timecode_callback = &timecode_irq_handler;
146 169
147 170 status_spw = spacewire_open_link(); // (1) open the link
148 171 if ( status_spw != RTEMS_SUCCESSFUL )
149 172 {
150 173 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
151 174 }
152 175
153 176 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
154 177 {
155 178 status_spw = spacewire_configure_link( fdSPW );
156 179 if ( status_spw != RTEMS_SUCCESSFUL )
157 180 {
158 181 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
159 182 }
160 183 }
161 184
162 185 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
163 186 {
164 187 status_spw = spacewire_start_link( fdSPW );
165 188 if ( status_spw != RTEMS_SUCCESSFUL )
166 189 {
167 190 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
168 191 }
169 192 }
170 193 // </SPACEWIRE INITIALIZATION>
171 194 // ***************************
172 195
173 196 status = start_all_tasks(); // start all tasks
174 197 if (status != RTEMS_SUCCESSFUL)
175 198 {
176 199 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
177 200 }
178 201
179 202 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
180 203 status = start_recv_send_tasks();
181 204 if ( status != RTEMS_SUCCESSFUL )
182 205 {
183 206 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
184 207 }
185 208
186 209 // suspend science tasks, they will be restarted later depending on the mode
187 210 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
188 211 if (status != RTEMS_SUCCESSFUL)
189 212 {
190 213 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
191 214 }
192 215
193 216 //******************************
194 217 // <SPECTRAL MATRICES SIMULATOR>
195 218 LEON_Mask_interrupt( IRQ_SM_SIMULATOR );
196 219 configure_timer((gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR, CLKDIV_SM_SIMULATOR,
197 220 IRQ_SPARC_SM_SIMULATOR, spectral_matrices_isr_simu );
198 221 // </SPECTRAL MATRICES SIMULATOR>
199 222 //*******************************
200 223
201 224 // configure IRQ handling for the waveform picker unit
202 225 status = rtems_interrupt_catch( waveforms_isr,
203 226 IRQ_SPARC_WAVEFORM_PICKER,
204 227 &old_isr_handler) ;
205 228 // configure IRQ handling for the spectral matrices unit
206 229 status = rtems_interrupt_catch( spectral_matrices_isr,
207 230 IRQ_SPARC_SPECTRAL_MATRIX,
208 231 &old_isr_handler) ;
209 232
210 233 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
211 234 if ( status_spw != RTEMS_SUCCESSFUL )
212 235 {
213 236 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
214 237 if ( status != RTEMS_SUCCESSFUL ) {
215 238 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
216 239 }
217 240 }
218 241
219 242 BOOT_PRINTF("delete INIT\n")
220 243
221 244 // test_TCH();
222 245
223 246 status = rtems_task_delete(RTEMS_SELF);
224 247
225 248 }
226 249
227 250 void init_local_mode_parameters( void )
228 251 {
229 252 /** This function initialize the param_local global variable with default values.
230 253 *
231 254 */
232 255
233 256 unsigned int i;
234 257
235 258 // LOCAL PARAMETERS
236 259
237 260 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
238 261 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
239 262 BOOT_PRINTF1("nb_interrupt_f0_MAX = %d\n", param_local.local_nb_interrupt_f0_MAX)
240 263
241 264 // init sequence counters
242 265
243 266 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
244 267 {
245 268 sequenceCounters_TC_EXE[i] = 0x00;
246 269 }
247 270 sequenceCounters_SCIENCE_NORMAL_BURST = 0x00;
248 271 sequenceCounters_SCIENCE_SBM1_SBM2 = 0x00;
249 272 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
250 273 sequenceCounterParameterDump = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
251 274 }
252 275
253 276 void reset_local_time( void )
254 277 {
255 278 time_management_regs->ctrl = time_management_regs->ctrl | 0x02; // [0010] software reset, coarse time = 0x80000000
256 279 }
257 280
258 281 void create_names( void ) // create all names for tasks and queues
259 282 {
260 283 /** This function creates all RTEMS names used in the software for tasks and queues.
261 284 *
262 285 * @return RTEMS directive status codes:
263 286 * - RTEMS_SUCCESSFUL - successful completion
264 287 *
265 288 */
266 289
267 290 // task names
268 291 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
269 292 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
270 293 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
271 294 Task_name[TASKID_STAT] = rtems_build_name( 'S', 'T', 'A', 'T' );
272 295 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
273 296 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
274 297 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
275 298 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
276 299 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
277 300 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
278 301 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
279 302 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
280 303 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
281 304 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
282 305 Task_name[TASKID_WTDG] = rtems_build_name( 'W', 'T', 'D', 'G' );
283 306 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
284 307 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
285 308 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
286 309 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
287 310
288 311 // rate monotonic period names
289 312 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
290 313
291 314 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
292 315 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
293 316 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
294 317 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
295 318 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
296 319 }
297 320
298 321 int create_all_tasks( void ) // create all tasks which run in the software
299 322 {
300 323 /** This function creates all RTEMS tasks used in the software.
301 324 *
302 325 * @return RTEMS directive status codes:
303 326 * - RTEMS_SUCCESSFUL - task created successfully
304 327 * - RTEMS_INVALID_ADDRESS - id is NULL
305 328 * - RTEMS_INVALID_NAME - invalid task name
306 329 * - RTEMS_INVALID_PRIORITY - invalid task priority
307 330 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
308 331 * - RTEMS_TOO_MANY - too many tasks created
309 332 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
310 333 * - RTEMS_TOO_MANY - too many global objects
311 334 *
312 335 */
313 336
314 337 rtems_status_code status;
315 338
316 339 //**********
317 340 // SPACEWIRE
318 341 // RECV
319 342 status = rtems_task_create(
320 343 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
321 344 RTEMS_DEFAULT_MODES,
322 345 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
323 346 );
324 347 if (status == RTEMS_SUCCESSFUL) // SEND
325 348 {
326 349 status = rtems_task_create(
327 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE,
328 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
350 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * 2,
351 RTEMS_DEFAULT_MODES,
329 352 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
330 353 );
331 354 }
332 355 if (status == RTEMS_SUCCESSFUL) // WTDG
333 356 {
334 357 status = rtems_task_create(
335 358 Task_name[TASKID_WTDG], TASK_PRIORITY_WTDG, RTEMS_MINIMUM_STACK_SIZE,
336 359 RTEMS_DEFAULT_MODES,
337 360 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_WTDG]
338 361 );
339 362 }
340 363 if (status == RTEMS_SUCCESSFUL) // ACTN
341 364 {
342 365 status = rtems_task_create(
343 366 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
344 367 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
345 368 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
346 369 );
347 370 }
348 371 if (status == RTEMS_SUCCESSFUL) // SPIQ
349 372 {
350 373 status = rtems_task_create(
351 374 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
352 375 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
353 376 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
354 377 );
355 378 }
356 379
357 380 //******************
358 381 // SPECTRAL MATRICES
359 382 if (status == RTEMS_SUCCESSFUL) // AVF0
360 383 {
361 384 status = rtems_task_create(
362 385 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
363 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
386 RTEMS_DEFAULT_MODES,
364 387 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
365 388 );
366 389 }
367 390 if (status == RTEMS_SUCCESSFUL) // PRC0
368 391 {
369 392 status = rtems_task_create(
370 393 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * 2,
371 394 RTEMS_DEFAULT_MODES,
372 395 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
373 396 );
374 397 }
375 398 if (status == RTEMS_SUCCESSFUL) // AVF1
376 399 {
377 400 status = rtems_task_create(
378 401 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
379 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
402 RTEMS_DEFAULT_MODES,
380 403 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
381 404 );
382 405 }
383 406 if (status == RTEMS_SUCCESSFUL) // PRC1
384 407 {
385 408 status = rtems_task_create(
386 409 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * 2,
387 410 RTEMS_DEFAULT_MODES,
388 411 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
389 412 );
390 413 }
391 414 if (status == RTEMS_SUCCESSFUL) // AVF2
392 415 {
393 416 status = rtems_task_create(
394 417 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
395 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
418 RTEMS_DEFAULT_MODES,
396 419 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
397 420 );
398 421 }
399 422 if (status == RTEMS_SUCCESSFUL) // PRC2
400 423 {
401 424 status = rtems_task_create(
402 425 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * 2,
403 426 RTEMS_DEFAULT_MODES,
404 427 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
405 428 );
406 429 }
407 430
408 431 //****************
409 432 // WAVEFORM PICKER
410 433 if (status == RTEMS_SUCCESSFUL) // WFRM
411 434 {
412 435 status = rtems_task_create(
413 436 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
414 437 RTEMS_DEFAULT_MODES,
415 438 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
416 439 );
417 440 }
418 441 if (status == RTEMS_SUCCESSFUL) // CWF3
419 442 {
420 443 status = rtems_task_create(
421 444 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
422 445 RTEMS_DEFAULT_MODES,
423 446 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
424 447 );
425 448 }
426 449 if (status == RTEMS_SUCCESSFUL) // CWF2
427 450 {
428 451 status = rtems_task_create(
429 452 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
430 453 RTEMS_DEFAULT_MODES,
431 454 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
432 455 );
433 456 }
434 457 if (status == RTEMS_SUCCESSFUL) // CWF1
435 458 {
436 459 status = rtems_task_create(
437 460 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
438 461 RTEMS_DEFAULT_MODES,
439 462 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
440 463 );
441 464 }
442 465 if (status == RTEMS_SUCCESSFUL) // SWBD
443 466 {
444 467 status = rtems_task_create(
445 468 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
446 469 RTEMS_DEFAULT_MODES,
447 470 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
448 471 );
449 472 }
450 473
451 474 //*****
452 475 // MISC
453 476 if (status == RTEMS_SUCCESSFUL) // STAT
454 477 {
455 478 status = rtems_task_create(
456 479 Task_name[TASKID_STAT], TASK_PRIORITY_STAT, RTEMS_MINIMUM_STACK_SIZE,
457 480 RTEMS_DEFAULT_MODES,
458 481 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_STAT]
459 482 );
460 483 }
461 484 if (status == RTEMS_SUCCESSFUL) // DUMB
462 485 {
463 486 status = rtems_task_create(
464 487 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
465 488 RTEMS_DEFAULT_MODES,
466 489 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
467 490 );
468 491 }
469 492 if (status == RTEMS_SUCCESSFUL) // HOUS
470 493 {
471 494 status = rtems_task_create(
472 495 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
473 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
496 RTEMS_DEFAULT_MODES,
474 497 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
475 498 );
476 499 }
477 500
478 501 return status;
479 502 }
480 503
481 504 int start_recv_send_tasks( void )
482 505 {
483 506 rtems_status_code status;
484 507
485 508 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
486 509 if (status!=RTEMS_SUCCESSFUL) {
487 510 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
488 511 }
489 512
490 513 if (status == RTEMS_SUCCESSFUL) // SEND
491 514 {
492 515 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
493 516 if (status!=RTEMS_SUCCESSFUL) {
494 517 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
495 518 }
496 519 }
497 520
498 521 return status;
499 522 }
500 523
501 524 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
502 525 {
503 526 /** This function starts all RTEMS tasks used in the software.
504 527 *
505 528 * @return RTEMS directive status codes:
506 529 * - RTEMS_SUCCESSFUL - ask started successfully
507 530 * - RTEMS_INVALID_ADDRESS - invalid task entry point
508 531 * - RTEMS_INVALID_ID - invalid task id
509 532 * - RTEMS_INCORRECT_STATE - task not in the dormant state
510 533 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
511 534 *
512 535 */
513 536 // starts all the tasks fot eh flight software
514 537
515 538 rtems_status_code status;
516 539
517 540 //**********
518 541 // SPACEWIRE
519 542 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
520 543 if (status!=RTEMS_SUCCESSFUL) {
521 544 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
522 545 }
523 546
524 547 if (status == RTEMS_SUCCESSFUL) // WTDG
525 548 {
526 549 status = rtems_task_start( Task_id[TASKID_WTDG], wtdg_task, 1 );
527 550 if (status!=RTEMS_SUCCESSFUL) {
528 551 BOOT_PRINTF("in INIT *** Error starting TASK_WTDG\n")
529 552 }
530 553 }
531 554
532 555 if (status == RTEMS_SUCCESSFUL) // ACTN
533 556 {
534 557 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
535 558 if (status!=RTEMS_SUCCESSFUL) {
536 559 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
537 560 }
538 561 }
539 562
540 563 //******************
541 564 // SPECTRAL MATRICES
542 565 if (status == RTEMS_SUCCESSFUL) // AVF0
543 566 {
544 567 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
545 568 if (status!=RTEMS_SUCCESSFUL) {
546 569 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
547 570 }
548 571 }
549 572 if (status == RTEMS_SUCCESSFUL) // PRC0
550 573 {
551 574 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
552 575 if (status!=RTEMS_SUCCESSFUL) {
553 576 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
554 577 }
555 578 }
556 579 if (status == RTEMS_SUCCESSFUL) // AVF1
557 580 {
558 581 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
559 582 if (status!=RTEMS_SUCCESSFUL) {
560 583 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
561 584 }
562 585 }
563 586 if (status == RTEMS_SUCCESSFUL) // PRC1
564 587 {
565 588 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
566 589 if (status!=RTEMS_SUCCESSFUL) {
567 590 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
568 591 }
569 592 }
570 593 if (status == RTEMS_SUCCESSFUL) // AVF2
571 594 {
572 595 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
573 596 if (status!=RTEMS_SUCCESSFUL) {
574 597 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
575 598 }
576 599 }
577 600 if (status == RTEMS_SUCCESSFUL) // PRC2
578 601 {
579 602 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
580 603 if (status!=RTEMS_SUCCESSFUL) {
581 604 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
582 605 }
583 606 }
584 607
585 608 //****************
586 609 // WAVEFORM PICKER
587 610 if (status == RTEMS_SUCCESSFUL) // WFRM
588 611 {
589 612 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
590 613 if (status!=RTEMS_SUCCESSFUL) {
591 614 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
592 615 }
593 616 }
594 617 if (status == RTEMS_SUCCESSFUL) // CWF3
595 618 {
596 619 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
597 620 if (status!=RTEMS_SUCCESSFUL) {
598 621 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
599 622 }
600 623 }
601 624 if (status == RTEMS_SUCCESSFUL) // CWF2
602 625 {
603 626 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
604 627 if (status!=RTEMS_SUCCESSFUL) {
605 628 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
606 629 }
607 630 }
608 631 if (status == RTEMS_SUCCESSFUL) // CWF1
609 632 {
610 633 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
611 634 if (status!=RTEMS_SUCCESSFUL) {
612 635 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
613 636 }
614 637 }
615 638 if (status == RTEMS_SUCCESSFUL) // SWBD
616 639 {
617 640 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
618 641 if (status!=RTEMS_SUCCESSFUL) {
619 642 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
620 643 }
621 644 }
622 645
623 646 //*****
624 647 // MISC
625 648 if (status == RTEMS_SUCCESSFUL) // HOUS
626 649 {
627 650 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
628 651 if (status!=RTEMS_SUCCESSFUL) {
629 652 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
630 653 }
631 654 }
632 655 if (status == RTEMS_SUCCESSFUL) // DUMB
633 656 {
634 657 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
635 658 if (status!=RTEMS_SUCCESSFUL) {
636 659 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
637 660 }
638 661 }
639 662 if (status == RTEMS_SUCCESSFUL) // STAT
640 663 {
641 664 status = rtems_task_start( Task_id[TASKID_STAT], stat_task, 1 );
642 665 if (status!=RTEMS_SUCCESSFUL) {
643 666 BOOT_PRINTF("in INIT *** Error starting TASK_STAT\n")
644 667 }
645 668 }
646 669
647 670 return status;
648 671 }
649 672
650 673 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
651 674 {
652 675 rtems_status_code status_recv;
653 676 rtems_status_code status_send;
654 677 rtems_status_code status_q_p0;
655 678 rtems_status_code status_q_p1;
656 679 rtems_status_code status_q_p2;
657 680 rtems_status_code ret;
658 681 rtems_id queue_id;
659 682
660 683 //****************************************
661 684 // create the queue for handling valid TCs
662 685 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
663 686 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
664 687 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
665 688 if ( status_recv != RTEMS_SUCCESSFUL ) {
666 689 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
667 690 }
668 691
669 692 //************************************************
670 693 // create the queue for handling TM packet sending
671 694 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
672 695 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
673 696 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
674 697 if ( status_send != RTEMS_SUCCESSFUL ) {
675 698 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
676 699 }
677 700
678 701 //*****************************************************************************
679 702 // create the queue for handling averaged spectral matrices for processing @ f0
680 703 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
681 704 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
682 705 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
683 706 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
684 707 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
685 708 }
686 709
687 710 //*****************************************************************************
688 711 // create the queue for handling averaged spectral matrices for processing @ f1
689 712 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
690 713 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
691 714 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
692 715 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
693 716 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
694 717 }
695 718
696 719 //*****************************************************************************
697 720 // create the queue for handling averaged spectral matrices for processing @ f2
698 721 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
699 722 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
700 723 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
701 724 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
702 725 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
703 726 }
704 727
705 728 if ( status_recv != RTEMS_SUCCESSFUL )
706 729 {
707 730 ret = status_recv;
708 731 }
709 732 else if( status_send != RTEMS_SUCCESSFUL )
710 733 {
711 734 ret = status_send;
712 735 }
713 736 else if( status_q_p0 != RTEMS_SUCCESSFUL )
714 737 {
715 738 ret = status_q_p0;
716 739 }
717 740 else if( status_q_p1 != RTEMS_SUCCESSFUL )
718 741 {
719 742 ret = status_q_p1;
720 743 }
721 744 else
722 745 {
723 746 ret = status_q_p2;
724 747 }
725 748
726 749 return ret;
727 750 }
728 751
729 752 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
730 753 {
731 754 rtems_status_code status;
732 755 rtems_name queue_name;
733 756
734 757 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
735 758
736 759 status = rtems_message_queue_ident( queue_name, 0, queue_id );
737 760
738 761 return status;
739 762 }
740 763
741 764 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
742 765 {
743 766 rtems_status_code status;
744 767 rtems_name queue_name;
745 768
746 769 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
747 770
748 771 status = rtems_message_queue_ident( queue_name, 0, queue_id );
749 772
750 773 return status;
751 774 }
752 775
753 776 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
754 777 {
755 778 rtems_status_code status;
756 779 rtems_name queue_name;
757 780
758 781 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
759 782
760 783 status = rtems_message_queue_ident( queue_name, 0, queue_id );
761 784
762 785 return status;
763 786 }
764 787
765 788 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
766 789 {
767 790 rtems_status_code status;
768 791 rtems_name queue_name;
769 792
770 793 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
771 794
772 795 status = rtems_message_queue_ident( queue_name, 0, queue_id );
773 796
774 797 return status;
775 798 }
776 799
777 800 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
778 801 {
779 802 rtems_status_code status;
780 803 rtems_name queue_name;
781 804
782 805 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
783 806
784 807 status = rtems_message_queue_ident( queue_name, 0, queue_id );
785 808
786 809 return status;
787 810 }
Show file before | Show file after | Hide comments
@@ -1,1113 +1,1115
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 // printf("sid = %d\n", incomingRingNodePtr->sid);
259 258 if ( (sid==SID_NORM_CWF_LONG_F3)
260 259 || (sid==SID_BURST_CWF_F2 )
261 260 || (sid==SID_SBM1_CWF_F1 )
262 261 || (sid==SID_SBM2_CWF_F2 ))
263 262 {
264 263 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
265 264 }
266 265 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
267 266 {
268 267 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
269 268 }
270 269 else if ( (sid==SID_NORM_CWF_F3) )
271 270 {
272 271 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
273 272 }
274 273 else if ( (sid==SID_NORM_ASM_F0) || (SID_NORM_ASM_F1) || (SID_NORM_ASM_F2) )
275 274 {
276 275 spw_send_asm( incomingRingNodePtr, &headerASM );
277 276 }
278 277 else
279 278 {
280 279 printf("unexpected sid = %d\n", sid);
281 280 }
282 281 }
283 282 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
284 283 {
285 284 status = write( fdSPW, incomingData, size );
286 285 if (status == -1){
287 286 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
288 287 }
289 288 }
290 289 else // the incoming message is a spw_ioctl_pkt_send structure
291 290 {
292 291 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
293 292 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
294 293 if (status == -1){
295 294 printf("size = %d, %x, %x, %x, %x, %x\n",
296 295 size,
297 296 incomingData[0],
298 297 incomingData[1],
299 298 incomingData[2],
300 299 incomingData[3],
301 300 incomingData[4]);
302 301 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
303 302 }
304 303 }
305 304 }
306 305
307 306 status = rtems_message_queue_get_number_pending( queue_id, &count );
308 307 if (status != RTEMS_SUCCESSFUL)
309 308 {
310 309 PRINTF1("in SEND *** (3) ERR = %d\n", status)
311 310 }
312 311 else
313 312 {
314 313 if (count > maxCount)
315 314 {
316 315 maxCount = count;
317 316 }
318 317 }
319 318 }
320 319 }
321 320
322 321 rtems_task wtdg_task( rtems_task_argument argument )
323 322 {
324 323 rtems_event_set event_out;
325 324 rtems_status_code status;
326 325 int linkStatus;
327 326
328 327 BOOT_PRINTF("in WTDG ***\n")
329 328
330 329 while(1)
331 330 {
332 331 // wait for an RTEMS_EVENT
333 332 rtems_event_receive( RTEMS_EVENT_0,
334 333 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
335 334 PRINTF("in WTDG *** wait for the link\n")
336 335 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
337 336 while( linkStatus != 5) // wait for the link
338 337 {
339 338 rtems_task_wake_after( 10 );
340 339 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
341 340 }
342 341
343 342 status = spacewire_stop_and_start_link( fdSPW );
344 343
345 344 if (status != RTEMS_SUCCESSFUL)
346 345 {
347 346 PRINTF1("in WTDG *** ERR link not started %d\n", status)
348 347 }
349 348 else
350 349 {
351 350 PRINTF("in WTDG *** OK link started\n")
352 351 }
353 352
354 353 // restart the SPIQ task
355 354 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
356 355 if ( status != RTEMS_SUCCESSFUL ) {
357 356 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
358 357 }
359 358
360 359 // restart RECV and SEND
361 360 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
362 361 if ( status != RTEMS_SUCCESSFUL ) {
363 362 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
364 363 }
365 364 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
366 365 if ( status != RTEMS_SUCCESSFUL ) {
367 366 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
368 367 }
369 368 }
370 369 }
371 370
372 371 //****************
373 372 // OTHER FUNCTIONS
374 373 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
375 374 {
376 375 /** This function opens the SpaceWire link.
377 376 *
378 377 * @return a valid file descriptor in case of success, -1 in case of a failure
379 378 *
380 379 */
381 380 rtems_status_code status;
382 381
383 382 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
384 383 if ( fdSPW < 0 ) {
385 384 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
386 385 }
387 386 else
388 387 {
389 388 status = RTEMS_SUCCESSFUL;
390 389 }
391 390
392 391 return status;
393 392 }
394 393
395 394 int spacewire_start_link( int fd )
396 395 {
397 396 rtems_status_code status;
398 397
399 398 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
400 399 // -1 default hardcoded driver timeout
401 400
402 401 return status;
403 402 }
404 403
405 404 int spacewire_stop_and_start_link( int fd )
406 405 {
407 406 rtems_status_code status;
408 407
409 408 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
410 409 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
411 410 // -1 default hardcoded driver timeout
412 411
413 412 return status;
414 413 }
415 414
416 415 int spacewire_configure_link( int fd )
417 416 {
418 417 /** This function configures the SpaceWire link.
419 418 *
420 419 * @return GR-RTEMS-DRIVER directive status codes:
421 420 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
422 421 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
423 422 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
424 423 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
425 424 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
426 425 * - 5 EIO - Error when writing to grswp hardware registers.
427 426 * - 2 ENOENT - No such file or directory
428 427 */
429 428
430 429 rtems_status_code status;
431 430
432 431 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
433 432 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
434 433
435 434 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
436 435 if (status!=RTEMS_SUCCESSFUL) {
437 436 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
438 437 }
439 438 //
440 439 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 440 if (status!=RTEMS_SUCCESSFUL) {
442 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
441 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
443 442 }
444 443 //
445 444 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
446 445 if (status!=RTEMS_SUCCESSFUL) {
447 446 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
448 447 }
449 448 //
450 449 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
451 450 if (status!=RTEMS_SUCCESSFUL) {
452 451 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
453 452 }
454 453 //
455 454 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
456 455 if (status!=RTEMS_SUCCESSFUL) {
457 456 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
458 457 }
459 458 //
460 459 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
461 460 if (status!=RTEMS_SUCCESSFUL) {
462 461 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
463 462 }
464 463 //
465 464 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, 0x0909); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
466 465 if (status!=RTEMS_SUCCESSFUL) {
467 466 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
468 467 }
469 468
470 469 return status;
471 470 }
472 471
473 472 int spacewire_reset_link( void )
474 473 {
475 474 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
476 475 *
477 476 * @return RTEMS directive status code:
478 477 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
479 478 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
480 479 *
481 480 */
482 481
483 482 rtems_status_code status_spw;
484 483 int i;
485 484
486 485 for ( i=0; i<SY_LFR_DPU_CONNECT_ATTEMPT; i++ )
487 486 {
488 487 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
489 488
490 489 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
491 490
492 491 status_spw = spacewire_stop_and_start_link( fdSPW );
493 492 if ( status_spw != RTEMS_SUCCESSFUL )
494 493 {
495 494 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw)
496 495 }
497 496
498 497 if ( status_spw == RTEMS_SUCCESSFUL)
499 498 {
500 499 break;
501 500 }
502 501 }
503 502
504 503 return status_spw;
505 504 }
506 505
507 506 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
508 507 {
509 508 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
510 509 *
511 510 * @param val is the value, 0 or 1, used to set the value of the NP bit.
512 511 * @param regAddr is the address of the GRSPW control register.
513 512 *
514 513 * NP is the bit 20 of the GRSPW control register.
515 514 *
516 515 */
517 516
518 517 unsigned int *spwptr = (unsigned int*) regAddr;
519 518
520 519 if (val == 1) {
521 520 *spwptr = *spwptr | 0x00100000; // [NP] set the No port force bit
522 521 }
523 522 if (val== 0) {
524 523 *spwptr = *spwptr & 0xffdfffff;
525 524 }
526 525 }
527 526
528 527 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
529 528 {
530 529 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
531 530 *
532 531 * @param val is the value, 0 or 1, used to set the value of the RE bit.
533 532 * @param regAddr is the address of the GRSPW control register.
534 533 *
535 534 * RE is the bit 16 of the GRSPW control register.
536 535 *
537 536 */
538 537
539 538 unsigned int *spwptr = (unsigned int*) regAddr;
540 539
541 540 if (val == 1)
542 541 {
543 542 *spwptr = *spwptr | 0x00010000; // [RE] set the RMAP Enable bit
544 543 }
545 544 if (val== 0)
546 545 {
547 546 *spwptr = *spwptr & 0xfffdffff;
548 547 }
549 548 }
550 549
551 550 void spacewire_compute_stats_offsets( void )
552 551 {
553 552 /** This function computes the SpaceWire statistics offsets in case of a SpaceWire related interruption raising.
554 553 *
555 554 * The offsets keep a record of the statistics in case of a reset of the statistics. They are added to the current statistics
556 555 * 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 556 * during the open systel call).
558 557 *
559 558 */
560 559
561 560 spw_stats spacewire_stats_grspw;
562 561 rtems_status_code status;
563 562
564 563 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
565 564
566 565 spacewire_stats_backup.packets_received = spacewire_stats_grspw.packets_received
567 566 + spacewire_stats.packets_received;
568 567 spacewire_stats_backup.packets_sent = spacewire_stats_grspw.packets_sent
569 568 + spacewire_stats.packets_sent;
570 569 spacewire_stats_backup.parity_err = spacewire_stats_grspw.parity_err
571 570 + spacewire_stats.parity_err;
572 571 spacewire_stats_backup.disconnect_err = spacewire_stats_grspw.disconnect_err
573 572 + spacewire_stats.disconnect_err;
574 573 spacewire_stats_backup.escape_err = spacewire_stats_grspw.escape_err
575 574 + spacewire_stats.escape_err;
576 575 spacewire_stats_backup.credit_err = spacewire_stats_grspw.credit_err
577 576 + spacewire_stats.credit_err;
578 577 spacewire_stats_backup.write_sync_err = spacewire_stats_grspw.write_sync_err
579 578 + spacewire_stats.write_sync_err;
580 579 spacewire_stats_backup.rx_rmap_header_crc_err = spacewire_stats_grspw.rx_rmap_header_crc_err
581 580 + spacewire_stats.rx_rmap_header_crc_err;
582 581 spacewire_stats_backup.rx_rmap_data_crc_err = spacewire_stats_grspw.rx_rmap_data_crc_err
583 582 + spacewire_stats.rx_rmap_data_crc_err;
584 583 spacewire_stats_backup.early_ep = spacewire_stats_grspw.early_ep
585 584 + spacewire_stats.early_ep;
586 585 spacewire_stats_backup.invalid_address = spacewire_stats_grspw.invalid_address
587 586 + spacewire_stats.invalid_address;
588 587 spacewire_stats_backup.rx_eep_err = spacewire_stats_grspw.rx_eep_err
589 588 + spacewire_stats.rx_eep_err;
590 589 spacewire_stats_backup.rx_truncated = spacewire_stats_grspw.rx_truncated
591 590 + spacewire_stats.rx_truncated;
592 591 }
593 592
594 593 void spacewire_update_statistics( void )
595 594 {
596 595 rtems_status_code status;
597 596 spw_stats spacewire_stats_grspw;
598 597
599 598 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
600 599
601 600 spacewire_stats.packets_received = spacewire_stats_backup.packets_received
602 601 + spacewire_stats_grspw.packets_received;
603 602 spacewire_stats.packets_sent = spacewire_stats_backup.packets_sent
604 603 + spacewire_stats_grspw.packets_sent;
605 604 spacewire_stats.parity_err = spacewire_stats_backup.parity_err
606 605 + spacewire_stats_grspw.parity_err;
607 606 spacewire_stats.disconnect_err = spacewire_stats_backup.disconnect_err
608 607 + spacewire_stats_grspw.disconnect_err;
609 608 spacewire_stats.escape_err = spacewire_stats_backup.escape_err
610 609 + spacewire_stats_grspw.escape_err;
611 610 spacewire_stats.credit_err = spacewire_stats_backup.credit_err
612 611 + spacewire_stats_grspw.credit_err;
613 612 spacewire_stats.write_sync_err = spacewire_stats_backup.write_sync_err
614 613 + spacewire_stats_grspw.write_sync_err;
615 614 spacewire_stats.rx_rmap_header_crc_err = spacewire_stats_backup.rx_rmap_header_crc_err
616 615 + spacewire_stats_grspw.rx_rmap_header_crc_err;
617 616 spacewire_stats.rx_rmap_data_crc_err = spacewire_stats_backup.rx_rmap_data_crc_err
618 617 + spacewire_stats_grspw.rx_rmap_data_crc_err;
619 618 spacewire_stats.early_ep = spacewire_stats_backup.early_ep
620 619 + spacewire_stats_grspw.early_ep;
621 620 spacewire_stats.invalid_address = spacewire_stats_backup.invalid_address
622 621 + spacewire_stats_grspw.invalid_address;
623 622 spacewire_stats.rx_eep_err = spacewire_stats_backup.rx_eep_err
624 623 + spacewire_stats_grspw.rx_eep_err;
625 624 spacewire_stats.rx_truncated = spacewire_stats_backup.rx_truncated
626 625 + spacewire_stats_grspw.rx_truncated;
627 626 //spacewire_stats.tx_link_err;
628 627
629 628 //****************************
630 629 // DPU_SPACEWIRE_IF_STATISTICS
631 630 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (spacewire_stats.packets_received >> 8);
632 631 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (spacewire_stats.packets_received);
633 632 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (spacewire_stats.packets_sent >> 8);
634 633 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (spacewire_stats.packets_sent);
635 634 //housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt;
636 635 //housekeeping_packet.hk_lfr_dpu_spw_last_timc;
637 636
638 637 //******************************************
639 638 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
640 639 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) spacewire_stats.parity_err;
641 640 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) spacewire_stats.disconnect_err;
642 641 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) spacewire_stats.escape_err;
643 642 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) spacewire_stats.credit_err;
644 643 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) spacewire_stats.write_sync_err;
645 644
646 645 //*********************************************
647 646 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
648 647 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) spacewire_stats.early_ep;
649 648 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) spacewire_stats.invalid_address;
650 649 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) spacewire_stats.rx_eep_err;
651 650 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) spacewire_stats.rx_truncated;
652 651 }
653 652
654 653 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
655 654 {
656 655 // rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_9 );
657 656 struct grgpio_regs_str *grgpio_regs = (struct grgpio_regs_str *) REGS_ADDR_GRGPIO;
658 657
659 658 grgpio_regs->io_port_direction_register =
660 659 grgpio_regs->io_port_direction_register | 0x04; // [0000 0100], 0 = output disabled, 1 = output enabled
661 660
662 661 if ( (grgpio_regs->io_port_output_register & 0x04) == 0x04 )
663 662 {
664 663 grgpio_regs->io_port_output_register = grgpio_regs->io_port_output_register & 0xfb; // [1111 1011]
665 664 }
666 665 else
667 666 {
668 667 grgpio_regs->io_port_output_register = grgpio_regs->io_port_output_register | 0x04; // [0000 0100]
669 668 }
670 669 }
671 670
672 671 rtems_timer_service_routine user_routine( rtems_id timer_id, void *user_data )
673 672 {
674 673 int linkStatus;
675 674 rtems_status_code status;
676 675
677 676 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
678 677
679 678 if ( linkStatus == 5) {
680 679 PRINTF("in spacewire_reset_link *** link is running\n")
681 680 status = RTEMS_SUCCESSFUL;
682 681 }
683 682 }
684 683
685 684 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
686 685 {
687 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
688 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
689 header->reserved = DEFAULT_RESERVED;
690 header->userApplication = CCSDS_USER_APP;
691 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
692 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
686 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
687 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
688 header->reserved = DEFAULT_RESERVED;
689 header->userApplication = CCSDS_USER_APP;
690 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
691 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
693 692 header->packetLength[0] = 0x00;
694 693 header->packetLength[1] = 0x00;
695 694 // DATA FIELD HEADER
696 695 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
697 696 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
698 697 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
699 698 header->destinationID = TM_DESTINATION_ID_GROUND;
700 699 header->time[0] = 0x00;
701 700 header->time[0] = 0x00;
702 701 header->time[0] = 0x00;
703 702 header->time[0] = 0x00;
704 703 header->time[0] = 0x00;
705 704 header->time[0] = 0x00;
706 705 // AUXILIARY DATA HEADER
707 706 header->sid = 0x00;
708 707 header->hkBIA = DEFAULT_HKBIA;
709 708 header->blkNr[0] = 0x00;
710 709 header->blkNr[1] = 0x00;
711 710 }
712 711
713 712 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
714 713 {
715 714 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
716 715 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
717 716 header->reserved = DEFAULT_RESERVED;
718 717 header->userApplication = CCSDS_USER_APP;
719 718 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
720 719 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
721 720 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
722 721 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
723 722 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
724 723 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
725 724 // DATA FIELD HEADER
726 725 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
727 726 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
728 727 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
729 728 header->destinationID = TM_DESTINATION_ID_GROUND;
730 729 header->time[0] = 0x00;
731 730 header->time[0] = 0x00;
732 731 header->time[0] = 0x00;
733 732 header->time[0] = 0x00;
734 733 header->time[0] = 0x00;
735 734 header->time[0] = 0x00;
736 735 // AUXILIARY DATA HEADER
737 736 header->sid = 0x00;
738 737 header->hkBIA = DEFAULT_HKBIA;
739 738 header->pktCnt = DEFAULT_PKTCNT; // PKT_CNT
740 739 header->pktNr = 0x00;
741 740 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
742 741 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
743 742 }
744 743
745 744 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
746 745 {
747 746 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
748 747 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
749 748 header->reserved = DEFAULT_RESERVED;
750 749 header->userApplication = CCSDS_USER_APP;
751 750 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
752 751 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
753 752 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
754 753 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
755 754 header->packetLength[0] = 0x00;
756 755 header->packetLength[1] = 0x00;
757 756 // DATA FIELD HEADER
758 757 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
759 758 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
760 759 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
761 760 header->destinationID = TM_DESTINATION_ID_GROUND;
762 761 header->time[0] = 0x00;
763 762 header->time[0] = 0x00;
764 763 header->time[0] = 0x00;
765 764 header->time[0] = 0x00;
766 765 header->time[0] = 0x00;
767 766 header->time[0] = 0x00;
768 767 // AUXILIARY DATA HEADER
769 768 header->sid = 0x00;
770 769 header->biaStatusInfo = 0x00;
771 770 header->pa_lfr_pkt_cnt_asm = 0x00;
772 771 header->pa_lfr_pkt_nr_asm = 0x00;
773 772 header->pa_lfr_asm_blk_nr[0] = 0x00;
774 773 header->pa_lfr_asm_blk_nr[1] = 0x00;
775 774 }
776 775
777 776 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
778 777 Header_TM_LFR_SCIENCE_CWF_t *header )
779 778 {
780 779 /** This function sends CWF CCSDS packets (F2, F1 or F0).
781 780 *
782 781 * @param waveform points to the buffer containing the data that will be send.
783 782 * @param sid is the source identifier of the data that will be sent.
784 783 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
785 784 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
786 785 * contain information to setup the transmission of the data packets.
787 786 *
788 787 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
789 788 *
790 789 */
791 790
792 791 unsigned int i;
793 792 int ret;
794 793 unsigned int coarseTime;
795 794 unsigned int fineTime;
796 795 rtems_status_code status;
797 796 spw_ioctl_pkt_send spw_ioctl_send_CWF;
798 797 int *dataPtr;
799 798 unsigned char sid;
800 799
801 800 spw_ioctl_send_CWF.hlen = TM_HEADER_LEN + 4 + 10; // + 4 is for the protocole extra header, + 10 is for the auxiliary header
802 801 spw_ioctl_send_CWF.options = 0;
803 802
804 803 ret = LFR_DEFAULT;
805 804 sid = (unsigned char) ring_node_to_send->sid;
806 805
807 806 coarseTime = ring_node_to_send->coarseTime;
808 807 fineTime = ring_node_to_send->fineTime;
809 808 dataPtr = (int*) ring_node_to_send->buffer_address;
810 809
811 810 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
812 811 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
813 812 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
814 813 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
815 814
816 815 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
817 816 {
818 817 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
819 818 spw_ioctl_send_CWF.hdr = (char*) header;
820 819 // BUILD THE DATA
821 820 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
822 821
823 822 // SET PACKET SEQUENCE CONTROL
824 823 increment_seq_counter_source_id( header->packetSequenceControl, sid );
825 824
826 825 // SET SID
827 826 header->sid = sid;
828 827
829 828 // SET PACKET TIME
830 829 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
831 830 //
832 831 header->time[0] = header->acquisitionTime[0];
833 832 header->time[1] = header->acquisitionTime[1];
834 833 header->time[2] = header->acquisitionTime[2];
835 834 header->time[3] = header->acquisitionTime[3];
836 835 header->time[4] = header->acquisitionTime[4];
837 836 header->time[5] = header->acquisitionTime[5];
838 837
839 838 // SET PACKET ID
840 839 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
841 840 {
842 841 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8);
843 842 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
844 843 }
845 844 else
846 845 {
847 846 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
848 847 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
849 848 }
850 849
851 850 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
852 851 if (status != RTEMS_SUCCESSFUL) {
853 852 printf("%d-%d, ERR %d\n", sid, i, (int) status);
854 853 ret = LFR_DEFAULT;
855 854 }
856 855 }
857 856
858 857 return ret;
859 858 }
860 859
861 860 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
862 861 Header_TM_LFR_SCIENCE_SWF_t *header )
863 862 {
864 863 /** This function sends SWF CCSDS packets (F2, F1 or F0).
865 864 *
866 865 * @param waveform points to the buffer containing the data that will be send.
867 866 * @param sid is the source identifier of the data that will be sent.
868 867 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
869 868 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
870 869 * contain information to setup the transmission of the data packets.
871 870 *
872 871 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
873 872 *
874 873 */
875 874
876 875 unsigned int i;
877 876 int ret;
878 877 unsigned int coarseTime;
879 878 unsigned int fineTime;
880 879 rtems_status_code status;
881 880 spw_ioctl_pkt_send spw_ioctl_send_SWF;
882 881 int *dataPtr;
883 882 unsigned char sid;
884 883
885 884 spw_ioctl_send_SWF.hlen = TM_HEADER_LEN + 4 + 12; // + 4 is for the protocole extra header, + 12 is for the auxiliary header
886 885 spw_ioctl_send_SWF.options = 0;
887 886
888 887 ret = LFR_DEFAULT;
889 888
890 889 coarseTime = ring_node_to_send->coarseTime;
891 890 fineTime = ring_node_to_send->fineTime;
892 891 dataPtr = (int*) ring_node_to_send->buffer_address;
893 892 sid = ring_node_to_send->sid;
894 893
895 894 for (i=0; i<7; i++) // send waveform
896 895 {
897 896 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
898 897 spw_ioctl_send_SWF.hdr = (char*) header;
899 898
900 899 // SET PACKET SEQUENCE CONTROL
901 900 increment_seq_counter_source_id( header->packetSequenceControl, sid );
902 901
903 902 // SET PACKET LENGTH AND BLKNR
904 903 if (i == 6)
905 904 {
906 905 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
907 906 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8);
908 907 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
909 908 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> 8);
910 909 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
911 910 }
912 911 else
913 912 {
914 913 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
915 914 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8);
916 915 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
917 916 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> 8);
918 917 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
919 918 }
920 919
921 920 // SET PACKET TIME
922 921 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
923 922 //
924 923 header->time[0] = header->acquisitionTime[0];
925 924 header->time[1] = header->acquisitionTime[1];
926 925 header->time[2] = header->acquisitionTime[2];
927 926 header->time[3] = header->acquisitionTime[3];
928 927 header->time[4] = header->acquisitionTime[4];
929 928 header->time[5] = header->acquisitionTime[5];
930 929
931 930 // SET SID
932 931 header->sid = sid;
933 932
934 933 // SET PKTNR
935 934 header->pktNr = i+1; // PKT_NR
936 935
937 936 // SEND PACKET
938 937 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
939 938 if (status != RTEMS_SUCCESSFUL) {
940 939 printf("%d-%d, ERR %d\n", sid, i, (int) status);
941 940 ret = LFR_DEFAULT;
942 941 }
943 942 }
944 943
945 944 return ret;
946 945 }
947 946
948 947 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
949 948 Header_TM_LFR_SCIENCE_CWF_t *header )
950 949 {
951 950 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
952 951 *
953 952 * @param waveform points to the buffer containing the data that will be send.
954 953 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
955 954 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
956 955 * contain information to setup the transmission of the data packets.
957 956 *
958 957 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
959 958 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
960 959 *
961 960 */
962 961
963 962 unsigned int i;
964 963 int ret;
965 964 unsigned int coarseTime;
966 965 unsigned int fineTime;
967 966 rtems_status_code status;
968 967 spw_ioctl_pkt_send spw_ioctl_send_CWF;
969 968 char *dataPtr;
970 969 unsigned char sid;
971 970
972 971 spw_ioctl_send_CWF.hlen = TM_HEADER_LEN + 4 + 10; // + 4 is for the protocole extra header, + 10 is for the auxiliary header
973 972 spw_ioctl_send_CWF.options = 0;
974 973
975 974 ret = LFR_DEFAULT;
976 975 sid = ring_node_to_send->sid;
977 976
978 977 coarseTime = ring_node_to_send->coarseTime;
979 978 fineTime = ring_node_to_send->fineTime;
980 979 dataPtr = (char*) ring_node_to_send->buffer_address;
981 980
982 981 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> 8);
983 982 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
984 983 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> 8);
985 984 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
986 985
986 printf("spw_send_waveform_CWF3_light => [0] = %x, [1] = %x, [2] = %x, [3] = %x, [4] = %x, [5] = %x\n",
987 dataPtr[0], dataPtr[1], dataPtr[2], dataPtr[3], dataPtr[4], dataPtr[5]);
988
987 989 //*********************
988 990 // SEND CWF3_light DATA
989 991 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
990 992 {
991 993 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
992 994 spw_ioctl_send_CWF.hdr = (char*) header;
993 995 // BUILD THE DATA
994 996 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
995 997
996 998 // SET PACKET SEQUENCE COUNTER
997 999 increment_seq_counter_source_id( header->packetSequenceControl, sid );
998 1000
999 1001 // SET SID
1000 1002 header->sid = sid;
1001 1003
1002 1004 // SET PACKET TIME
1003 1005 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1004 1006 //
1005 1007 header->time[0] = header->acquisitionTime[0];
1006 1008 header->time[1] = header->acquisitionTime[1];
1007 1009 header->time[2] = header->acquisitionTime[2];
1008 1010 header->time[3] = header->acquisitionTime[3];
1009 1011 header->time[4] = header->acquisitionTime[4];
1010 1012 header->time[5] = header->acquisitionTime[5];
1011 1013
1012 1014 // SET PACKET ID
1013 1015 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1014 1016 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1015 1017
1016 1018 // SEND PACKET
1017 1019 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1018 1020 if (status != RTEMS_SUCCESSFUL) {
1019 1021 printf("%d-%d, ERR %d\n", sid, i, (int) status);
1020 1022 ret = LFR_DEFAULT;
1021 1023 }
1022 1024 }
1023 1025
1024 1026 return ret;
1025 1027 }
1026 1028
1027 1029 void spw_send_asm( ring_node *ring_node_to_send,
1028 1030 Header_TM_LFR_SCIENCE_ASM_t *header )
1029 1031 {
1030 1032 unsigned int i;
1031 1033 unsigned int length = 0;
1032 1034 rtems_status_code status;
1033 1035 unsigned int sid;
1034 1036 char *spectral_matrix;
1035 1037 int coarseTime;
1036 1038 int fineTime;
1037 1039 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1038 1040
1039 1041 sid = ring_node_to_send->sid;
1040 1042 spectral_matrix = (char*) ring_node_to_send->buffer_address;
1041 1043 coarseTime = ring_node_to_send->coarseTime;
1042 1044 fineTime = ring_node_to_send->fineTime;
1043 1045
1044 1046 for (i=0; i<2; i++)
1045 1047 {
1046 1048 // (1) BUILD THE DATA
1047 1049 switch(sid)
1048 1050 {
1049 1051 case SID_NORM_ASM_F0:
1050 1052 spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F0_IN_BYTES / 2; // 2 packets will be sent
1051 1053 spw_ioctl_send_ASM.data = &spectral_matrix[
1052 1054 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0) ) * NB_VALUES_PER_SM ) * 2
1053 1055 ];
1054 1056 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0;
1055 1057 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0) >> 8 ); // BLK_NR MSB
1056 1058 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0); // BLK_NR LSB
1057 1059 break;
1058 1060 case SID_NORM_ASM_F1:
1059 1061 spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F1_IN_BYTES / 2; // 2 packets will be sent
1060 1062 spw_ioctl_send_ASM.data = &spectral_matrix[
1061 1063 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1) ) * NB_VALUES_PER_SM ) * 2
1062 1064 ];
1063 1065 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1;
1064 1066 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1) >> 8 ); // BLK_NR MSB
1065 1067 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1); // BLK_NR LSB
1066 1068 break;
1067 1069 case SID_NORM_ASM_F2:
1068 1070 spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F2_IN_BYTES / 2; // 2 packets will be sent
1069 1071 spw_ioctl_send_ASM.data = &spectral_matrix[
1070 1072 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM ) * 2
1071 1073 ];
1072 1074 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1073 1075 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB
1074 1076 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1075 1077 break;
1076 1078 default:
1077 1079 PRINTF1("ERR *** in spw_send_asm *** unexpected sid %d\n", sid)
1078 1080 break;
1079 1081 }
1080 1082 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM + CCSDS_PROTOCOLE_EXTRA_BYTES;
1081 1083 spw_ioctl_send_ASM.hdr = (char *) header;
1082 1084 spw_ioctl_send_ASM.options = 0;
1083 1085
1084 1086 // (2) BUILD THE HEADER
1085 1087 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1086 1088 header->packetLength[0] = (unsigned char) (length>>8);
1087 1089 header->packetLength[1] = (unsigned char) (length);
1088 1090 header->sid = (unsigned char) sid; // SID
1089 1091 header->pa_lfr_pkt_cnt_asm = 2;
1090 1092 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1091 1093
1092 1094 // (3) SET PACKET TIME
1093 1095 header->time[0] = (unsigned char) (coarseTime>>24);
1094 1096 header->time[1] = (unsigned char) (coarseTime>>16);
1095 1097 header->time[2] = (unsigned char) (coarseTime>>8);
1096 1098 header->time[3] = (unsigned char) (coarseTime);
1097 1099 header->time[4] = (unsigned char) (fineTime>>8);
1098 1100 header->time[5] = (unsigned char) (fineTime);
1099 1101 //
1100 1102 header->acquisitionTime[0] = header->time[0];
1101 1103 header->acquisitionTime[1] = header->time[1];
1102 1104 header->acquisitionTime[2] = header->time[2];
1103 1105 header->acquisitionTime[3] = header->time[3];
1104 1106 header->acquisitionTime[4] = header->time[4];
1105 1107 header->acquisitionTime[5] = header->time[5];
1106 1108
1107 1109 // (4) SEND PACKET
1108 1110 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1109 1111 if (status != RTEMS_SUCCESSFUL) {
1110 1112 printf("in ASM_send *** ERR %d\n", (int) status);
1111 1113 }
1112 1114 }
1113 1115 }
Show file before | Show file after | Hide comments
@@ -1,391 +1,391
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 27
28 28 float k_coeff_intercalib_f0_norm[ NB_BINS_COMPRESSED_SM_F0 * NB_K_COEFF_PER_BIN ]; // 11 * 32 = 352
29 29 float k_coeff_intercalib_f0_sbm[ NB_BINS_COMPRESSED_SM_SBM_F0 * NB_K_COEFF_PER_BIN ]; // 22 * 32 = 704
30 30
31 31 //************
32 32 // RTEMS TASKS
33 33
34 34 rtems_task avf0_task( rtems_task_argument lfrRequestedMode )
35 35 {
36 36 int i;
37 37
38 38 rtems_event_set event_out;
39 39 rtems_status_code status;
40 40 rtems_id queue_id_prc0;
41 41 asm_msg msgForMATR;
42 42 ring_node *nodeForAveraging;
43 43 ring_node *ring_node_tab[8];
44 44 ring_node_asm *current_ring_node_asm_burst_sbm_f0;
45 45 ring_node_asm *current_ring_node_asm_norm_f0;
46 46
47 47 unsigned int nb_norm_bp1;
48 48 unsigned int nb_norm_bp2;
49 49 unsigned int nb_norm_asm;
50 50 unsigned int nb_sbm_bp1;
51 51 unsigned int nb_sbm_bp2;
52 52
53 53 nb_norm_bp1 = 0;
54 54 nb_norm_bp2 = 0;
55 55 nb_norm_asm = 0;
56 56 nb_sbm_bp1 = 0;
57 57 nb_sbm_bp2 = 0;
58 58
59 59 reset_nb_sm_f0( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
60 60 ASM_generic_init_ring( asm_ring_norm_f0, NB_RING_NODES_ASM_NORM_F0 );
61 61 ASM_generic_init_ring( asm_ring_burst_sbm_f0, NB_RING_NODES_ASM_BURST_SBM_F0 );
62 62 current_ring_node_asm_norm_f0 = asm_ring_norm_f0;
63 63 current_ring_node_asm_burst_sbm_f0 = asm_ring_burst_sbm_f0;
64 64
65 65 BOOT_PRINTF1("in AVFO *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
66 66
67 67 status = get_message_queue_id_prc0( &queue_id_prc0 );
68 68 if (status != RTEMS_SUCCESSFUL)
69 69 {
70 70 PRINTF1("in MATR *** ERR get_message_queue_id_prc0 %d\n", status)
71 71 }
72 72
73 73 while(1){
74 74 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
75 75
76 76 //****************************************
77 77 // initialize the mesage for the MATR task
78 78 msgForMATR.norm = current_ring_node_asm_norm_f0;
79 79 msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f0;
80 80 msgForMATR.event = 0x00; // this composite event will be sent to the PRC0 task
81 81 //
82 82 //****************************************
83 83
84 84 nodeForAveraging = getRingNodeForAveraging( 0 );
85 85
86 86 ring_node_tab[NB_SM_BEFORE_AVF0-1] = nodeForAveraging;
87 87 for ( i = 2; i < (NB_SM_BEFORE_AVF0+1); i++ )
88 88 {
89 89 nodeForAveraging = nodeForAveraging->previous;
90 90 ring_node_tab[NB_SM_BEFORE_AVF0-i] = nodeForAveraging;
91 91 }
92 92
93 93 // compute the average and store it in the averaged_sm_f1 buffer
94 94 SM_average( current_ring_node_asm_norm_f0->matrix,
95 95 current_ring_node_asm_burst_sbm_f0->matrix,
96 96 ring_node_tab,
97 97 nb_norm_bp1, nb_sbm_bp1,
98 98 &msgForMATR );
99 99
100 100 // update nb_average
101 101 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0;
102 102 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0;
103 103 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0;
104 104 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0;
105 105 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0;
106 106
107 107 if (nb_sbm_bp1 == nb_sm_before_f0.burst_sbm_bp1)
108 108 {
109 109 nb_sbm_bp1 = 0;
110 110 // set another ring for the ASM storage
111 111 current_ring_node_asm_burst_sbm_f0 = current_ring_node_asm_burst_sbm_f0->next;
112 112 if ( lfrCurrentMode == LFR_MODE_BURST )
113 113 {
114 114 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP1_F0;
115 115 }
116 116 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
117 117 {
118 118 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP1_F0;
119 119 }
120 120 }
121 121
122 122 if (nb_sbm_bp2 == nb_sm_before_f0.burst_sbm_bp2)
123 123 {
124 124 nb_sbm_bp2 = 0;
125 125 if ( lfrCurrentMode == LFR_MODE_BURST )
126 126 {
127 127 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP2_F0;
128 128 }
129 129 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
130 130 {
131 131 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP2_F0;
132 132 }
133 133 }
134 134
135 135 if (nb_norm_bp1 == nb_sm_before_f0.norm_bp1)
136 136 {
137 137 nb_norm_bp1 = 0;
138 138 // set another ring for the ASM storage
139 139 current_ring_node_asm_norm_f0 = current_ring_node_asm_norm_f0->next;
140 140 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
141 141 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
142 142 {
143 143 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F0;
144 144 }
145 145 }
146 146
147 147 if (nb_norm_bp2 == nb_sm_before_f0.norm_bp2)
148 148 {
149 149 nb_norm_bp2 = 0;
150 150 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
151 151 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
152 152 {
153 153 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F0;
154 154 }
155 155 }
156 156
157 157 if (nb_norm_asm == nb_sm_before_f0.norm_asm)
158 158 {
159 159 nb_norm_asm = 0;
160 160 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
161 161 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
162 162 {
163 163 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F0;
164 164 }
165 165 }
166 166
167 167 //*************************
168 168 // send the message to MATR
169 169 if (msgForMATR.event != 0x00)
170 170 {
171 171 status = rtems_message_queue_send( queue_id_prc0, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC0);
172 172 }
173 173
174 174 if (status != RTEMS_SUCCESSFUL) {
175 175 printf("in AVF0 *** Error sending message to MATR, code %d\n", status);
176 176 }
177 177 }
178 178 }
179 179
180 180 rtems_task prc0_task( rtems_task_argument lfrRequestedMode )
181 181 {
182 182 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
183 183 size_t size; // size of the incoming TC packet
184 184 asm_msg *incomingMsg;
185 185 //
186 186 unsigned char sid;
187 187 rtems_status_code status;
188 188 rtems_id queue_id;
189 189 rtems_id queue_id_q_p0;
190 190 bp_packet_with_spare packet_norm_bp1;
191 191 bp_packet packet_norm_bp2;
192 192 bp_packet packet_sbm_bp1;
193 193 bp_packet packet_sbm_bp2;
194 194 ring_node *current_ring_node_to_send_asm_f0;
195 195
196 196 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
197 197 init_ring( ring_to_send_asm_f0, NB_RING_NODES_ASM_F0, (volatile int*) buffer_asm_f0, TOTAL_SIZE_SM );
198 198 current_ring_node_to_send_asm_f0 = ring_to_send_asm_f0;
199 199
200 200 //*************
201 201 // NORM headers
202 BP_init_header_with_spare( &packet_norm_bp1.header,
202 BP_init_header_with_spare( &packet_norm_bp1,
203 203 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F0,
204 204 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0, NB_BINS_COMPRESSED_SM_F0 );
205 205 BP_init_header( &packet_norm_bp2,
206 206 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F0,
207 207 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0, NB_BINS_COMPRESSED_SM_F0);
208 208
209 209 //****************************
210 210 // BURST SBM1 and SBM2 headers
211 211 if ( lfrRequestedMode == LFR_MODE_BURST )
212 212 {
213 213 BP_init_header( &packet_sbm_bp1,
214 214 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F0,
215 215 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
216 216 BP_init_header( &packet_sbm_bp2,
217 217 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F0,
218 218 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
219 219 }
220 220 else if ( lfrRequestedMode == LFR_MODE_SBM1 )
221 221 {
222 222 BP_init_header( &packet_sbm_bp1,
223 223 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP1_F0,
224 224 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
225 225 BP_init_header( &packet_sbm_bp2,
226 226 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP2_F0,
227 227 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
228 228 }
229 229 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
230 230 {
231 231 BP_init_header( &packet_sbm_bp1,
232 232 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F0,
233 233 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
234 234 BP_init_header( &packet_sbm_bp2,
235 235 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F0,
236 236 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
237 237 }
238 238 else
239 239 {
240 240 PRINTF1("in PRC0 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
241 241 }
242 242
243 243 status = get_message_queue_id_send( &queue_id );
244 244 if (status != RTEMS_SUCCESSFUL)
245 245 {
246 246 PRINTF1("in PRC0 *** ERR get_message_queue_id_send %d\n", status)
247 247 }
248 248 status = get_message_queue_id_prc0( &queue_id_q_p0);
249 249 if (status != RTEMS_SUCCESSFUL)
250 250 {
251 251 PRINTF1("in PRC0 *** ERR get_message_queue_id_prc0 %d\n", status)
252 252 }
253 253
254 254 BOOT_PRINTF1("in PRC0 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
255 255
256 256 while(1){
257 257 status = rtems_message_queue_receive( queue_id_q_p0, incomingData, &size, //************************************
258 258 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
259 259
260 260 incomingMsg = (asm_msg*) incomingData;
261 261
262 262 //****************
263 263 //****************
264 264 // BURST SBM1 SBM2
265 265 //****************
266 266 //****************
267 267 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F0 ) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F0 ) )
268 268 {
269 269 sid = getSID( incomingMsg->event );
270 270 // 1) compress the matrix for Basic Parameters calculation
271 271 ASM_compress_reorganize_and_divide( incomingMsg->burst_sbm->matrix, compressed_sm_sbm_f0,
272 272 nb_sm_before_f0.burst_sbm_bp1,
273 273 NB_BINS_COMPRESSED_SM_SBM_F0, NB_BINS_TO_AVERAGE_ASM_SBM_F0,
274 274 ASM_F0_INDICE_START);
275 275 // 2) compute the BP1 set
276 276 BP1_set( compressed_sm_sbm_f0, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp1.data );
277 277 // 3) send the BP1 set
278 278 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
279 279 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
280 280 BP_send( (char *) &packet_sbm_bp1, queue_id,
281 281 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0 + PACKET_LENGTH_DELTA,
282 282 sid);
283 283 // 4) compute the BP2 set if needed
284 284 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F0) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F0) )
285 285 {
286 286 // 1) compute the BP2 set
287 287 BP2_set( compressed_sm_sbm_f0, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp2.data );
288 288 // 2) send the BP2 set
289 289 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
290 290 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
291 291 BP_send( (char *) &packet_sbm_bp2, queue_id,
292 292 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0 + PACKET_LENGTH_DELTA,
293 293 sid);
294 294 }
295 295 }
296 296
297 297 //*****
298 298 //*****
299 299 // NORM
300 300 //*****
301 301 //*****
302 302 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F0)
303 303 {
304 304 // 1) compress the matrix for Basic Parameters calculation
305 305 ASM_compress_reorganize_and_divide( incomingMsg->norm->matrix, compressed_sm_norm_f0,
306 306 nb_sm_before_f0.norm_bp1,
307 307 NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0,
308 308 ASM_F0_INDICE_START );
309 309 // 2) compute the BP1 set
310 310 BP1_set( compressed_sm_norm_f0, k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp1.data );
311 311 // 3) send the BP1 set
312 set_time( packet_norm_bp1.header.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
313 set_time( packet_norm_bp1.header.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
312 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
313 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
314 314 BP_send( (char *) &packet_norm_bp1, queue_id,
315 315 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0 + PACKET_LENGTH_DELTA,
316 316 SID_NORM_BP1_F0 );
317 317 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F0)
318 318 {
319 319 // 1) compute the BP2 set using the same ASM as the one used for BP1
320 320 BP2_set( compressed_sm_norm_f0, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp2.data );
321 321 // 2) send the BP2 set
322 322 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
323 323 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
324 324 BP_send( (char *) &packet_norm_bp2, queue_id,
325 325 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0 + PACKET_LENGTH_DELTA,
326 326 SID_NORM_BP2_F0);
327 327 }
328 328 }
329 329
330 330 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F0)
331 331 {
332 332 // 1) reorganize the ASM and divide
333 333 ASM_reorganize_and_divide( incomingMsg->norm->matrix,
334 334 asm_f0_reorganized,
335 335 nb_sm_before_f0.norm_bp1 );
336 336 // 2) convert the float array in a char array
337 337 ASM_convert( asm_f0_reorganized, (char*) current_ring_node_to_send_asm_f0->buffer_address );
338 338 current_ring_node_to_send_asm_f0->coarseTime = incomingMsg->coarseTimeNORM;
339 339 current_ring_node_to_send_asm_f0->fineTime = incomingMsg->fineTimeNORM;
340 340 current_ring_node_to_send_asm_f0->sid = SID_NORM_ASM_F0;
341 341
342 342 // 3) send the spectral matrix packets
343 343 status = rtems_message_queue_send( queue_id, &current_ring_node_to_send_asm_f0, sizeof( ring_node* ) );
344 344 // change asm ring node
345 345 current_ring_node_to_send_asm_f0 = current_ring_node_to_send_asm_f0->next;
346 346 }
347 347 }
348 348 }
349 349
350 350 //**********
351 351 // FUNCTIONS
352 352
353 353 void reset_nb_sm_f0( unsigned char lfrMode )
354 354 {
355 355 nb_sm_before_f0.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 96;
356 356 nb_sm_before_f0.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 96;
357 357 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;
358 358 nb_sm_before_f0.sbm1_bp1 = parameter_dump_packet.sy_lfr_s1_bp_p0 * 24; // 0.25 s per digit
359 359 nb_sm_before_f0.sbm1_bp2 = parameter_dump_packet.sy_lfr_s1_bp_p1 * 96;
360 360 nb_sm_before_f0.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 96;
361 361 nb_sm_before_f0.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 96;
362 362 nb_sm_before_f0.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 96;
363 363 nb_sm_before_f0.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 96;
364 364
365 365 if (lfrMode == LFR_MODE_SBM1)
366 366 {
367 367 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm1_bp1;
368 368 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm1_bp2;
369 369 }
370 370 else if (lfrMode == LFR_MODE_SBM2)
371 371 {
372 372 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm2_bp1;
373 373 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm2_bp2;
374 374 }
375 375 else if (lfrMode == LFR_MODE_BURST)
376 376 {
377 377 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
378 378 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
379 379 }
380 380 else
381 381 {
382 382 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
383 383 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
384 384 }
385 385 }
386 386
387 387 void init_k_coefficients_f0( void )
388 388 {
389 389 init_k_coefficients( k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0 );
390 390 init_k_coefficients( k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_SBM_F0);
391 391 }
Show file before | Show file after | Hide comments
@@ -1,379 +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 14 extern ring_node sm_ring_f1[ ];
15 15
16 16 //***
17 17 // F1
18 18 ring_node_asm asm_ring_norm_f1 [ NB_RING_NODES_ASM_NORM_F1 ];
19 19 ring_node_asm asm_ring_burst_sbm_f1 [ NB_RING_NODES_ASM_BURST_SBM_F1 ];
20 20
21 21 ring_node ring_to_send_asm_f1 [ NB_RING_NODES_ASM_F1 ];
22 22 int buffer_asm_f1 [ NB_RING_NODES_ASM_F1 * TOTAL_SIZE_SM ];
23 23
24 24 float asm_f1_reorganized [ TOTAL_SIZE_SM ];
25 25 char asm_f1_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ];
26 26 float compressed_sm_norm_f1[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F1];
27 27 float compressed_sm_sbm_f1 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 ];
28 28
29 29 float k_coeff_intercalib_f1_norm[ NB_BINS_COMPRESSED_SM_F1 * NB_K_COEFF_PER_BIN ]; // 13 * 32 = 416
30 30 float k_coeff_intercalib_f1_sbm[ NB_BINS_COMPRESSED_SM_SBM_F1 * NB_K_COEFF_PER_BIN ]; // 26 * 32 = 832
31 31
32 32 //************
33 33 // RTEMS TASKS
34 34
35 35 rtems_task avf1_task( rtems_task_argument lfrRequestedMode )
36 36 {
37 37 int i;
38 38
39 39 rtems_event_set event_out;
40 40 rtems_status_code status;
41 41 rtems_id queue_id_prc1;
42 42 asm_msg msgForMATR;
43 43 ring_node *nodeForAveraging;
44 44 ring_node *ring_node_tab[NB_SM_BEFORE_AVF0];
45 45 ring_node_asm *current_ring_node_asm_burst_sbm_f1;
46 46 ring_node_asm *current_ring_node_asm_norm_f1;
47 47
48 48 unsigned int nb_norm_bp1;
49 49 unsigned int nb_norm_bp2;
50 50 unsigned int nb_norm_asm;
51 51 unsigned int nb_sbm_bp1;
52 52 unsigned int nb_sbm_bp2;
53 53
54 54 nb_norm_bp1 = 0;
55 55 nb_norm_bp2 = 0;
56 56 nb_norm_asm = 0;
57 57 nb_sbm_bp1 = 0;
58 58 nb_sbm_bp2 = 0;
59 59
60 60 reset_nb_sm_f1( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
61 61 ASM_generic_init_ring( asm_ring_norm_f1, NB_RING_NODES_ASM_NORM_F1 );
62 62 ASM_generic_init_ring( asm_ring_burst_sbm_f1, NB_RING_NODES_ASM_BURST_SBM_F1 );
63 63 current_ring_node_asm_norm_f1 = asm_ring_norm_f1;
64 64 current_ring_node_asm_burst_sbm_f1 = asm_ring_burst_sbm_f1;
65 65
66 66 BOOT_PRINTF1("in AVF1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
67 67
68 68 status = get_message_queue_id_prc1( &queue_id_prc1 );
69 69 if (status != RTEMS_SUCCESSFUL)
70 70 {
71 71 PRINTF1("in AVF1 *** ERR get_message_queue_id_prc1 %d\n", status)
72 72 }
73 73
74 74 while(1){
75 75 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
76 76
77 77 //****************************************
78 78 // initialize the mesage for the MATR task
79 79 msgForMATR.norm = current_ring_node_asm_norm_f1;
80 80 msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f1;
81 81 msgForMATR.event = 0x00; // this composite event will be sent to the PRC1 task
82 82 //
83 83 //****************************************
84 84
85 85 nodeForAveraging = getRingNodeForAveraging( 1 );
86 86
87 87 ring_node_tab[NB_SM_BEFORE_AVF1-1] = nodeForAveraging;
88 88 for ( i = 2; i < (NB_SM_BEFORE_AVF1+1); i++ )
89 89 {
90 90 nodeForAveraging = nodeForAveraging->previous;
91 91 ring_node_tab[NB_SM_BEFORE_AVF1-i] = nodeForAveraging;
92 92 }
93 93
94 94 // compute the average and store it in the averaged_sm_f1 buffer
95 95 SM_average( current_ring_node_asm_norm_f1->matrix,
96 96 current_ring_node_asm_burst_sbm_f1->matrix,
97 97 ring_node_tab,
98 98 nb_norm_bp1, nb_sbm_bp1,
99 99 &msgForMATR );
100 100
101 101 // update nb_average
102 102 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF1;
103 103 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF1;
104 104 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF1;
105 105 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF1;
106 106 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF1;
107 107
108 108 if (nb_sbm_bp1 == nb_sm_before_f1.burst_sbm_bp1)
109 109 {
110 110 nb_sbm_bp1 = 0;
111 111 // set another ring for the ASM storage
112 112 current_ring_node_asm_burst_sbm_f1 = current_ring_node_asm_burst_sbm_f1->next;
113 113 if ( lfrCurrentMode == LFR_MODE_BURST )
114 114 {
115 115 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP1_F1;
116 116 }
117 117 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
118 118 {
119 119 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP1_F1;
120 120 }
121 121 }
122 122
123 123 if (nb_sbm_bp2 == nb_sm_before_f1.burst_sbm_bp2)
124 124 {
125 125 nb_sbm_bp2 = 0;
126 126 if ( lfrCurrentMode == LFR_MODE_BURST )
127 127 {
128 128 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP2_F1;
129 129 }
130 130 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
131 131 {
132 132 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP2_F1;
133 133 }
134 134 }
135 135
136 136 if (nb_norm_bp1 == nb_sm_before_f1.norm_bp1)
137 137 {
138 138 nb_norm_bp1 = 0;
139 139 // set another ring for the ASM storage
140 140 current_ring_node_asm_norm_f1 = current_ring_node_asm_norm_f1->next;
141 141 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
142 142 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
143 143 {
144 144 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F1;
145 145 }
146 146 }
147 147
148 148 if (nb_norm_bp2 == nb_sm_before_f1.norm_bp2)
149 149 {
150 150 nb_norm_bp2 = 0;
151 151 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
152 152 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
153 153 {
154 154 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F1;
155 155 }
156 156 }
157 157
158 158 if (nb_norm_asm == nb_sm_before_f1.norm_asm)
159 159 {
160 160 nb_norm_asm = 0;
161 161 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
162 162 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
163 163 {
164 164 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F1;
165 165 }
166 166 }
167 167
168 168 //*************************
169 169 // send the message to MATR
170 170 if (msgForMATR.event != 0x00)
171 171 {
172 172 status = rtems_message_queue_send( queue_id_prc1, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC1);
173 173 }
174 174
175 175 if (status != RTEMS_SUCCESSFUL) {
176 176 printf("in AVF1 *** Error sending message to PRC1, code %d\n", status);
177 177 }
178 178 }
179 179 }
180 180
181 181 rtems_task prc1_task( rtems_task_argument lfrRequestedMode )
182 182 {
183 183 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
184 184 size_t size; // size of the incoming TC packet
185 185 asm_msg *incomingMsg;
186 186 //
187 187 unsigned char sid;
188 188 rtems_status_code status;
189 189 rtems_id queue_id_send;
190 190 rtems_id queue_id_q_p1;
191 191 bp_packet_with_spare packet_norm_bp1;
192 192 bp_packet packet_norm_bp2;
193 193 bp_packet packet_sbm_bp1;
194 194 bp_packet packet_sbm_bp2;
195 195 ring_node *current_ring_node_to_send_asm_f1;
196 196
197 197 unsigned long long int localTime;
198 198
199 199 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
200 200 init_ring( ring_to_send_asm_f1, NB_RING_NODES_ASM_F1, (volatile int*) buffer_asm_f1, TOTAL_SIZE_SM );
201 201 current_ring_node_to_send_asm_f1 = ring_to_send_asm_f1;
202 202
203 203 //*************
204 204 // NORM headers
205 BP_init_header_with_spare( &packet_norm_bp1.header,
205 BP_init_header_with_spare( &packet_norm_bp1,
206 206 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F1,
207 207 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1, NB_BINS_COMPRESSED_SM_F1 );
208 208 BP_init_header( &packet_norm_bp2,
209 209 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F1,
210 210 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1, NB_BINS_COMPRESSED_SM_F1);
211 211
212 212 //***********************
213 213 // BURST and SBM2 headers
214 214 if ( lfrRequestedMode == LFR_MODE_BURST )
215 215 {
216 216 BP_init_header( &packet_sbm_bp1,
217 217 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F1,
218 218 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
219 219 BP_init_header( &packet_sbm_bp2,
220 220 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F1,
221 221 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
222 222 }
223 223 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
224 224 {
225 225 BP_init_header( &packet_sbm_bp1,
226 226 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F1,
227 227 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
228 228 BP_init_header( &packet_sbm_bp2,
229 229 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F1,
230 230 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
231 231 }
232 232 else
233 233 {
234 234 PRINTF1("in PRC1 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
235 235 }
236 236
237 237 status = get_message_queue_id_send( &queue_id_send );
238 238 if (status != RTEMS_SUCCESSFUL)
239 239 {
240 240 PRINTF1("in PRC1 *** ERR get_message_queue_id_send %d\n", status)
241 241 }
242 242 status = get_message_queue_id_prc1( &queue_id_q_p1);
243 243 if (status != RTEMS_SUCCESSFUL)
244 244 {
245 245 PRINTF1("in PRC1 *** ERR get_message_queue_id_prc1 %d\n", status)
246 246 }
247 247
248 248 BOOT_PRINTF1("in PRC1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
249 249
250 250 while(1){
251 251 status = rtems_message_queue_receive( queue_id_q_p1, incomingData, &size, //************************************
252 252 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
253 253
254 254 incomingMsg = (asm_msg*) incomingData;
255 255
256 256 localTime = getTimeAsUnsignedLongLongInt( );
257 257 //***********
258 258 //***********
259 259 // BURST SBM2
260 260 //***********
261 261 //***********
262 262 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F1) )
263 263 {
264 264 sid = getSID( incomingMsg->event );
265 265 // 1) compress the matrix for Basic Parameters calculation
266 266 ASM_compress_reorganize_and_divide( incomingMsg->burst_sbm->matrix, compressed_sm_sbm_f1,
267 267 nb_sm_before_f1.burst_sbm_bp1,
268 268 NB_BINS_COMPRESSED_SM_SBM_F1, NB_BINS_TO_AVERAGE_ASM_SBM_F1,
269 269 ASM_F1_INDICE_START);
270 270 // 2) compute the BP1 set
271 271 BP1_set( compressed_sm_sbm_f1, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp1.data );
272 272 // 3) send the BP1 set
273 273 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
274 274 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
275 275 BP_send( (char *) &packet_sbm_bp1, queue_id_send,
276 276 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 + PACKET_LENGTH_DELTA,
277 277 sid );
278 278 // 4) compute the BP2 set if needed
279 279 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F1) )
280 280 {
281 281 // 1) compute the BP2 set
282 282 BP2_set( compressed_sm_sbm_f1, NB_BINS_COMPRESSED_SM_SBM_F1, packet_norm_bp2.data );
283 283 // 2) send the BP2 set
284 284 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
285 285 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
286 286 BP_send( (char *) &packet_sbm_bp2, queue_id_send,
287 287 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1 + PACKET_LENGTH_DELTA,
288 288 sid );
289 289 }
290 290 }
291 291
292 292 //*****
293 293 //*****
294 294 // NORM
295 295 //*****
296 296 //*****
297 297 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F1)
298 298 {
299 299 // 1) compress the matrix for Basic Parameters calculation
300 300 ASM_compress_reorganize_and_divide( incomingMsg->norm->matrix, compressed_sm_norm_f1,
301 301 nb_sm_before_f1.norm_bp1,
302 NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0,
303 ASM_F0_INDICE_START );
302 NB_BINS_COMPRESSED_SM_F1, NB_BINS_TO_AVERAGE_ASM_F1,
303 ASM_F1_INDICE_START );
304 304 // 2) compute the BP1 set
305 305 BP1_set( compressed_sm_norm_f1, k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp1.data );
306 306 // 3) send the BP1 set
307 set_time( packet_norm_bp1.header.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
308 set_time( packet_norm_bp1.header.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
307 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
308 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
309 309 BP_send( (char *) &packet_norm_bp1, queue_id_send,
310 310 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 + PACKET_LENGTH_DELTA,
311 311 SID_NORM_BP1_F1 );
312 312 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F1)
313 313 {
314 314 // 1) compute the BP2 set
315 315 BP2_set( compressed_sm_norm_f1, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp2.data );
316 316 // 2) send the BP2 set
317 317 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
318 318 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
319 319 BP_send( (char *) &packet_norm_bp2, queue_id_send,
320 320 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 + PACKET_LENGTH_DELTA,
321 321 SID_NORM_BP2_F1 );
322 322 }
323 323 }
324 324
325 325 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F1)
326 326 {
327 327 // 1) reorganize the ASM and divide
328 328 ASM_reorganize_and_divide( incomingMsg->norm->matrix,
329 329 asm_f1_reorganized,
330 330 nb_sm_before_f1.norm_bp1 );
331 331 // 2) convert the float array in a char array
332 332 ASM_convert( asm_f1_reorganized, (char*) current_ring_node_to_send_asm_f1->buffer_address );
333 333 current_ring_node_to_send_asm_f1->coarseTime = incomingMsg->coarseTimeNORM;
334 334 current_ring_node_to_send_asm_f1->fineTime = incomingMsg->fineTimeNORM;
335 335 current_ring_node_to_send_asm_f1->sid = SID_NORM_ASM_F1;
336 336 // 3) send the spectral matrix packets
337 337 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f1, sizeof( ring_node* ) );
338 338 // change asm ring node
339 339 current_ring_node_to_send_asm_f1 = current_ring_node_to_send_asm_f1->next;
340 340 }
341 341
342 342 }
343 343 }
344 344
345 345 //**********
346 346 // FUNCTIONS
347 347
348 348 void reset_nb_sm_f1( unsigned char lfrMode )
349 349 {
350 350 nb_sm_before_f1.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 16;
351 351 nb_sm_before_f1.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 16;
352 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;
353 353 nb_sm_before_f1.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 16;
354 354 nb_sm_before_f1.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 16;
355 355 nb_sm_before_f1.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 16;
356 356 nb_sm_before_f1.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 16;
357 357
358 358 if (lfrMode == LFR_MODE_SBM2)
359 359 {
360 360 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.sbm2_bp1;
361 361 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.sbm2_bp2;
362 362 }
363 363 else if (lfrMode == LFR_MODE_BURST)
364 364 {
365 365 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
366 366 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
367 367 }
368 368 else
369 369 {
370 370 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
371 371 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
372 372 }
373 373 }
374 374
375 375 void init_k_coefficients_f1( void )
376 376 {
377 377 init_k_coefficients( k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1 );
378 378 init_k_coefficients( k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1);
379 379 }
Show file before | Show file after | Hide comments
@@ -1,287 +1,283
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 14 extern ring_node sm_ring_f2[ ];
15 15
16 16 //***
17 17 // F2
18 18 ring_node_asm asm_ring_norm_f2 [ NB_RING_NODES_ASM_NORM_F2 ];
19 ring_node_asm asm_ring_burst_sbm_f2[ NB_RING_NODES_ASM_BURST_SBM_F2 ];
20 19
21 20 ring_node ring_to_send_asm_f2 [ NB_RING_NODES_ASM_F2 ];
22 21 int buffer_asm_f2 [ NB_RING_NODES_ASM_F2 * TOTAL_SIZE_SM ];
23 22
24 23 float asm_f2_reorganized [ TOTAL_SIZE_SM ];
25 char asm_f2_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ];
24 char asm_f2_char [ TOTAL_SIZE_SM * 2 ];
26 25 float compressed_sm_norm_f2[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F2];
27 float compressed_sm_sbm_f2 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F2 ];
28 26
29 27 float k_coeff_intercalib_f2[ NB_BINS_COMPRESSED_SM_F2 * NB_K_COEFF_PER_BIN ]; // 12 * 32 = 384
30 28
31 29 //************
32 30 // RTEMS TASKS
33 31
34 32 //***
35 33 // F2
36 34 rtems_task avf2_task( rtems_task_argument argument )
37 35 {
38 36 rtems_event_set event_out;
39 37 rtems_status_code status;
40 38 rtems_id queue_id_prc2;
41 39 asm_msg msgForMATR;
42 40 ring_node *nodeForAveraging;
43 41 ring_node_asm *current_ring_node_asm_norm_f2;
44 42
45 43 unsigned int nb_norm_bp1;
46 44 unsigned int nb_norm_bp2;
47 45 unsigned int nb_norm_asm;
48 46
49 47 nb_norm_bp1 = 0;
50 48 nb_norm_bp2 = 0;
51 49 nb_norm_asm = 0;
52 50
53 51 reset_nb_sm_f2( ); // reset the sm counters that drive the BP and ASM computations / transmissions
54 52 ASM_generic_init_ring( asm_ring_norm_f2, NB_RING_NODES_ASM_NORM_F2 );
55 53 current_ring_node_asm_norm_f2 = asm_ring_norm_f2;
56 54
57 55 BOOT_PRINTF("in AVF2 ***\n")
58 56
59 57 status = get_message_queue_id_prc2( &queue_id_prc2 );
60 58 if (status != RTEMS_SUCCESSFUL)
61 59 {
62 60 PRINTF1("in AVF2 *** ERR get_message_queue_id_prc2 %d\n", status)
63 61 }
64 62
65 63 while(1){
66 64 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
67 65
68 66 //****************************************
69 67 // initialize the mesage for the MATR task
70 68 msgForMATR.norm = current_ring_node_asm_norm_f2;
71 69 msgForMATR.burst_sbm = NULL;
72 70 msgForMATR.event = 0x00; // this composite event will be sent to the PRC2 task
73 71 //
74 72 //****************************************
75 73
76 74 nodeForAveraging = getRingNodeForAveraging( 2 );
77 75
78 76 // printf(" **0** %x . %x", sm_ring_f2[0].coarseTime, sm_ring_f2[0].fineTime);
79 77 // printf(" **1** %x . %x", sm_ring_f2[1].coarseTime, sm_ring_f2[1].fineTime);
80 78 // printf(" **2** %x . %x", sm_ring_f2[2].coarseTime, sm_ring_f2[2].fineTime);
81 79 // printf(" **3** %x . %x", sm_ring_f2[3].coarseTime, sm_ring_f2[3].fineTime);
82 80 // printf(" **4** %x . %x", sm_ring_f2[4].coarseTime, sm_ring_f2[4].fineTime);
83 81 // printf(" **5** %x . %x", sm_ring_f2[5].coarseTime, sm_ring_f2[5].fineTime);
84 82 // printf(" **6** %x . %x", sm_ring_f2[6].coarseTime, sm_ring_f2[6].fineTime);
85 83 // printf(" **7** %x . %x", sm_ring_f2[7].coarseTime, sm_ring_f2[7].fineTime);
86 84 // printf(" **8** %x . %x", sm_ring_f2[8].coarseTime, sm_ring_f2[8].fineTime);
87 85 // printf(" **9** %x . %x", sm_ring_f2[9].coarseTime, sm_ring_f2[9].fineTime);
88 86 // printf(" **10** %x . %x\n", sm_ring_f2[10].coarseTime, sm_ring_f2[10].fineTime);
89 87
90 88 // compute the average and store it in the averaged_sm_f2 buffer
91 89 SM_average_f2( current_ring_node_asm_norm_f2->matrix,
92 90 nodeForAveraging,
93 91 nb_norm_bp1,
94 92 &msgForMATR );
95 93
96 94 // update nb_average
97 95 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF2;
98 96 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF2;
99 97 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF2;
100 98
101 99 if (nb_norm_bp1 == nb_sm_before_f2.norm_bp1)
102 100 {
103 101 nb_norm_bp1 = 0;
104 102 // set another ring for the ASM storage
105 103 current_ring_node_asm_norm_f2 = current_ring_node_asm_norm_f2->next;
106 104 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
107 105 || (lfrCurrentMode == LFR_MODE_SBM2) )
108 106 {
109 107 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F2;
110 108 }
111 109 }
112 110
113 111 if (nb_norm_bp2 == nb_sm_before_f2.norm_bp2)
114 112 {
115 113 nb_norm_bp2 = 0;
116 114 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
117 115 || (lfrCurrentMode == LFR_MODE_SBM2) )
118 116 {
119 117 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F2;
120 118 }
121 119 }
122 120
123 121 if (nb_norm_asm == nb_sm_before_f2.norm_asm)
124 122 {
125 123 nb_norm_asm = 0;
126 124 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
127 125 || (lfrCurrentMode == LFR_MODE_SBM2) )
128 126 {
129 127 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F2;
130 128 }
131 129 }
132 130
133 131 //*************************
134 132 // send the message to MATR
135 133 if (msgForMATR.event != 0x00)
136 134 {
137 status = rtems_message_queue_send( queue_id_prc2, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC0);
135 status = rtems_message_queue_send( queue_id_prc2, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC2);
138 136 }
139 137
140 138 if (status != RTEMS_SUCCESSFUL) {
141 139 printf("in AVF2 *** Error sending message to MATR, code %d\n", status);
142 140 }
143 141 }
144 142 }
145 143
146 144 rtems_task prc2_task( rtems_task_argument argument )
147 145 {
148 146 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
149 147 size_t size; // size of the incoming TC packet
150 148 asm_msg *incomingMsg;
151 149 //
152 150 rtems_status_code status;
153 rtems_id queue_id;
151 rtems_id queue_id_send;
154 152 rtems_id queue_id_q_p2;
155 153 bp_packet packet_norm_bp1;
156 154 bp_packet packet_norm_bp2;
157 155 ring_node *current_ring_node_to_send_asm_f2;
158 156
159 157 unsigned long long int localTime;
160 158
161 159 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
162 160 init_ring( ring_to_send_asm_f2, NB_RING_NODES_ASM_F2, (volatile int*) buffer_asm_f2, TOTAL_SIZE_SM );
163 161 current_ring_node_to_send_asm_f2 = ring_to_send_asm_f2;
164 162
165 incomingMsg = NULL;
166
167 163 //*************
168 164 // NORM headers
169 165 BP_init_header( &packet_norm_bp1,
170 166 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F2,
171 167 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2, NB_BINS_COMPRESSED_SM_F2 );
172 168 BP_init_header( &packet_norm_bp2,
173 169 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F2,
174 170 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2, NB_BINS_COMPRESSED_SM_F2 );
175 171
176 status = get_message_queue_id_send( &queue_id );
172 status = get_message_queue_id_send( &queue_id_send );
177 173 if (status != RTEMS_SUCCESSFUL)
178 174 {
179 175 PRINTF1("in PRC2 *** ERR get_message_queue_id_send %d\n", status)
180 176 }
181 177 status = get_message_queue_id_prc2( &queue_id_q_p2);
182 178 if (status != RTEMS_SUCCESSFUL)
183 179 {
184 180 PRINTF1("in PRC2 *** ERR get_message_queue_id_prc2 %d\n", status)
185 181 }
186 182
187 183 BOOT_PRINTF("in PRC2 ***\n")
188 184
189 185 while(1){
190 186 status = rtems_message_queue_receive( queue_id_q_p2, incomingData, &size, //************************************
191 187 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
192 188
193 189 incomingMsg = (asm_msg*) incomingData;
194 190
195 191 localTime = getTimeAsUnsignedLongLongInt( );
196 192
197 193 //*****
198 194 //*****
199 195 // NORM
200 196 //*****
201 197 //*****
202 198 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F2)
203 199 {
204 200 // 1) compress the matrix for Basic Parameters calculation
205 201 ASM_compress_reorganize_and_divide( incomingMsg->norm->matrix, compressed_sm_norm_f2,
206 202 nb_sm_before_f2.norm_bp1,
207 203 NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2,
208 204 ASM_F2_INDICE_START );
209 205 // 2) compute the BP1 set
210 206 BP1_set( compressed_sm_norm_f2, k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp1.data );
211 207 // 3) send the BP1 set
212 208 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
213 209 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
214 BP_send( (char *) &packet_norm_bp1, queue_id,
210 BP_send( (char *) &packet_norm_bp1, queue_id_send,
215 211 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2 + PACKET_LENGTH_DELTA,
216 212 SID_NORM_BP1_F2 );
217 213 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F2)
218 214 {
219 215 // 1) compute the BP2 set using the same ASM as the one used for BP1
220 216 BP2_set( compressed_sm_norm_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp2.data );
221 217 // 2) send the BP2 set
222 218 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
223 219 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
224 BP_send( (char *) &packet_norm_bp2, queue_id,
220 BP_send( (char *) &packet_norm_bp2, queue_id_send,
225 221 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2 + PACKET_LENGTH_DELTA,
226 222 SID_NORM_BP2_F2 );
227 223 }
228 224 }
229 225
230 226 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F2)
231 227 {
232 228 // 1) reorganize the ASM and divide
233 229 ASM_reorganize_and_divide( incomingMsg->norm->matrix,
234 230 asm_f2_reorganized,
235 231 nb_sm_before_f2.norm_bp1 );
236 232 // 2) convert the float array in a char array
237 233 ASM_convert( asm_f2_reorganized, (char*) current_ring_node_to_send_asm_f2->buffer_address );
238 234 current_ring_node_to_send_asm_f2->coarseTime = incomingMsg->coarseTimeNORM;
239 235 current_ring_node_to_send_asm_f2->fineTime = incomingMsg->fineTimeNORM;
240 236 current_ring_node_to_send_asm_f2->sid = SID_NORM_ASM_F2;
241 237 // 3) send the spectral matrix packets
242 status = rtems_message_queue_send( queue_id, &current_ring_node_to_send_asm_f2, sizeof( ring_node* ) );
238 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f2, sizeof( ring_node* ) );
243 239 // change asm ring node
244 240 current_ring_node_to_send_asm_f2 = current_ring_node_to_send_asm_f2->next;
245 241 }
246 242
247 243 }
248 244 }
249 245
250 246 //**********
251 247 // FUNCTIONS
252 248
253 249 void reset_nb_sm_f2( void )
254 250 {
255 251 nb_sm_before_f2.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0;
256 252 nb_sm_before_f2.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1;
257 253 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];
258 254 }
259 255
260 256 void SM_average_f2( float *averaged_spec_mat_f2,
261 257 ring_node *ring_node,
262 258 unsigned int nbAverageNormF2,
263 259 asm_msg *msgForMATR )
264 260 {
265 261 float sum;
266 262 unsigned int i;
267 263
268 264 for(i=0; i<TOTAL_SIZE_SM; i++)
269 265 {
270 266 sum = ( (int *) (ring_node->buffer_address) ) [ i ];
271 267 if ( (nbAverageNormF2 == 0) )
272 268 {
273 269 averaged_spec_mat_f2[ i ] = sum;
274 270 msgForMATR->coarseTimeNORM = ring_node->coarseTime;
275 271 msgForMATR->fineTimeNORM = ring_node->fineTime;
276 272 }
277 273 else
278 274 {
279 275 averaged_spec_mat_f2[ i ] = ( averaged_spec_mat_f2[ i ] + sum );
280 276 }
281 277 }
282 278 }
283 279
284 280 void init_k_coefficients_f2( void )
285 281 {
286 282 init_k_coefficients( k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2);
287 283 }
Show file before | Show file after | Hide comments
@@ -1,538 +1,538
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 30 //
31 31 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
32 32 {
33 33 ring_node *node;
34 34
35 35 node = NULL;
36 36 switch ( frequencyChannel ) {
37 37 case 0:
38 38 node = ring_node_for_averaging_sm_f0;
39 39 break;
40 40 case 1:
41 41 node = ring_node_for_averaging_sm_f1;
42 42 break;
43 43 case 2:
44 44 node = ring_node_for_averaging_sm_f2;
45 45 break;
46 46 default:
47 47 break;
48 48 }
49 49
50 50 return node;
51 51 }
52 52
53 53 //***********************************************************
54 54 // Interrupt Service Routine for spectral matrices processing
55 55
56 56 void spectral_matrices_isr_f0( void )
57 57 {
58 58 unsigned char status;
59 59 rtems_status_code status_code;
60 60 ring_node *full_ring_node;
61 61
62 62 status = spectral_matrix_regs->status & 0x03; // [0011] get the status_ready_matrix_f0_x bits
63 63
64 64 switch(status)
65 65 {
66 66 case 0:
67 67 break;
68 68 case 3:
69 69 // UNEXPECTED VALUE
70 70 spectral_matrix_regs->status = 0x03; // [0011]
71 71 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
72 72 break;
73 73 case 1:
74 74 full_ring_node = current_ring_node_sm_f0->previous;
75 full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
76 full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time;
75 77 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
76 78 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
77 79 // if there are enough ring nodes ready, wake up an AVFx task
78 80 nb_sm_f0 = nb_sm_f0 + 1;
79 81 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
80 82 {
81 83 ring_node_for_averaging_sm_f0 = full_ring_node;
82 ring_node_for_averaging_sm_f0->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
83 ring_node_for_averaging_sm_f0->fineTime = spectral_matrix_regs->f0_0_fine_time;
84 84 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
85 85 {
86 86 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
87 87 }
88 88 nb_sm_f0 = 0;
89 89 }
90 90 spectral_matrix_regs->status = 0x01; // [0000 0001]
91 91 break;
92 92 case 2:
93 93 full_ring_node = current_ring_node_sm_f0->previous;
94 full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
95 full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time;
94 96 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
95 97 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
96 98 // if there are enough ring nodes ready, wake up an AVFx task
97 99 nb_sm_f0 = nb_sm_f0 + 1;
98 100 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
99 101 {
100 102 ring_node_for_averaging_sm_f0 = full_ring_node;
101 ring_node_for_averaging_sm_f0->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
102 ring_node_for_averaging_sm_f0->fineTime = spectral_matrix_regs->f0_1_fine_time;
103 103 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
104 104 {
105 105 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
106 106 }
107 107 nb_sm_f0 = 0;
108 108 }
109 109 spectral_matrix_regs->status = 0x02; // [0000 0010]
110 110 break;
111 111 }
112 112 }
113 113
114 114 void spectral_matrices_isr_f1( void )
115 115 {
116 116 rtems_status_code status_code;
117 117 unsigned char status;
118 118 ring_node *full_ring_node;
119 119
120 120 status = (spectral_matrix_regs->status & 0x0c) >> 2; // [1100] get the status_ready_matrix_f0_x bits
121 121
122 122 switch(status)
123 123 {
124 124 case 0:
125 125 break;
126 126 case 3:
127 127 // UNEXPECTED VALUE
128 128 spectral_matrix_regs->status = 0xc0; // [1100]
129 129 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
130 130 break;
131 131 case 1:
132 132 full_ring_node = current_ring_node_sm_f1->previous;
133 full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
134 full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
133 135 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
134 136 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
135 137 // if there are enough ring nodes ready, wake up an AVFx task
136 138 nb_sm_f1 = nb_sm_f1 + 1;
137 139 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
138 140 {
139 141 ring_node_for_averaging_sm_f1 = full_ring_node;
140 ring_node_for_averaging_sm_f1->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
141 ring_node_for_averaging_sm_f1->fineTime = spectral_matrix_regs->f1_0_fine_time;
142 142 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
143 143 {
144 144 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
145 145 }
146 146 nb_sm_f1 = 0;
147 147 }
148 148 spectral_matrix_regs->status = 0x04; // [0000 0100]
149 149 break;
150 150 case 2:
151 151 full_ring_node = current_ring_node_sm_f1->previous;
152 full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
153 full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
152 154 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
153 155 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
154 156 // if there are enough ring nodes ready, wake up an AVFx task
155 157 nb_sm_f1 = nb_sm_f1 + 1;
156 158 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
157 159 {
158 160 ring_node_for_averaging_sm_f1 = full_ring_node;
159 ring_node_for_averaging_sm_f1->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
160 ring_node_for_averaging_sm_f1->fineTime = spectral_matrix_regs->f1_1_fine_time;
161 161 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
162 162 {
163 163 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
164 164 }
165 165 nb_sm_f1 = 0;
166 166 }
167 167 spectral_matrix_regs->status = 0x08; // [1000 0000]
168 168 break;
169 169 }
170 170 }
171 171
172 172 void spectral_matrices_isr_f2( void )
173 173 {
174 174 unsigned char status;
175 175 rtems_status_code status_code;
176 176
177 177 status = (spectral_matrix_regs->status & 0x30) >> 4; // [0011 0000] get the status_ready_matrix_f0_x bits
178 178
179 179 switch(status)
180 180 {
181 181 case 0:
182 182 break;
183 183 case 3:
184 184 // UNEXPECTED VALUE
185 185 spectral_matrix_regs->status = 0x30; // [0011 0000]
186 186 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
187 187 break;
188 188 case 1:
189 189 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
190 190 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
191 191 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
192 192 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
193 193 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
194 194 spectral_matrix_regs->status = 0x10; // [0001 0000]
195 195 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
196 196 {
197 197 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
198 198 }
199 199 break;
200 200 case 2:
201 201 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
202 202 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
203 203 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
204 204 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
205 205 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
206 206 spectral_matrix_regs->status = 0x20; // [0010 0000]
207 207 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
208 208 {
209 209 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
210 210 }
211 211 break;
212 212 }
213 213 }
214 214
215 215 void spectral_matrix_isr_error_handler( void )
216 216 {
217 217 rtems_status_code status_code;
218 218
219 219 if (spectral_matrix_regs->status & 0x7c0) // [0111 1100 0000]
220 220 {
221 221 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
222 222 }
223 223
224 224 spectral_matrix_regs->status = spectral_matrix_regs->status & 0x7c0;
225 225 }
226 226
227 227 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
228 228 {
229 229 // STATUS REGISTER
230 230 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
231 231 // 10 9 8
232 232 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
233 233 // 7 6 5 4 3 2 1 0
234 234
235 235 spectral_matrices_isr_f0();
236 236
237 237 spectral_matrices_isr_f1();
238 238
239 239 spectral_matrices_isr_f2();
240 240
241 241 spectral_matrix_isr_error_handler();
242 242 }
243 243
244 244 rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector )
245 245 {
246 246 rtems_status_code status_code;
247 247
248 248 //***
249 249 // F0
250 250 nb_sm_f0 = nb_sm_f0 + 1;
251 251 if (nb_sm_f0 == NB_SM_BEFORE_AVF0 )
252 252 {
253 253 ring_node_for_averaging_sm_f0 = current_ring_node_sm_f0;
254 254 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
255 255 {
256 256 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
257 257 }
258 258 nb_sm_f0 = 0;
259 259 }
260 260
261 261 //***
262 262 // F1
263 263 nb_sm_f0_aux_f1 = nb_sm_f0_aux_f1 + 1;
264 264 if (nb_sm_f0_aux_f1 == 6)
265 265 {
266 266 nb_sm_f0_aux_f1 = 0;
267 267 nb_sm_f1 = nb_sm_f1 + 1;
268 268 }
269 269 if (nb_sm_f1 == NB_SM_BEFORE_AVF1 )
270 270 {
271 271 ring_node_for_averaging_sm_f1 = current_ring_node_sm_f1;
272 272 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
273 273 {
274 274 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
275 275 }
276 276 nb_sm_f1 = 0;
277 277 }
278 278
279 279 //***
280 280 // F2
281 281 nb_sm_f0_aux_f2 = nb_sm_f0_aux_f2 + 1;
282 282 if (nb_sm_f0_aux_f2 == 96)
283 283 {
284 284 nb_sm_f0_aux_f2 = 0;
285 285 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2;
286 286 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
287 287 {
288 288 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
289 289 }
290 290 }
291 291 }
292 292
293 293 //******************
294 294 // Spectral Matrices
295 295
296 296 void reset_nb_sm( void )
297 297 {
298 298 nb_sm_f0 = 0;
299 299 nb_sm_f0_aux_f1 = 0;
300 300 nb_sm_f0_aux_f2 = 0;
301 301
302 302 nb_sm_f1 = 0;
303 303 }
304 304
305 305 void SM_init_rings( void )
306 306 {
307 307 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
308 308 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
309 309 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
310 310
311 311 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
312 312 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
313 313 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
314 314 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
315 315 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
316 316 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
317 317 }
318 318
319 319 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
320 320 {
321 321 unsigned char i;
322 322
323 323 ring[ nbNodes - 1 ].next
324 324 = (ring_node_asm*) &ring[ 0 ];
325 325
326 326 for(i=0; i<nbNodes-1; i++)
327 327 {
328 328 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
329 329 }
330 330 }
331 331
332 332 void SM_reset_current_ring_nodes( void )
333 333 {
334 334 current_ring_node_sm_f0 = sm_ring_f0[0].next;
335 335 current_ring_node_sm_f1 = sm_ring_f1[0].next;
336 336 current_ring_node_sm_f2 = sm_ring_f2[0].next;
337 337
338 338 ring_node_for_averaging_sm_f0 = NULL;
339 339 ring_node_for_averaging_sm_f1 = NULL;
340 340 ring_node_for_averaging_sm_f2 = NULL;
341 341 }
342 342
343 343 //*****************
344 344 // Basic Parameters
345 345
346 void BP_init_header( bp_packet *header,
346 void BP_init_header( bp_packet *packet,
347 347 unsigned int apid, unsigned char sid,
348 348 unsigned int packetLength, unsigned char blkNr )
349 349 {
350 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
351 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
352 header->reserved = 0x00;
353 header->userApplication = CCSDS_USER_APP;
354 header->packetID[0] = (unsigned char) (apid >> 8);
355 header->packetID[1] = (unsigned char) (apid);
356 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
357 header->packetSequenceControl[1] = 0x00;
358 header->packetLength[0] = (unsigned char) (packetLength >> 8);
359 header->packetLength[1] = (unsigned char) (packetLength);
350 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
351 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
352 packet->reserved = 0x00;
353 packet->userApplication = CCSDS_USER_APP;
354 packet->packetID[0] = (unsigned char) (apid >> 8);
355 packet->packetID[1] = (unsigned char) (apid);
356 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
357 packet->packetSequenceControl[1] = 0x00;
358 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
359 packet->packetLength[1] = (unsigned char) (packetLength);
360 360 // DATA FIELD HEADER
361 header->spare1_pusVersion_spare2 = 0x10;
362 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
363 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
364 header->destinationID = TM_DESTINATION_ID_GROUND;
365 header->time[0] = 0x00;
366 header->time[1] = 0x00;
367 header->time[2] = 0x00;
368 header->time[3] = 0x00;
369 header->time[4] = 0x00;
370 header->time[5] = 0x00;
361 packet->spare1_pusVersion_spare2 = 0x10;
362 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
363 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
364 packet->destinationID = TM_DESTINATION_ID_GROUND;
365 packet->time[0] = 0x00;
366 packet->time[1] = 0x00;
367 packet->time[2] = 0x00;
368 packet->time[3] = 0x00;
369 packet->time[4] = 0x00;
370 packet->time[5] = 0x00;
371 371 // AUXILIARY DATA HEADER
372 header->sid = sid;
373 header->biaStatusInfo = 0x00;
374 header->acquisitionTime[0] = 0x00;
375 header->acquisitionTime[1] = 0x00;
376 header->acquisitionTime[2] = 0x00;
377 header->acquisitionTime[3] = 0x00;
378 header->acquisitionTime[4] = 0x00;
379 header->acquisitionTime[5] = 0x00;
380 header->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
381 header->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
372 packet->sid = sid;
373 packet->biaStatusInfo = 0x00;
374 packet->acquisitionTime[0] = 0x00;
375 packet->acquisitionTime[1] = 0x00;
376 packet->acquisitionTime[2] = 0x00;
377 packet->acquisitionTime[3] = 0x00;
378 packet->acquisitionTime[4] = 0x00;
379 packet->acquisitionTime[5] = 0x00;
380 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
381 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
382 382 }
383 383
384 void BP_init_header_with_spare(Header_TM_LFR_SCIENCE_BP_with_spare_t *header,
384 void BP_init_header_with_spare( bp_packet_with_spare *packet,
385 385 unsigned int apid, unsigned char sid,
386 386 unsigned int packetLength , unsigned char blkNr)
387 387 {
388 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
389 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
390 header->reserved = 0x00;
391 header->userApplication = CCSDS_USER_APP;
392 header->packetID[0] = (unsigned char) (apid >> 8);
393 header->packetID[1] = (unsigned char) (apid);
394 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
395 header->packetSequenceControl[1] = 0x00;
396 header->packetLength[0] = (unsigned char) (packetLength >> 8);
397 header->packetLength[1] = (unsigned char) (packetLength);
388 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
389 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
390 packet->reserved = 0x00;
391 packet->userApplication = CCSDS_USER_APP;
392 packet->packetID[0] = (unsigned char) (apid >> 8);
393 packet->packetID[1] = (unsigned char) (apid);
394 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
395 packet->packetSequenceControl[1] = 0x00;
396 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
397 packet->packetLength[1] = (unsigned char) (packetLength);
398 398 // DATA FIELD HEADER
399 header->spare1_pusVersion_spare2 = 0x10;
400 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
401 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
402 header->destinationID = TM_DESTINATION_ID_GROUND;
399 packet->spare1_pusVersion_spare2 = 0x10;
400 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
401 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
402 packet->destinationID = TM_DESTINATION_ID_GROUND;
403 403 // AUXILIARY DATA HEADER
404 header->sid = sid;
405 header->biaStatusInfo = 0x00;
406 header->time[0] = 0x00;
407 header->time[0] = 0x00;
408 header->time[0] = 0x00;
409 header->time[0] = 0x00;
410 header->time[0] = 0x00;
411 header->time[0] = 0x00;
412 header->source_data_spare = 0x00;
413 header->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
414 header->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
404 packet->sid = sid;
405 packet->biaStatusInfo = 0x00;
406 packet->time[0] = 0x00;
407 packet->time[0] = 0x00;
408 packet->time[0] = 0x00;
409 packet->time[0] = 0x00;
410 packet->time[0] = 0x00;
411 packet->time[0] = 0x00;
412 packet->source_data_spare = 0x00;
413 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
414 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
415 415 }
416 416
417 417 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
418 418 {
419 419 rtems_status_code status;
420 420
421 421 // SET THE SEQUENCE_CNT PARAMETER
422 422 increment_seq_counter_source_id( (unsigned char*) &data[ PACKET_POS_SEQUENCE_CNT ], sid );
423 423 // SEND PACKET
424 424 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
425 425 if (status != RTEMS_SUCCESSFUL)
426 426 {
427 427 printf("ERR *** in BP_send *** ERR %d\n", (int) status);
428 428 }
429 429 }
430 430
431 431 //******************
432 432 // general functions
433 433
434 434 void reset_sm_status( void )
435 435 {
436 436 // error
437 437 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
438 438 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
439 439 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
440 440 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
441 441
442 442 spectral_matrix_regs->status = 0x7ff; // [0111 1111 1111]
443 443 }
444 444
445 445 void reset_spectral_matrix_regs( void )
446 446 {
447 447 /** This function resets the spectral matrices module registers.
448 448 *
449 449 * The registers affected by this function are located at the following offset addresses:
450 450 *
451 451 * - 0x00 config
452 452 * - 0x04 status
453 453 * - 0x08 matrixF0_Address0
454 454 * - 0x10 matrixFO_Address1
455 455 * - 0x14 matrixF1_Address
456 456 * - 0x18 matrixF2_Address
457 457 *
458 458 */
459 459
460 460 set_sm_irq_onError( 0 );
461 461
462 462 set_sm_irq_onNewMatrix( 0 );
463 463
464 464 reset_sm_status();
465 465
466 466 // F1
467 467 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
468 468 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
469 469 // F2
470 470 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
471 471 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
472 472 // F3
473 473 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
474 474 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
475 475
476 476 spectral_matrix_regs->matrix_length = 0xc8; // 25 * 128 / 16 = 200 = 0xc8
477 477 }
478 478
479 479 void set_time( unsigned char *time, unsigned char * timeInBuffer )
480 480 {
481 481 time[0] = timeInBuffer[0];
482 482 time[1] = timeInBuffer[1];
483 483 time[2] = timeInBuffer[2];
484 484 time[3] = timeInBuffer[3];
485 485 time[4] = timeInBuffer[6];
486 486 time[5] = timeInBuffer[7];
487 487 }
488 488
489 489 unsigned long long int get_acquisition_time( unsigned char *timePtr )
490 490 {
491 491 unsigned long long int acquisitionTimeAslong;
492 492 acquisitionTimeAslong = 0x00;
493 493 acquisitionTimeAslong = ( (unsigned long long int) (timePtr[0] & 0x7f) << 40 ) // [0111 1111] mask the synchronization bit
494 494 + ( (unsigned long long int) timePtr[1] << 32 )
495 495 + ( (unsigned long long int) timePtr[2] << 24 )
496 496 + ( (unsigned long long int) timePtr[3] << 16 )
497 497 + ( (unsigned long long int) timePtr[6] << 8 )
498 498 + ( (unsigned long long int) timePtr[7] );
499 499 return acquisitionTimeAslong;
500 500 }
501 501
502 502 unsigned char getSID( rtems_event_set event )
503 503 {
504 504 unsigned char sid;
505 505
506 506 rtems_event_set eventSetBURST;
507 507 rtems_event_set eventSetSBM;
508 508
509 509 //******
510 510 // BURST
511 511 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
512 512 | RTEMS_EVENT_BURST_BP1_F1
513 513 | RTEMS_EVENT_BURST_BP2_F0
514 514 | RTEMS_EVENT_BURST_BP2_F1;
515 515
516 516 //****
517 517 // SBM
518 518 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
519 519 | RTEMS_EVENT_SBM_BP1_F1
520 520 | RTEMS_EVENT_SBM_BP2_F0
521 521 | RTEMS_EVENT_SBM_BP2_F1;
522 522
523 523 if (event & eventSetBURST)
524 524 {
525 525 sid = SID_BURST_BP1_F0;
526 526 }
527 527 else if (event & eventSetSBM)
528 528 {
529 529 sid = SID_SBM1_BP1_F0;
530 530 }
531 531 else
532 532 {
533 533 sid = 0;
534 534 }
535 535
536 536 return sid;
537 537 }
538 538
Show file before | Show file after | Hide comments
@@ -1,1402 +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 char wf_cont_f3_light[ (NB_SAMPLES_PER_SNAPSHOT) * NB_BYTES_CWF3_LIGHT_BLK ];
32 33
33 34 bool extractSWF = false;
34 35 bool swf_f0_ready = false;
35 36 bool swf_f1_ready = false;
36 37 bool swf_f2_ready = false;
37 38
38 39 int wf_snap_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ];
39 40 ring_node ring_node_wf_snap_extracted;
40 41
41 42 //*********************
42 43 // Interrupt SubRoutine
43 44
44 45 ring_node * getRingNodeToSendCWF( unsigned char frequencyChannel)
45 46 {
46 47 ring_node *node;
47 48
48 49 node = NULL;
49 50 switch ( frequencyChannel ) {
50 51 case 1:
51 52 node = ring_node_to_send_cwf_f1;
52 53 break;
53 54 case 2:
54 55 node = ring_node_to_send_cwf_f2;
55 56 break;
56 57 case 3:
57 58 node = ring_node_to_send_cwf_f3;
58 59 break;
59 60 default:
60 61 break;
61 62 }
62 63
63 64 return node;
64 65 }
65 66
66 67 ring_node * getRingNodeToSendSWF( unsigned char frequencyChannel)
67 68 {
68 69 ring_node *node;
69 70
70 71 node = NULL;
71 72 switch ( frequencyChannel ) {
72 73 case 0:
73 74 node = ring_node_to_send_swf_f0;
74 75 break;
75 76 case 1:
76 77 node = ring_node_to_send_swf_f1;
77 78 break;
78 79 case 2:
79 80 node = ring_node_to_send_swf_f2;
80 81 break;
81 82 default:
82 83 break;
83 84 }
84 85
85 86 return node;
86 87 }
87 88
88 89 void reset_extractSWF( void )
89 90 {
90 91 extractSWF = false;
91 92 swf_f0_ready = false;
92 93 swf_f1_ready = false;
93 94 swf_f2_ready = false;
94 95 }
95 96
96 97 inline void waveforms_isr_f3( void )
97 98 {
98 99 rtems_status_code spare_status;
99 100
100 101 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
101 102 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
102 103 { // in modes other than STANDBY and BURST, send the CWF_F3 data
103 104 //***
104 105 // F3
105 106 if ( (waveform_picker_regs->status & 0xc0) != 0x00 ) { // [1100 0000] check the f3 full bits
106 107 ring_node_to_send_cwf_f3 = current_ring_node_f3->previous;
107 108 current_ring_node_f3 = current_ring_node_f3->next;
108 109 if ((waveform_picker_regs->status & 0x40) == 0x40){ // [0100 0000] f3 buffer 0 is full
109 110 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_0_coarse_time;
110 111 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_0_fine_time;
111 112 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->buffer_address;
112 113 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008840; // [1000 1000 0100 0000]
113 114 }
114 115 else if ((waveform_picker_regs->status & 0x80) == 0x80){ // [1000 0000] f3 buffer 1 is full
115 116 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_1_coarse_time;
116 117 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_1_fine_time;
117 118 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address;
118 119 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008880; // [1000 1000 1000 0000]
119 120 }
120 121 if (rtems_event_send( Task_id[TASKID_CWF3], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
121 122 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
122 123 }
123 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_2);
124 124 }
125 125 }
126 126 }
127 127
128 128 inline void waveforms_isr_normal( void )
129 129 {
130 130 rtems_status_code status;
131 131
132 132 if ( ( (waveform_picker_regs->status & 0x30) != 0x00 ) // [0011 0000] check the f2 full bits
133 133 && ( (waveform_picker_regs->status & 0x0c) != 0x00 ) // [0000 1100] check the f1 full bits
134 134 && ( (waveform_picker_regs->status & 0x03) != 0x00 )) // [0000 0011] check the f0 full bits
135 135 {
136 136 //***
137 137 // F0
138 138 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
139 139 current_ring_node_f0 = current_ring_node_f0->next;
140 140 if ( (waveform_picker_regs->status & 0x01) == 0x01)
141 141 {
142 142
143 143 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
144 144 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
145 145 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
146 146 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
147 147 }
148 148 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
149 149 {
150 150 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
151 151 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
152 152 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
153 153 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
154 154 }
155 155
156 156 //***
157 157 // F1
158 158 ring_node_to_send_swf_f1 = current_ring_node_f1->previous;
159 159 current_ring_node_f1 = current_ring_node_f1->next;
160 160 if ( (waveform_picker_regs->status & 0x04) == 0x04)
161 161 {
162 162 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
163 163 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
164 164 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
165 165 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
166 166 }
167 167 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
168 168 {
169 169 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
170 170 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
171 171 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
172 172 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
173 173 }
174 174
175 175 //***
176 176 // F2
177 177 ring_node_to_send_swf_f2 = current_ring_node_f2->previous;
178 178 current_ring_node_f2 = current_ring_node_f2->next;
179 179 if ( (waveform_picker_regs->status & 0x10) == 0x10)
180 180 {
181 181 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
182 182 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
183 183 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
184 184 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
185 185 }
186 186 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
187 187 {
188 188 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
189 189 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
190 190 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
191 191 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
192 192 }
193 193 //
194 194 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_NORMAL );
195 195 if ( status != RTEMS_SUCCESSFUL)
196 196 {
197 197 status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
198 198 }
199 199 }
200 200 }
201 201
202 202 inline void waveforms_isr_burst( void )
203 203 {
204 204 unsigned char status;
205 205 rtems_status_code spare_status;
206 206
207 status = (waveform_picker_regs->status & 0x30) >> 4; // [0011 0000] get the status_ready_matrix_f0_x bits
207 status = (waveform_picker_regs->status & 0x30) >> 4; // [0011 0000] get the status bits for f2
208
208 209
209 210 switch(status)
210 211 {
211 212 case 1:
212 213 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
214 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
213 215 current_ring_node_f2 = current_ring_node_f2->next;
214 216 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
215 217 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
216 218 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
217 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
218 219 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
219 220 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
220 221 }
222 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
221 223 break;
222 224 case 2:
223 225 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
226 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
224 227 current_ring_node_f2 = current_ring_node_f2->next;
225 228 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
226 229 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
227 230 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
228 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
229 231 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
230 232 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
231 233 }
234 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
232 235 break;
233 236 default:
234 237 break;
235 238 }
236 239 }
237 240
238 241 inline void waveforms_isr_sbm1( void )
239 242 {
240 243 rtems_status_code status;
241 244
242 245 //***
243 246 // F1
244 247 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bits
245 248 // (1) change the receiving buffer for the waveform picker
246 249 ring_node_to_send_cwf_f1 = current_ring_node_f1->previous;
247 250 current_ring_node_f1 = current_ring_node_f1->next;
248 251 if ( (waveform_picker_regs->status & 0x04) == 0x04)
249 252 {
250 253 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
251 254 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
252 255 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
253 256 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
254 257 }
255 258 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
256 259 {
257 260 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
258 261 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
259 262 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
260 263 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
261 264 }
262 265 // (2) send an event for the the CWF1 task for transmission (and snapshot extraction if needed)
263 266 status = rtems_event_send( Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_SBM1 );
264 267 }
265 268
266 269 //***
267 270 // F0
268 271 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) { // [0000 0011] check the f0 full bits
269 272 swf_f0_ready = true;
270 273 // change f0 buffer
271 274 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
272 275 current_ring_node_f0 = current_ring_node_f0->next;
273 276 if ( (waveform_picker_regs->status & 0x01) == 0x01)
274 277 {
275 278
276 279 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
277 280 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
278 281 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
279 282 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
280 283 }
281 284 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
282 285 {
283 286 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
284 287 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
285 288 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
286 289 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
287 290 }
288 291 }
289 292
290 293 //***
291 294 // F2
292 295 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bits
293 296 swf_f2_ready = true;
294 297 // change f2 buffer
295 298 ring_node_to_send_swf_f2 = current_ring_node_f2->previous;
296 299 current_ring_node_f2 = current_ring_node_f2->next;
297 300 if ( (waveform_picker_regs->status & 0x10) == 0x10)
298 301 {
299 302 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
300 303 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
301 304 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
302 305 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
303 306 }
304 307 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
305 308 {
306 309 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
307 310 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
308 311 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
309 312 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
310 313 }
311 314 }
312 315 }
313 316
314 317 inline void waveforms_isr_sbm2( void )
315 318 {
316 319 rtems_status_code status;
317 320
318 321 //***
319 322 // F2
320 323 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bit
321 324 // (1) change the receiving buffer for the waveform picker
322 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
323 current_ring_node_f2 = current_ring_node_f2->next;
325 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
326 ring_node_to_send_cwf_f2->sid = SID_SBM2_CWF_F2;
327 current_ring_node_f2 = current_ring_node_f2->next;
324 328 if ( (waveform_picker_regs->status & 0x10) == 0x10)
325 329 {
326 330 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
327 331 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
328 332 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
329 333 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
330 334 }
331 335 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
332 336 {
333 337 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
334 338 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
335 339 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
336 340 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
337 341 }
338 342 // (2) send an event for the waveforms transmission
339 343 status = rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_SBM2 );
340 344 }
341 345
342 346 //***
343 347 // F0
344 348 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) { // [0000 0011] check the f0 full bit
345 349 swf_f0_ready = true;
346 350 // change f0 buffer
347 351 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
348 352 current_ring_node_f0 = current_ring_node_f0->next;
349 353 if ( (waveform_picker_regs->status & 0x01) == 0x01)
350 354 {
351 355
352 356 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
353 357 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
354 358 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
355 359 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
356 360 }
357 361 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
358 362 {
359 363 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
360 364 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
361 365 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
362 366 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
363 367 }
364 368 }
365 369
366 370 //***
367 371 // F1
368 372 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bit
369 373 swf_f1_ready = true;
370 374 ring_node_to_send_swf_f1 = current_ring_node_f1->previous;
371 375 current_ring_node_f1 = current_ring_node_f1->next;
372 376 if ( (waveform_picker_regs->status & 0x04) == 0x04)
373 377 {
374 378 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
375 379 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
376 380 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
377 381 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
378 382 }
379 383 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
380 384 {
381 385 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
382 386 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
383 387 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
384 388 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
385 389 }
386 390 }
387 391 }
388 392
389 393 rtems_isr waveforms_isr( rtems_vector_number vector )
390 394 {
391 395 /** This is the interrupt sub routine called by the waveform picker core.
392 396 *
393 397 * This ISR launch different actions depending mainly on two pieces of information:
394 398 * 1. the values read in the registers of the waveform picker.
395 399 * 2. the current LFR mode.
396 400 *
397 401 */
398 402
399 403 // STATUS
400 404 // new error error buffer full
401 405 // 15 14 13 12 11 10 9 8
402 406 // f3 f2 f1 f0 f3 f2 f1 f0
403 407 //
404 408 // ready buffer
405 409 // 7 6 5 4 3 2 1 0
406 410 // f3_1 f3_0 f2_1 f2_0 f1_1 f1_0 f0_1 f0_0
407 411
408 412 rtems_status_code spare_status;
409 413
410 414 waveforms_isr_f3();
411 415
412 416 if ( (waveform_picker_regs->status & 0xff00) != 0x00) // [1111 1111 0000 0000] check the error bits
413 417 {
414 418 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_10 );
415 419 }
416 420
417 421 switch(lfrCurrentMode)
418 422 {
419 423 //********
420 424 // STANDBY
421 425 case(LFR_MODE_STANDBY):
422 426 break;
423 427
424 428 //******
425 429 // NORMAL
426 430 case(LFR_MODE_NORMAL):
427 431 waveforms_isr_normal();
428 432 break;
429 433
430 434 //******
431 435 // BURST
432 436 case(LFR_MODE_BURST):
433 437 waveforms_isr_burst();
434 438 break;
435 439
436 440 //*****
437 441 // SBM1
438 442 case(LFR_MODE_SBM1):
439 443 waveforms_isr_sbm1();
440 444 break;
441 445
442 446 //*****
443 447 // SBM2
444 448 case(LFR_MODE_SBM2):
445 449 waveforms_isr_sbm2();
446 450 break;
447 451
448 452 //********
449 453 // DEFAULT
450 454 default:
451 455 break;
452 456 }
453 457 }
454 458
455 459 //************
456 460 // RTEMS TASKS
457 461
458 462 rtems_task wfrm_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
459 463 {
460 464 /** This RTEMS task is dedicated to the transmission of snapshots of the NORMAL mode.
461 465 *
462 466 * @param unused is the starting argument of the RTEMS task
463 467 *
464 468 * The following data packets are sent by this task:
465 469 * - TM_LFR_SCIENCE_NORMAL_SWF_F0
466 470 * - TM_LFR_SCIENCE_NORMAL_SWF_F1
467 471 * - TM_LFR_SCIENCE_NORMAL_SWF_F2
468 472 *
469 473 */
470 474
471 475 rtems_event_set event_out;
472 476 rtems_id queue_id;
473 477 rtems_status_code status;
474 478 bool resynchronisationEngaged;
475 479 ring_node *ring_node_wf_snap_extracted_ptr;
476 480
477 481 ring_node_wf_snap_extracted_ptr = (ring_node *) &ring_node_wf_snap_extracted;
478 482
479 483 resynchronisationEngaged = false;
480 484
481 485 status = get_message_queue_id_send( &queue_id );
482 486 if (status != RTEMS_SUCCESSFUL)
483 487 {
484 488 PRINTF1("in WFRM *** ERR get_message_queue_id_send %d\n", status)
485 489 }
486 490
487 491 BOOT_PRINTF("in WFRM ***\n")
488 492
489 493 while(1){
490 494 // wait for an RTEMS_EVENT
491 495 rtems_event_receive(RTEMS_EVENT_MODE_NORMAL | RTEMS_EVENT_MODE_SBM1
492 496 | RTEMS_EVENT_MODE_SBM2 | RTEMS_EVENT_MODE_SBM2_WFRM,
493 497 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
494 498 if(resynchronisationEngaged == false)
495 499 { // engage resynchronisation
496 500 snapshot_resynchronization( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
497 501 resynchronisationEngaged = true;
498 502 }
499 503 else
500 504 { // reset delta_snapshot to the nominal value
501 505 PRINTF("no resynchronisation, reset delta_snapshot to the nominal value\n")
502 506 set_wfp_delta_snapshot();
503 507 resynchronisationEngaged = false;
504 508 }
505 509 //
506 510
507 511 if (event_out == RTEMS_EVENT_MODE_NORMAL)
508 512 {
509 513 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_NORMAL\n")
510 514 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
511 515 ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1;
512 516 ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2;
513 517 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
514 518 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) );
515 519 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) );
516 520 }
517 521 if (event_out == RTEMS_EVENT_MODE_SBM1)
518 522 {
519 523 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM1\n")
520 524 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
521 525 ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F1;
522 526 ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2;
523 527 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
524 528 status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) );
525 529 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) );
526 530 }
527 531 if (event_out == RTEMS_EVENT_MODE_SBM2)
528 532 {
529 533 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM2\n")
530 534 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
531 535 ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1;
532 536 ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F2;
533 537 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
534 538 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) );
535 539 status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) );
536 540 }
537 541 }
538 542 }
539 543
540 544 rtems_task cwf3_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
541 545 {
542 546 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f3.
543 547 *
544 548 * @param unused is the starting argument of the RTEMS task
545 549 *
546 550 * The following data packet is sent by this task:
547 551 * - TM_LFR_SCIENCE_NORMAL_CWF_F3
548 552 *
549 553 */
550 554
551 555 rtems_event_set event_out;
552 556 rtems_id queue_id;
553 557 rtems_status_code status;
554 558 ring_node ring_node_cwf3_light;
555 559
556 560 status = get_message_queue_id_send( &queue_id );
557 561 if (status != RTEMS_SUCCESSFUL)
558 562 {
559 563 PRINTF1("in CWF3 *** ERR get_message_queue_id_send %d\n", status)
560 564 }
561 565
562 566 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
563 567
564 568 // init the ring_node_cwf3_light structure
565 569 ring_node_cwf3_light.buffer_address = (int) wf_cont_f3_light;
566 570 ring_node_cwf3_light.coarseTime = 0x00;
567 571 ring_node_cwf3_light.fineTime = 0x00;
568 572 ring_node_cwf3_light.next = NULL;
569 573 ring_node_cwf3_light.previous = NULL;
570 574 ring_node_cwf3_light.sid = SID_NORM_CWF_F3;
571 575 ring_node_cwf3_light.status = 0x00;
572 576
573 577 BOOT_PRINTF("in CWF3 ***\n")
574 578
575 579 while(1){
576 580 // wait for an RTEMS_EVENT
577 581 rtems_event_receive( RTEMS_EVENT_0,
578 582 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
579 583 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
580 584 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode==LFR_MODE_SBM2) )
581 585 {
582 586 if ( (parameter_dump_packet.sy_lfr_n_cwf_long_f3 & 0x01) == 0x01)
583 587 {
584 588 PRINTF("send CWF_LONG_F3\n")
585 589 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
586 590 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf_f3, sizeof( ring_node* ) );
587 591 }
588 592 else
589 593 {
590 594 PRINTF("send CWF_F3 (light)\n")
591 595 send_waveform_CWF3_light( ring_node_to_send_cwf_f3, &ring_node_cwf3_light, queue_id );
592 596 }
593 597
594 598 }
595 599 else
596 600 {
597 601 PRINTF1("in CWF3 *** lfrCurrentMode is %d, no data will be sent\n", lfrCurrentMode)
598 602 }
599 603 }
600 604 }
601 605
602 606 rtems_task cwf2_task(rtems_task_argument argument) // ONLY USED IN BURST AND SBM2
603 607 {
604 608 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f2.
605 609 *
606 610 * @param unused is the starting argument of the RTEMS task
607 611 *
608 612 * The following data packet is sent by this function:
609 613 * - TM_LFR_SCIENCE_BURST_CWF_F2
610 614 * - TM_LFR_SCIENCE_SBM2_CWF_F2
611 615 *
612 616 */
613 617
614 618 rtems_event_set event_out;
615 619 rtems_id queue_id;
616 620 rtems_status_code status;
617 621 ring_node *ring_node_to_send;
622 unsigned long long int acquisitionTimeF0_asLong;
623
624 acquisitionTimeF0_asLong = 0x00;
618 625
619 626 status = get_message_queue_id_send( &queue_id );
620 627 if (status != RTEMS_SUCCESSFUL)
621 628 {
622 629 PRINTF1("in CWF2 *** ERR get_message_queue_id_send %d\n", status)
623 630 }
624 631
625 632 BOOT_PRINTF("in CWF2 ***\n")
626 633
627 634 while(1){
628 635 // wait for an RTEMS_EVENT
629 636 rtems_event_receive( RTEMS_EVENT_MODE_BURST | RTEMS_EVENT_MODE_SBM2,
630 637 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
631 638 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);
638 639 if (event_out == RTEMS_EVENT_MODE_BURST)
639 640 {
640 ring_node_to_send->sid = SID_BURST_CWF_F2;
641 641 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
642 642 }
643 643 if (event_out == RTEMS_EVENT_MODE_SBM2)
644 644 {
645 ring_node_to_send->sid = SID_SBM2_CWF_F2;
646 645 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
647 646 // launch snapshot extraction if needed
648 647 if (extractSWF == true)
649 648 {
650 ring_node_to_send_swf_f2 = ring_node_to_send;
649 ring_node_to_send_swf_f2 = ring_node_to_send_cwf_f2;
651 650 // extract the snapshot
652 build_snapshot_from_ring( ring_node_to_send_swf_f2, 2 );
651 build_snapshot_from_ring( ring_node_to_send_swf_f2, 2, acquisitionTimeF0_asLong );
653 652 // send the snapshot when built
654 653 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM2 );
655 654 extractSWF = false;
656 655 }
657 656 if (swf_f0_ready && swf_f1_ready)
658 657 {
659 658 extractSWF = true;
659 // record the acquition time of the fΓ  snapshot to use to build the snapshot at f2
660 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
660 661 swf_f0_ready = false;
661 662 swf_f1_ready = false;
662 663 }
663 664 }
664 665 }
665 666 }
666 667
667 668 rtems_task cwf1_task(rtems_task_argument argument) // ONLY USED IN SBM1
668 669 {
669 670 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f1.
670 671 *
671 672 * @param unused is the starting argument of the RTEMS task
672 673 *
673 674 * The following data packet is sent by this function:
674 675 * - TM_LFR_SCIENCE_SBM1_CWF_F1
675 676 *
676 677 */
677 678
678 679 rtems_event_set event_out;
679 680 rtems_id queue_id;
680 681 rtems_status_code status;
681 682
682 683 ring_node * ring_node_to_send_cwf;
683 684
684 685 status = get_message_queue_id_send( &queue_id );
685 686 if (status != RTEMS_SUCCESSFUL)
686 687 {
687 688 PRINTF1("in CWF1 *** ERR get_message_queue_id_send %d\n", status)
688 689 }
689 690
690 691 BOOT_PRINTF("in CWF1 ***\n")
691 692
692 693 while(1){
693 694 // wait for an RTEMS_EVENT
694 695 rtems_event_receive( RTEMS_EVENT_MODE_SBM1,
695 696 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
696 697 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);
703 698 ring_node_to_send_cwf_f1->sid = SID_SBM1_CWF_F1;
704 699 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
705 700 // launch snapshot extraction if needed
706 701 if (extractSWF == true)
707 702 {
708 703 ring_node_to_send_swf_f1 = ring_node_to_send_cwf;
709 704 // launch the snapshot extraction
710 705 status = rtems_event_send( Task_id[TASKID_SWBD], RTEMS_EVENT_MODE_SBM1 );
711 706 extractSWF = false;
712 707 }
713 708 if (swf_f0_ready == true)
714 709 {
715 710 extractSWF = true;
716 711 swf_f0_ready = false; // this step shall be executed only one time
717 712 }
718 713 if ((swf_f1_ready == true) && (swf_f2_ready == true)) // swf_f1 is ready after the extraction
719 714 {
720 715 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM1 );
721 716 swf_f1_ready = false;
722 717 swf_f2_ready = false;
723 718 }
724 719 }
725 720 }
726 721
727 722 rtems_task swbd_task(rtems_task_argument argument)
728 723 {
729 724 /** This RTEMS task is dedicated to the building of snapshots from different continuous waveforms buffers.
730 725 *
731 726 * @param unused is the starting argument of the RTEMS task
732 727 *
733 728 */
734 729
735 730 rtems_event_set event_out;
731 unsigned long long int acquisitionTimeF0_asLong;
732
733 acquisitionTimeF0_asLong = 0x00;
736 734
737 735 BOOT_PRINTF("in SWBD ***\n")
738 736
739 737 while(1){
740 738 // wait for an RTEMS_EVENT
741 739 rtems_event_receive( RTEMS_EVENT_MODE_SBM1 | RTEMS_EVENT_MODE_SBM2,
742 740 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
743 741 if (event_out == RTEMS_EVENT_MODE_SBM1)
744 742 {
745 build_snapshot_from_ring( ring_node_to_send_swf_f1, 1 );
743 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
744 build_snapshot_from_ring( ring_node_to_send_swf_f1, 1, acquisitionTimeF0_asLong );
746 745 swf_f1_ready = true; // the snapshot has been extracted and is ready to be sent
747 746 }
748 747 else
749 748 {
750 749 PRINTF1("in SWBD *** unexpected rtems event received %x\n", (int) event_out)
751 750 }
752 751 }
753 752 }
754 753
755 754 //******************
756 755 // general functions
757 756
758 757 void WFP_init_rings( void )
759 758 {
760 759 // F0 RING
761 760 init_ring( waveform_ring_f0, NB_RING_NODES_F0, wf_buffer_f0, WFRM_BUFFER );
762 761 // F1 RING
763 762 init_ring( waveform_ring_f1, NB_RING_NODES_F1, wf_buffer_f1, WFRM_BUFFER );
764 763 // F2 RING
765 764 init_ring( waveform_ring_f2, NB_RING_NODES_F2, wf_buffer_f2, WFRM_BUFFER );
766 765 // F3 RING
767 766 init_ring( waveform_ring_f3, NB_RING_NODES_F3, wf_buffer_f3, WFRM_BUFFER );
768 767
769 768 ring_node_wf_snap_extracted.buffer_address = (int) wf_snap_extracted;
770 769
771 770 DEBUG_PRINTF1("waveform_ring_f0 @%x\n", (unsigned int) waveform_ring_f0)
772 771 DEBUG_PRINTF1("waveform_ring_f1 @%x\n", (unsigned int) waveform_ring_f1)
773 772 DEBUG_PRINTF1("waveform_ring_f2 @%x\n", (unsigned int) waveform_ring_f2)
774 773 DEBUG_PRINTF1("waveform_ring_f3 @%x\n", (unsigned int) waveform_ring_f3)
775 774 DEBUG_PRINTF1("wf_buffer_f0 @%x\n", (unsigned int) wf_buffer_f0)
776 775 DEBUG_PRINTF1("wf_buffer_f1 @%x\n", (unsigned int) wf_buffer_f1)
777 776 DEBUG_PRINTF1("wf_buffer_f2 @%x\n", (unsigned int) wf_buffer_f2)
778 777 DEBUG_PRINTF1("wf_buffer_f3 @%x\n", (unsigned int) wf_buffer_f3)
779 778
780 779 }
781 780
782 781 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
783 782 {
784 783 unsigned char i;
785 784
786 785 //***************
787 786 // BUFFER ADDRESS
788 787 for(i=0; i<nbNodes; i++)
789 788 {
790 789 ring[i].coarseTime = 0x00;
791 790 ring[i].fineTime = 0x00;
792 791 ring[i].sid = 0x00;
793 792 ring[i].status = 0x00;
794 793 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
795 794 }
796 795
797 796 //*****
798 797 // NEXT
799 798 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
800 799 for(i=0; i<nbNodes-1; i++)
801 800 {
802 801 ring[i].next = (ring_node*) &ring[ i + 1 ];
803 802 }
804 803
805 804 //*********
806 805 // PREVIOUS
807 806 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
808 807 for(i=1; i<nbNodes; i++)
809 808 {
810 809 ring[i].previous = (ring_node*) &ring[ i - 1 ];
811 810 }
812 811 }
813 812
814 813 void WFP_reset_current_ring_nodes( void )
815 814 {
816 815 current_ring_node_f0 = waveform_ring_f0[0].next;
817 816 current_ring_node_f1 = waveform_ring_f1[0].next;
818 817 current_ring_node_f2 = waveform_ring_f2[0].next;
819 818 current_ring_node_f3 = waveform_ring_f3[0].next;
820 819
821 820 ring_node_to_send_swf_f0 = waveform_ring_f0;
822 821 ring_node_to_send_swf_f1 = waveform_ring_f1;
823 822 ring_node_to_send_swf_f2 = waveform_ring_f2;
824 823
825 824 ring_node_to_send_cwf_f1 = waveform_ring_f1;
826 825 ring_node_to_send_cwf_f2 = waveform_ring_f2;
827 826 ring_node_to_send_cwf_f3 = waveform_ring_f3;
828 827 }
829 828
830 829 int send_waveform_CWF3_light( ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id )
831 830 {
832 831 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
833 832 *
834 833 * @param waveform points to the buffer containing the data that will be send.
835 834 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
836 835 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
837 836 * contain information to setup the transmission of the data packets.
838 837 *
839 838 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
840 839 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
841 840 *
842 841 */
843 842
844 843 unsigned int i;
845 844 int ret;
846 845 rtems_status_code status;
847 846
848 847 char *sample;
849 848 int *dataPtr;
850 849
851 850 ret = LFR_DEFAULT;
852 851
853 852 dataPtr = (int*) ring_node_to_send->buffer_address;
854 853
855 854 ring_node_cwf3_light->coarseTime = ring_node_to_send->coarseTime;
856 855 ring_node_cwf3_light->fineTime = ring_node_to_send->fineTime;
857 856
858 857 //**********************
859 858 // BUILD CWF3_light DATA
860 859 for ( i=0; i< NB_SAMPLES_PER_SNAPSHOT; i++)
861 860 {
862 861 sample = (char*) &dataPtr[ (i * NB_WORDS_SWF_BLK) ];
863 862 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) ] = sample[ 0 ];
864 863 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 1 ] = sample[ 1 ];
865 864 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 2 ] = sample[ 2 ];
866 865 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 3 ] = sample[ 3 ];
867 866 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 4 ] = sample[ 4 ];
868 867 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 5 ] = sample[ 5 ];
869 868 }
870 869
870 printf("send_waveform_CWF3_light => [0] = %x, [1] = %x, [2] = %x\n", dataPtr[0], dataPtr[1], dataPtr[2]);
871
871 872 // SEND PACKET
872 873 status = rtems_message_queue_send( queue_id, &ring_node_cwf3_light, sizeof( ring_node* ) );
873 874 if (status != RTEMS_SUCCESSFUL) {
874 875 printf("%d-%d, ERR %d\n", SID_NORM_CWF_F3, i, (int) status);
875 876 ret = LFR_DEFAULT;
876 877 }
877 878
878 879 return ret;
879 880 }
880 881
881 882 void compute_acquisition_time( unsigned int coarseTime, unsigned int fineTime,
882 883 unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char * acquisitionTime )
883 884 {
884 885 unsigned long long int acquisitionTimeAsLong;
885 886 unsigned char localAcquisitionTime[6];
886 887 double deltaT;
887 888
888 889 deltaT = 0.;
889 890
890 891 localAcquisitionTime[0] = (unsigned char) ( coarseTime >> 24 );
891 892 localAcquisitionTime[1] = (unsigned char) ( coarseTime >> 16 );
892 893 localAcquisitionTime[2] = (unsigned char) ( coarseTime >> 8 );
893 894 localAcquisitionTime[3] = (unsigned char) ( coarseTime );
894 895 localAcquisitionTime[4] = (unsigned char) ( fineTime >> 8 );
895 896 localAcquisitionTime[5] = (unsigned char) ( fineTime );
896 897
897 898 acquisitionTimeAsLong = ( (unsigned long long int) localAcquisitionTime[0] << 40 )
898 899 + ( (unsigned long long int) localAcquisitionTime[1] << 32 )
899 900 + ( (unsigned long long int) localAcquisitionTime[2] << 24 )
900 901 + ( (unsigned long long int) localAcquisitionTime[3] << 16 )
901 902 + ( (unsigned long long int) localAcquisitionTime[4] << 8 )
902 903 + ( (unsigned long long int) localAcquisitionTime[5] );
903 904
904 905 switch( sid )
905 906 {
906 907 case SID_NORM_SWF_F0:
907 908 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 24576. ;
908 909 break;
909 910
910 911 case SID_NORM_SWF_F1:
911 912 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 4096. ;
912 913 break;
913 914
914 915 case SID_NORM_SWF_F2:
915 916 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 256. ;
916 917 break;
917 918
918 919 case SID_SBM1_CWF_F1:
919 920 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 4096. ;
920 921 break;
921 922
922 923 case SID_SBM2_CWF_F2:
923 924 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
924 925 break;
925 926
926 927 case SID_BURST_CWF_F2:
927 928 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
928 929 break;
929 930
930 931 case SID_NORM_CWF_F3:
931 932 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF_SHORT_F3 * 65536. / 16. ;
932 933 break;
933 934
934 935 case SID_NORM_CWF_LONG_F3:
935 936 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 16. ;
936 937 break;
937 938
938 939 default:
939 940 PRINTF1("in compute_acquisition_time *** ERR unexpected sid %d\n", sid)
940 941 deltaT = 0.;
941 942 break;
942 943 }
943 944
944 945 acquisitionTimeAsLong = acquisitionTimeAsLong + (unsigned long long int) deltaT;
945 946 //
946 947 acquisitionTime[0] = (unsigned char) (acquisitionTimeAsLong >> 40);
947 948 acquisitionTime[1] = (unsigned char) (acquisitionTimeAsLong >> 32);
948 949 acquisitionTime[2] = (unsigned char) (acquisitionTimeAsLong >> 24);
949 950 acquisitionTime[3] = (unsigned char) (acquisitionTimeAsLong >> 16);
950 951 acquisitionTime[4] = (unsigned char) (acquisitionTimeAsLong >> 8 );
951 952 acquisitionTime[5] = (unsigned char) (acquisitionTimeAsLong );
952 953
953 954 }
954 955
955 void build_snapshot_from_ring( ring_node *ring_node_to_send, unsigned char frequencyChannel )
956 void build_snapshot_from_ring( ring_node *ring_node_to_send, unsigned char frequencyChannel, unsigned long long int acquisitionTimeF0_asLong )
956 957 {
957 958 unsigned int i;
958 959 unsigned long long int centerTime_asLong;
959 unsigned long long int acquisitionTimeF0_asLong;
960 960 unsigned long long int acquisitionTime_asLong;
961 961 unsigned long long int bufferAcquisitionTime_asLong;
962 962 unsigned char *ptr1;
963 963 unsigned char *ptr2;
964 964 unsigned char *timeCharPtr;
965 965 unsigned char nb_ring_nodes;
966 966 unsigned long long int frequency_asLong;
967 967 unsigned long long int nbTicksPerSample_asLong;
968 968 unsigned long long int nbSamplesPart1_asLong;
969 969 unsigned long long int sampleOffset_asLong;
970 970
971 971 unsigned int deltaT_F0;
972 972 unsigned int deltaT_F1;
973 973 unsigned long long int deltaT_F2;
974 974
975 975 deltaT_F0 = 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
976 976 deltaT_F1 = 16384; // (2048. / 4096. / 2.) * 65536. = 16384;
977 977 deltaT_F2 = 262144; // (2048. / 256. / 2.) * 65536. = 262144;
978 978 sampleOffset_asLong = 0x00;
979 979
980 // (1) get the f0 acquisition time
981 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send->coarseTime );
980 // (1) get the f0 acquisition time => the value is passed in argument
982 981
983 982 // (2) compute the central reference time
984 983 centerTime_asLong = acquisitionTimeF0_asLong + deltaT_F0;
984 printf("centerTime_asLong = %llx\n", centerTime_asLong);
985 985
986 986 // (3) compute the acquisition time of the current snapshot
987 987 switch(frequencyChannel)
988 988 {
989 989 case 1: // 1 is for F1 = 4096 Hz
990 990 acquisitionTime_asLong = centerTime_asLong - deltaT_F1;
991 991 nb_ring_nodes = NB_RING_NODES_F1;
992 992 frequency_asLong = 4096;
993 993 nbTicksPerSample_asLong = 16; // 65536 / 4096;
994 994 break;
995 995 case 2: // 2 is for F2 = 256 Hz
996 996 acquisitionTime_asLong = centerTime_asLong - deltaT_F2;
997 997 nb_ring_nodes = NB_RING_NODES_F2;
998 998 frequency_asLong = 256;
999 999 nbTicksPerSample_asLong = 256; // 65536 / 256;
1000 1000 break;
1001 1001 default:
1002 1002 acquisitionTime_asLong = centerTime_asLong;
1003 1003 frequency_asLong = 256;
1004 1004 nbTicksPerSample_asLong = 256;
1005 1005 break;
1006 1006 }
1007 1007
1008 1008 //****************************************************************************
1009 1009 // (4) search the ring_node with the acquisition time <= acquisitionTime_asLong
1010 1010 for (i=0; i<nb_ring_nodes; i++)
1011 1011 {
1012 1012 PRINTF1("%d ... ", i)
1013 bufferAcquisitionTime_asLong = get_acquisition_time( (unsigned char *) ring_node_to_send->coarseTime );
1013 bufferAcquisitionTime_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send->coarseTime );
1014 1014 if (bufferAcquisitionTime_asLong <= acquisitionTime_asLong)
1015 1015 {
1016 1016 PRINTF1("buffer found with acquisition time = %llx\n", bufferAcquisitionTime_asLong)
1017 1017 break;
1018 1018 }
1019 1019 ring_node_to_send = ring_node_to_send->previous;
1020 1020 }
1021 1021
1022 1022 // (5) compute the number of samples to take in the current buffer
1023 1023 sampleOffset_asLong = ((acquisitionTime_asLong - bufferAcquisitionTime_asLong) * frequency_asLong ) >> 16;
1024 1024 nbSamplesPart1_asLong = NB_SAMPLES_PER_SNAPSHOT - sampleOffset_asLong;
1025 PRINTF2("sampleOffset_asLong = %llx, nbSamplesPart1_asLong = %llx\n", sampleOffset_asLong, nbSamplesPart1_asLong)
1025 PRINTF2("sampleOffset_asLong = %lld, nbSamplesPart1_asLong = %lld\n", sampleOffset_asLong, nbSamplesPart1_asLong)
1026 1026
1027 1027 // (6) compute the final acquisition time
1028 1028 acquisitionTime_asLong = bufferAcquisitionTime_asLong +
1029 1029 sampleOffset_asLong * nbTicksPerSample_asLong;
1030 1030
1031 1031 // (7) copy the acquisition time at the beginning of the extrated snapshot
1032 1032 ptr1 = (unsigned char*) &acquisitionTime_asLong;
1033 1033 // fine time
1034 1034 ptr2 = (unsigned char*) &ring_node_wf_snap_extracted.fineTime;
1035 1035 ptr2[2] = ptr1[ 4 + 2 ];
1036 1036 ptr2[3] = ptr1[ 5 + 2 ];
1037 1037 // coarse time
1038 1038 ptr2 = (unsigned char*) &ring_node_wf_snap_extracted.coarseTime;
1039 1039 ptr2[0] = ptr1[ 0 + 2 ];
1040 1040 ptr2[1] = ptr1[ 1 + 2 ];
1041 1041 ptr2[2] = ptr1[ 2 + 2 ];
1042 1042 ptr2[3] = ptr1[ 3 + 2 ];
1043 1043
1044 1044 // re set the synchronization bit
1045 1045 timeCharPtr = (unsigned char*) &ring_node_to_send->coarseTime;
1046 1046 ptr2[0] = ptr2[0] | (timeCharPtr[0] & 0x80); // [1000 0000]
1047 1047
1048 1048 if ( (nbSamplesPart1_asLong >= NB_SAMPLES_PER_SNAPSHOT) | (nbSamplesPart1_asLong < 0) )
1049 1049 {
1050 1050 nbSamplesPart1_asLong = 0;
1051 1051 }
1052 1052 // copy the part 1 of the snapshot in the extracted buffer
1053 1053 for ( i = 0; i < (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i++ )
1054 1054 {
1055 1055 wf_snap_extracted[i] =
1056 1056 ((int*) ring_node_to_send->buffer_address)[ i + (sampleOffset_asLong * NB_WORDS_SWF_BLK) ];
1057 1057 }
1058 1058 // copy the part 2 of the snapshot in the extracted buffer
1059 1059 ring_node_to_send = ring_node_to_send->next;
1060 1060 for ( i = (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i < (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK); i++ )
1061 1061 {
1062 1062 wf_snap_extracted[i] =
1063 1063 ((int*) ring_node_to_send->buffer_address)[ (i-(nbSamplesPart1_asLong * NB_WORDS_SWF_BLK)) ];
1064 1064 }
1065 1065 }
1066 1066
1067 1067 void snapshot_resynchronization( unsigned char *timePtr )
1068 1068 {
1069 1069 unsigned long long int acquisitionTime;
1070 1070 unsigned long long int centerTime;
1071 1071 unsigned long long int previousTick;
1072 1072 unsigned long long int nextTick;
1073 1073 unsigned long long int deltaPreviousTick;
1074 1074 unsigned long long int deltaNextTick;
1075 1075 unsigned int deltaTickInF2;
1076 1076 double deltaPrevious;
1077 1077 double deltaNext;
1078 1078
1079 1079 acquisitionTime = get_acquisition_time( timePtr );
1080 1080
1081 1081 // compute center time
1082 1082 centerTime = acquisitionTime + 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
1083 1083 previousTick = centerTime - (centerTime & 0xffff);
1084 1084 nextTick = previousTick + 65536;
1085 1085
1086 1086 deltaPreviousTick = centerTime - previousTick;
1087 1087 deltaNextTick = nextTick - centerTime;
1088 1088
1089 1089 deltaPrevious = ((double) deltaPreviousTick) / 65536. * 1000.;
1090 1090 deltaNext = ((double) deltaNextTick) / 65536. * 1000.;
1091 1091
1092 1092 printf("delta previous = %f ms, delta next = %f ms\n", deltaPrevious, deltaNext);
1093 1093 printf("delta previous = %llu, delta next = %llu\n", deltaPreviousTick, deltaNextTick);
1094 1094
1095 1095 // which tick is the closest
1096 1096 if (deltaPreviousTick > deltaNextTick)
1097 1097 {
1098 1098 deltaTickInF2 = floor( (deltaNext * 256. / 1000.) ); // the division by 2 is important here
1099 1099 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot + deltaTickInF2;
1100 1100 printf("correction of = + %u\n", deltaTickInF2);
1101 1101 }
1102 1102 else
1103 1103 {
1104 1104 deltaTickInF2 = floor( (deltaPrevious * 256. / 1000.) ); // the division by 2 is important here
1105 1105 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot - deltaTickInF2;
1106 1106 printf("correction of = - %u\n", deltaTickInF2);
1107 1107 }
1108 1108 }
1109 1109
1110 1110 //**************
1111 1111 // wfp registers
1112 1112 void reset_wfp_burst_enable( void )
1113 1113 {
1114 1114 /** This function resets the waveform picker burst_enable register.
1115 1115 *
1116 1116 * The burst bits [f2 f1 f0] and the enable bits [f3 f2 f1 f0] are set to 0.
1117 1117 *
1118 1118 */
1119 1119
1120 1120 // [1000 000] burst f2, f1, f0 enable f3, f2, f1, f0
1121 1121 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable & 0x80;
1122 1122 }
1123 1123
1124 1124 void reset_wfp_status( void )
1125 1125 {
1126 1126 /** This function resets the waveform picker status register.
1127 1127 *
1128 1128 * All status bits are set to 0 [new_err full_err full].
1129 1129 *
1130 1130 */
1131 1131
1132 1132 waveform_picker_regs->status = 0xffff;
1133 1133 }
1134 1134
1135 1135 void reset_wfp_buffer_addresses( void )
1136 1136 {
1137 1137 // F0
1138 1138 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->previous->buffer_address; // 0x08
1139 1139 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address; // 0x0c
1140 1140 // F1
1141 1141 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->previous->buffer_address; // 0x10
1142 1142 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address; // 0x14
1143 1143 // F2
1144 1144 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->previous->buffer_address; // 0x18
1145 1145 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; // 0x1c
1146 1146 // F3
1147 1147 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->previous->buffer_address; // 0x20
1148 1148 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address; // 0x24
1149 1149 }
1150 1150
1151 1151 void reset_waveform_picker_regs( void )
1152 1152 {
1153 1153 /** This function resets the waveform picker module registers.
1154 1154 *
1155 1155 * The registers affected by this function are located at the following offset addresses:
1156 1156 * - 0x00 data_shaping
1157 1157 * - 0x04 run_burst_enable
1158 1158 * - 0x08 addr_data_f0
1159 1159 * - 0x0C addr_data_f1
1160 1160 * - 0x10 addr_data_f2
1161 1161 * - 0x14 addr_data_f3
1162 1162 * - 0x18 status
1163 1163 * - 0x1C delta_snapshot
1164 1164 * - 0x20 delta_f0
1165 1165 * - 0x24 delta_f0_2
1166 1166 * - 0x28 delta_f1
1167 1167 * - 0x2c delta_f2
1168 1168 * - 0x30 nb_data_by_buffer
1169 1169 * - 0x34 nb_snapshot_param
1170 1170 * - 0x38 start_date
1171 1171 * - 0x3c nb_word_in_buffer
1172 1172 *
1173 1173 */
1174 1174
1175 1175 set_wfp_data_shaping(); // 0x00 *** R1 R0 SP1 SP0 BW
1176 1176
1177 1177 reset_wfp_burst_enable(); // 0x04 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
1178 1178
1179 1179 reset_wfp_buffer_addresses();
1180 1180
1181 1181 reset_wfp_status(); // 0x18
1182 1182
1183 1183 set_wfp_delta_snapshot(); // 0x1c *** 300 s => 0x12bff
1184 1184
1185 1185 set_wfp_delta_f0_f0_2(); // 0x20, 0x24
1186 1186
1187 1187 set_wfp_delta_f1(); // 0x28
1188 1188
1189 1189 set_wfp_delta_f2(); // 0x2c
1190 1190
1191 1191 DEBUG_PRINTF1("delta_snapshot %x\n", waveform_picker_regs->delta_snapshot)
1192 1192 DEBUG_PRINTF1("delta_f0 %x\n", waveform_picker_regs->delta_f0)
1193 1193 DEBUG_PRINTF1("delta_f0_2 %x\n", waveform_picker_regs->delta_f0_2)
1194 1194 DEBUG_PRINTF1("delta_f1 %x\n", waveform_picker_regs->delta_f1)
1195 1195 DEBUG_PRINTF1("delta_f2 %x\n", waveform_picker_regs->delta_f2)
1196 1196 // 2688 = 8 * 336
1197 1197 waveform_picker_regs->nb_data_by_buffer = 0xa7f; // 0x30 *** 2688 - 1 => nb samples -1
1198 1198 waveform_picker_regs->snapshot_param = 0xa80; // 0x34 *** 2688 => nb samples
1199 1199 waveform_picker_regs->start_date = 0x7fffffff; // 0x38
1200 1200 //
1201 1201 // coarse time and fine time registers are not initialized, they are volatile
1202 1202 //
1203 1203 waveform_picker_regs->buffer_length = 0x1f8;// buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
1204 1204 }
1205 1205
1206 1206 void set_wfp_data_shaping( void )
1207 1207 {
1208 1208 /** This function sets the data_shaping register of the waveform picker module.
1209 1209 *
1210 1210 * The value is read from one field of the parameter_dump_packet structure:\n
1211 1211 * bw_sp0_sp1_r0_r1
1212 1212 *
1213 1213 */
1214 1214
1215 1215 unsigned char data_shaping;
1216 1216
1217 1217 // get the parameters for the data shaping [BW SP0 SP1 R0 R1] in sy_lfr_common1 and configure the register
1218 1218 // waveform picker : [R1 R0 SP1 SP0 BW]
1219 1219
1220 1220 data_shaping = parameter_dump_packet.bw_sp0_sp1_r0_r1;
1221 1221
1222 1222 waveform_picker_regs->data_shaping =
1223 1223 ( (data_shaping & 0x10) >> 4 ) // BW
1224 1224 + ( (data_shaping & 0x08) >> 2 ) // SP0
1225 1225 + ( (data_shaping & 0x04) ) // SP1
1226 1226 + ( (data_shaping & 0x02) << 2 ) // R0
1227 1227 + ( (data_shaping & 0x01) << 4 ); // R1
1228 1228 }
1229 1229
1230 1230 void set_wfp_burst_enable_register( unsigned char mode )
1231 1231 {
1232 1232 /** This function sets the waveform picker burst_enable register depending on the mode.
1233 1233 *
1234 1234 * @param mode is the LFR mode to launch.
1235 1235 *
1236 1236 * The burst bits shall be before the enable bits.
1237 1237 *
1238 1238 */
1239 1239
1240 1240 // [0000 0000] burst f2, f1, f0 enable f3 f2 f1 f0
1241 1241 // the burst bits shall be set first, before the enable bits
1242 1242 switch(mode) {
1243 1243 case(LFR_MODE_NORMAL):
1244 1244 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enable
1245 1245 waveform_picker_regs->run_burst_enable = 0x0f; // [0000 1111] enable f3 f2 f1 f0
1246 1246 break;
1247 1247 case(LFR_MODE_BURST):
1248 1248 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1249 1249 // waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x04; // [0100] enable f2
1250 1250 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0c; // [1100] enable f3 AND f2
1251 1251 break;
1252 1252 case(LFR_MODE_SBM1):
1253 1253 waveform_picker_regs->run_burst_enable = 0x20; // [0010 0000] f1 burst enabled
1254 1254 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1255 1255 break;
1256 1256 case(LFR_MODE_SBM2):
1257 1257 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1258 1258 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1259 1259 break;
1260 1260 default:
1261 1261 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enabled, no waveform enabled
1262 1262 break;
1263 1263 }
1264 1264 }
1265 1265
1266 1266 void set_wfp_delta_snapshot( void )
1267 1267 {
1268 1268 /** This function sets the delta_snapshot register of the waveform picker module.
1269 1269 *
1270 1270 * The value is read from two (unsigned char) of the parameter_dump_packet structure:
1271 1271 * - sy_lfr_n_swf_p[0]
1272 1272 * - sy_lfr_n_swf_p[1]
1273 1273 *
1274 1274 */
1275 1275
1276 1276 unsigned int delta_snapshot;
1277 1277 unsigned int delta_snapshot_in_T2;
1278 1278
1279 1279 delta_snapshot = parameter_dump_packet.sy_lfr_n_swf_p[0]*256
1280 1280 + parameter_dump_packet.sy_lfr_n_swf_p[1];
1281 1281
1282 1282 delta_snapshot_in_T2 = delta_snapshot * 256;
1283 1283 waveform_picker_regs->delta_snapshot = delta_snapshot_in_T2 - 1; // max 4 bytes
1284 1284 }
1285 1285
1286 1286 void set_wfp_delta_f0_f0_2( void )
1287 1287 {
1288 1288 unsigned int delta_snapshot;
1289 1289 unsigned int nb_samples_per_snapshot;
1290 1290 float delta_f0_in_float;
1291 1291
1292 1292 delta_snapshot = waveform_picker_regs->delta_snapshot;
1293 1293 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1294 1294 delta_f0_in_float =nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 24576.) * 256.;
1295 1295
1296 1296 waveform_picker_regs->delta_f0 = delta_snapshot - floor( delta_f0_in_float );
1297 1297 waveform_picker_regs->delta_f0_2 = 0x7; // max 7 bits
1298 1298 }
1299 1299
1300 1300 void set_wfp_delta_f1( void )
1301 1301 {
1302 1302 unsigned int delta_snapshot;
1303 1303 unsigned int nb_samples_per_snapshot;
1304 1304 float delta_f1_in_float;
1305 1305
1306 1306 delta_snapshot = waveform_picker_regs->delta_snapshot;
1307 1307 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1308 1308 delta_f1_in_float = nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 4096.) * 256.;
1309 1309
1310 1310 waveform_picker_regs->delta_f1 = delta_snapshot - floor( delta_f1_in_float );
1311 1311 }
1312 1312
1313 1313 void set_wfp_delta_f2()
1314 1314 {
1315 1315 unsigned int delta_snapshot;
1316 1316 unsigned int nb_samples_per_snapshot;
1317 1317
1318 1318 delta_snapshot = waveform_picker_regs->delta_snapshot;
1319 1319 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1320 1320
1321 1321 waveform_picker_regs->delta_f2 = delta_snapshot - nb_samples_per_snapshot / 2;
1322 1322 }
1323 1323
1324 1324 //*****************
1325 1325 // local parameters
1326 1326
1327 1327 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid )
1328 1328 {
1329 1329 /** This function increments the parameter "sequence_cnt" depending on the sid passed in argument.
1330 1330 *
1331 1331 * @param packet_sequence_control is a pointer toward the parameter sequence_cnt to update.
1332 1332 * @param sid is the source identifier of the packet being updated.
1333 1333 *
1334 1334 * REQ-LFR-SRS-5240 / SSS-CP-FS-590
1335 1335 * The sequence counters shall wrap around from 2^14 to zero.
1336 1336 * The sequence counter shall start at zero at startup.
1337 1337 *
1338 1338 * REQ-LFR-SRS-5239 / SSS-CP-FS-580
1339 1339 * All TM_LFR_SCIENCE_ packets are sent to ground, i.e. destination id = 0
1340 1340 *
1341 1341 */
1342 1342
1343 1343 unsigned short *sequence_cnt;
1344 1344 unsigned short segmentation_grouping_flag;
1345 1345 unsigned short new_packet_sequence_control;
1346 1346 rtems_mode initial_mode_set;
1347 1347 rtems_mode current_mode_set;
1348 1348 rtems_status_code status;
1349 1349
1350 1350 //******************************************
1351 1351 // CHANGE THE MODE OF THE CALLING RTEMS TASK
1352 1352 status = rtems_task_mode( RTEMS_NO_PREEMPT, RTEMS_PREEMPT_MASK, &initial_mode_set );
1353 1353
1354 1354 if ( (sid == SID_NORM_SWF_F0) || (sid == SID_NORM_SWF_F1) || (sid == SID_NORM_SWF_F2)
1355 1355 || (sid == SID_NORM_CWF_F3) || (sid == SID_NORM_CWF_LONG_F3)
1356 1356 || (sid == SID_BURST_CWF_F2)
1357 1357 || (sid == SID_NORM_ASM_F0) || (sid == SID_NORM_ASM_F1) || (sid == SID_NORM_ASM_F2)
1358 1358 || (sid == SID_NORM_BP1_F0) || (sid == SID_NORM_BP1_F1) || (sid == SID_NORM_BP1_F2)
1359 1359 || (sid == SID_NORM_BP2_F0) || (sid == SID_NORM_BP2_F1) || (sid == SID_NORM_BP2_F2)
1360 1360 || (sid == SID_BURST_BP1_F0) || (sid == SID_BURST_BP2_F0)
1361 1361 || (sid == SID_BURST_BP1_F1) || (sid == SID_BURST_BP2_F1) )
1362 1362 {
1363 1363 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_NORMAL_BURST;
1364 1364 }
1365 1365 else if ( (sid ==SID_SBM1_CWF_F1) || (sid ==SID_SBM2_CWF_F2)
1366 1366 || (sid == SID_SBM1_BP1_F0) || (sid == SID_SBM1_BP2_F0)
1367 1367 || (sid == SID_SBM2_BP1_F0) || (sid == SID_SBM2_BP2_F0)
1368 1368 || (sid == SID_SBM2_BP1_F1) || (sid == SID_SBM2_BP2_F1) )
1369 1369 {
1370 1370 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_SBM1_SBM2;
1371 1371 }
1372 1372 else
1373 1373 {
1374 1374 sequence_cnt = (unsigned short *) NULL;
1375 1375 PRINTF1("in increment_seq_counter_source_id *** ERR apid_destid %d not known\n", sid)
1376 1376 }
1377 1377
1378 1378 if (sequence_cnt != NULL)
1379 1379 {
1380 1380 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
1381 1381 *sequence_cnt = (*sequence_cnt) & 0x3fff;
1382 1382
1383 1383 new_packet_sequence_control = segmentation_grouping_flag | (*sequence_cnt) ;
1384 1384
1385 1385 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
1386 1386 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1387 1387
1388 1388 // increment the sequence counter
1389 1389 if ( *sequence_cnt < SEQ_CNT_MAX)
1390 1390 {
1391 1391 *sequence_cnt = *sequence_cnt + 1;
1392 1392 }
1393 1393 else
1394 1394 {
1395 1395 *sequence_cnt = 0;
1396 1396 }
1397 1397 }
1398 1398
1399 1399 //***********************************
1400 1400 // RESET THE MODE OF THE CALLING TASK
1401 1401 status = rtems_task_mode( initial_mode_set, RTEMS_PREEMPT_MASK, &current_mode_set );
1402 1402 }
General Comments 0
You need to be logged in to leave comments. Login now