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

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

Major bug corrected on the handling of TC_LFR_LOAD_COMMON_PAR
paul -
r199:6628458d4fb0 R3
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,72 +1,72
1 1 #include <stdio.h>
2 2
3 3 #include "grspw.h"
4 4 #include "fsw_params.h"
5 5
6 6 #define DSU_TIME_TAG_COUNTER 0x90000008
7 7
8 8 //**********
9 9 // IRQ LINES
10 10 #define IRQ_GRSPW 11
11 11 #define IRQ_SPARC_GRSPW 0x1b // see sparcv8.pdf p.76 for interrupt levels
12 12
13 13 extern void *catch_interrupt(void func(), int irq);
14 14 int *lreg = (int *) 0x80000000;
15 15
16 16 #define ICLEAR 0x20c
17 17 #define IMASK 0x240
18 18 #define IFORCE 0x208
19 19
20 20 void enable_irq (int irq)
21 21 {
22 22 lreg[ICLEAR/4] = (1 << irq); // clear any pending irq
23 23 lreg[IMASK/4] |= (1 << irq); // unmaks irq
24 24 }
25 25
26 26 void disable_irq (int irq) { lreg[IMASK/4] &= ~(1 << irq); } // mask irq
27 27
28 28 void force_irq (int irq) { lreg[IFORCE/4] = (1 << irq); } // force irq
29 29
30 30 /* NOTE: NEVER put printf() or other stdio routines in interrupt handlers,
31 31 they are not re-entrant. This (bad) example is just a demo */
32 32
33 33 volatile unsigned char processTimecode = 0;
34 34 unsigned int counter = 0;
35 35
36 36 void irqhandler(int irq)
37 37 {
38 38 processTimecode = 1;
39 39 counter ++;
40 40 }
41 41
42 42 int main( void )
43 43 {
44 44 unsigned int *grspwCtrlReg;
45 45 unsigned int k;
46 46 volatile unsigned int *reg;
47 47 float aux;
48 48
49 49 printf("hello world!\n");
50 50
51 51 grspwCtrlReg = (unsigned int*) REGS_ADDR_GRSPW;
52 52 grspw_set_ie( 1, grspwCtrlReg );
53 53 grspw_set_tq( 1, grspwCtrlReg );
54 54 grspw_set_tr( 1, grspwCtrlReg );
55 55
56 56 catch_interrupt(irqhandler, IRQ_GRSPW);
57 57 enable_irq( IRQ_GRSPW );
58 58 force_irq( IRQ_GRSPW );
59 59
60 60 reg = (volatile unsigned int *) DSU_TIME_TAG_COUNTER;
61 61
62 62 while(1)
63 63 {
64 // if (processTimecode == 1)
65 // {
66 // printf("timecode counter = %d\n", counter);
67 // processTimecode = 0;
68 // }
64 if (processTimecode == 1)
65 {
66 printf("timecode counter = %d\n", counter);
67 processTimecode = 0;
68 }
69 69 }
70 70
71 71 return 0;
72 72 }
Show file before | Show file after | Hide comments
@@ -1,112 +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 5 CONFIG += console verbose lpp_dpu_destid cpu_usage_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=3 # major
13 13 DEFINES += SW_VERSION_N2=0 # minor
14 14 DEFINES += SW_VERSION_N3=0 # patch
15 DEFINES += SW_VERSION_N4=0 # internal
15 DEFINES += SW_VERSION_N4=1 # internal
16 16
17 17 # <GCOV>
18 18 #QMAKE_CFLAGS_RELEASE += -fprofile-arcs -ftest-coverage
19 19 #LIBS += -lgcov /opt/GCOV/01A/lib/overload.o -lc
20 20 # </GCOV>
21 21
22 22 # <CHANGE BEFORE FLIGHT>
23 23 contains( CONFIG, lpp_dpu_destid ) {
24 24 DEFINES += LPP_DPU_DESTID
25 25 }
26 26 # </CHANGE BEFORE FLIGHT>
27 27
28 28 contains( CONFIG, debug_tch ) {
29 29 DEFINES += DEBUG_TCH
30 30 }
31 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 110 ../LFR_basic-parameters/basic_parameters_params.h \
111 111 ../header/GscMemoryLPP.hpp
112 112
Show file before | Show file after | Hide comments
@@ -1,1120 +1,1127
1 1 /** Functions to load and dump parameters in the LFR registers.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TC related to parameter loading and dumping.\n
7 7 * TC_LFR_LOAD_COMMON_PAR\n
8 8 * TC_LFR_LOAD_NORMAL_PAR\n
9 9 * TC_LFR_LOAD_BURST_PAR\n
10 10 * TC_LFR_LOAD_SBM1_PAR\n
11 11 * TC_LFR_LOAD_SBM2_PAR\n
12 12 *
13 13 */
14 14
15 15 #include "tc_load_dump_parameters.h"
16 16
17 17 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_1;
18 18 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2;
19 19 ring_node kcoefficient_node_1;
20 20 ring_node kcoefficient_node_2;
21 21
22 22 int action_load_common_par(ccsdsTelecommandPacket_t *TC)
23 23 {
24 24 /** This function updates the LFR registers with the incoming common parameters.
25 25 *
26 26 * @param TC points to the TeleCommand packet that is being processed
27 27 *
28 28 *
29 29 */
30 30
31 31 parameter_dump_packet.sy_lfr_common_parameters_spare = TC->dataAndCRC[0];
32 32 parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
33 33 set_wfp_data_shaping( );
34 34 return LFR_SUCCESSFUL;
35 35 }
36 36
37 37 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
38 38 {
39 39 /** This function updates the LFR registers with the incoming normal parameters.
40 40 *
41 41 * @param TC points to the TeleCommand packet that is being processed
42 42 * @param queue_id is the id of the queue which handles TM related to this execution step
43 43 *
44 44 */
45 45
46 46 int result;
47 47 int flag;
48 48 rtems_status_code status;
49 49
50 50 flag = LFR_SUCCESSFUL;
51 51
52 52 if ( (lfrCurrentMode == LFR_MODE_NORMAL) ||
53 53 (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) {
54 54 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
55 55 flag = LFR_DEFAULT;
56 56 }
57 57
58 58 // CHECK THE PARAMETERS SET CONSISTENCY
59 59 if (flag == LFR_SUCCESSFUL)
60 60 {
61 61 flag = check_common_par_consistency( TC, queue_id );
62 62 }
63 63
64 64 // SET THE PARAMETERS IF THEY ARE CONSISTENT
65 65 if (flag == LFR_SUCCESSFUL)
66 66 {
67 67 result = set_sy_lfr_n_swf_l( TC );
68 68 result = set_sy_lfr_n_swf_p( TC );
69 69 result = set_sy_lfr_n_bp_p0( TC );
70 70 result = set_sy_lfr_n_bp_p1( TC );
71 71 result = set_sy_lfr_n_asm_p( TC );
72 72 result = set_sy_lfr_n_cwf_long_f3( TC );
73 73 }
74 74
75 75 return flag;
76 76 }
77 77
78 78 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
79 79 {
80 80 /** This function updates the LFR registers with the incoming burst parameters.
81 81 *
82 82 * @param TC points to the TeleCommand packet that is being processed
83 83 * @param queue_id is the id of the queue which handles TM related to this execution step
84 84 *
85 85 */
86 86
87 87 int flag;
88 88 rtems_status_code status;
89 89 unsigned char sy_lfr_b_bp_p0;
90 90 unsigned char sy_lfr_b_bp_p1;
91 91 float aux;
92 92
93 93 flag = LFR_SUCCESSFUL;
94 94
95 95 if ( lfrCurrentMode == LFR_MODE_BURST ) {
96 96 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
97 97 flag = LFR_DEFAULT;
98 98 }
99 99
100 100 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
101 101 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
102 102
103 103 // sy_lfr_b_bp_p0
104 104 if (flag == LFR_SUCCESSFUL)
105 105 {
106 106 if (sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0 )
107 107 {
108 108 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0+10, sy_lfr_b_bp_p0 );
109 109 flag = WRONG_APP_DATA;
110 110 }
111 111 }
112 112 // sy_lfr_b_bp_p1
113 113 if (flag == LFR_SUCCESSFUL)
114 114 {
115 115 if (sy_lfr_b_bp_p1 < DEFAULT_SY_LFR_B_BP_P1 )
116 116 {
117 117 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P1+10, sy_lfr_b_bp_p1 );
118 118 flag = WRONG_APP_DATA;
119 119 }
120 120 }
121 121 //****************************************************************
122 122 // check the consistency between sy_lfr_b_bp_p0 and sy_lfr_b_bp_p1
123 123 if (flag == LFR_SUCCESSFUL)
124 124 {
125 125 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
126 126 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
127 127 aux = ( (float ) sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0 ) - floor(sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0);
128 128 if (aux > FLOAT_EQUAL_ZERO)
129 129 {
130 130 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0+10, sy_lfr_b_bp_p0 );
131 131 flag = LFR_DEFAULT;
132 132 }
133 133 }
134 134
135 135 // SET HTE PARAMETERS
136 136 if (flag == LFR_SUCCESSFUL)
137 137 {
138 138 flag = set_sy_lfr_b_bp_p0( TC );
139 139 flag = set_sy_lfr_b_bp_p1( TC );
140 140 }
141 141
142 142 return flag;
143 143 }
144 144
145 145 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
146 146 {
147 147 /** This function updates the LFR registers with the incoming sbm1 parameters.
148 148 *
149 149 * @param TC points to the TeleCommand packet that is being processed
150 150 * @param queue_id is the id of the queue which handles TM related to this execution step
151 151 *
152 152 */
153 153
154 154 int flag;
155 155 rtems_status_code status;
156 156 unsigned char sy_lfr_s1_bp_p0;
157 157 unsigned char sy_lfr_s1_bp_p1;
158 158 float aux;
159 159
160 160 flag = LFR_SUCCESSFUL;
161 161
162 162 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
163 163 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
164 164 flag = LFR_DEFAULT;
165 165 }
166 166
167 167 sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
168 168 sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
169 169
170 170 // sy_lfr_s1_bp_p0
171 171 if (flag == LFR_SUCCESSFUL)
172 172 {
173 173 if (sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0 )
174 174 {
175 175 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0+10, sy_lfr_s1_bp_p0 );
176 176 flag = WRONG_APP_DATA;
177 177 }
178 178 }
179 179 // sy_lfr_s1_bp_p1
180 180 if (flag == LFR_SUCCESSFUL)
181 181 {
182 182 if (sy_lfr_s1_bp_p1 < DEFAULT_SY_LFR_S1_BP_P1 )
183 183 {
184 184 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P1+10, sy_lfr_s1_bp_p1 );
185 185 flag = WRONG_APP_DATA;
186 186 }
187 187 }
188 188 //******************************************************************
189 189 // check the consistency between sy_lfr_s1_bp_p0 and sy_lfr_s1_bp_p1
190 190 if (flag == LFR_SUCCESSFUL)
191 191 {
192 192 aux = ( (float ) sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0*0.25) ) - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0*0.25));
193 193 if (aux > FLOAT_EQUAL_ZERO)
194 194 {
195 195 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0+10, sy_lfr_s1_bp_p0 );
196 196 flag = LFR_DEFAULT;
197 197 }
198 198 }
199 199
200 200 // SET THE PARAMETERS
201 201 if (flag == LFR_SUCCESSFUL)
202 202 {
203 203 flag = set_sy_lfr_s1_bp_p0( TC );
204 204 flag = set_sy_lfr_s1_bp_p1( TC );
205 205 }
206 206
207 207 return flag;
208 208 }
209 209
210 210 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
211 211 {
212 212 /** This function updates the LFR registers with the incoming sbm2 parameters.
213 213 *
214 214 * @param TC points to the TeleCommand packet that is being processed
215 215 * @param queue_id is the id of the queue which handles TM related to this execution step
216 216 *
217 217 */
218 218
219 219 int flag;
220 220 rtems_status_code status;
221 221 unsigned char sy_lfr_s2_bp_p0;
222 222 unsigned char sy_lfr_s2_bp_p1;
223 223 float aux;
224 224
225 225 flag = LFR_SUCCESSFUL;
226 226
227 227 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
228 228 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
229 229 flag = LFR_DEFAULT;
230 230 }
231 231
232 232 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
233 233 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
234 234
235 235 // sy_lfr_s2_bp_p0
236 236 if (flag == LFR_SUCCESSFUL)
237 237 {
238 238 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
239 239 {
240 240 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
241 241 flag = WRONG_APP_DATA;
242 242 }
243 243 }
244 244 // sy_lfr_s2_bp_p1
245 245 if (flag == LFR_SUCCESSFUL)
246 246 {
247 247 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
248 248 {
249 249 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P1+10, sy_lfr_s2_bp_p1 );
250 250 flag = WRONG_APP_DATA;
251 251 }
252 252 }
253 253 //******************************************************************
254 254 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
255 255 if (flag == LFR_SUCCESSFUL)
256 256 {
257 257 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
258 258 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
259 259 aux = ( (float ) sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0 ) - floor(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
260 260 if (aux > FLOAT_EQUAL_ZERO)
261 261 {
262 262 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
263 263 flag = LFR_DEFAULT;
264 264 }
265 265 }
266 266
267 267 // SET THE PARAMETERS
268 268 if (flag == LFR_SUCCESSFUL)
269 269 {
270 270 flag = set_sy_lfr_s2_bp_p0( TC );
271 271 flag = set_sy_lfr_s2_bp_p1( TC );
272 272 }
273 273
274 274 return flag;
275 275 }
276 276
277 277 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
278 278 {
279 279 /** This function updates the LFR registers with the incoming sbm2 parameters.
280 280 *
281 281 * @param TC points to the TeleCommand packet that is being processed
282 282 * @param queue_id is the id of the queue which handles TM related to this execution step
283 283 *
284 284 */
285 285
286 286 int flag;
287 287
288 288 flag = LFR_DEFAULT;
289 289
290 290 flag = set_sy_lfr_kcoeff( TC );
291 291
292 292 return flag;
293 293 }
294 294
295 295 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
296 296 {
297 297 /** This function updates the LFR registers with the incoming sbm2 parameters.
298 298 *
299 299 * @param TC points to the TeleCommand packet that is being processed
300 300 * @param queue_id is the id of the queue which handles TM related to this execution step
301 301 *
302 302 */
303 303
304 304 int flag;
305 305
306 306 flag = LFR_DEFAULT;
307 307
308 308 flag = set_sy_lfr_fbins( TC );
309 309
310 310 return flag;
311 311 }
312 312
313 313 void printKCoefficients(unsigned int freq, unsigned int bin, float *k_coeff)
314 314 {
315 315 printf("freq = %d *** bin = %d *** (0) %f *** (1) %f *** (2) %f *** (3) %f *** (4) %f\n",
316 316 freq,
317 317 bin,
318 318 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 0 ],
319 319 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 1 ],
320 320 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 2 ],
321 321 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 3 ],
322 322 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 4 ]);
323 323 }
324 324
325 325 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
326 326 {
327 327 /** This function updates the LFR registers with the incoming sbm2 parameters.
328 328 *
329 329 * @param TC points to the TeleCommand packet that is being processed
330 330 * @param queue_id is the id of the queue which handles TM related to this execution step
331 331 *
332 332 */
333 333
334 334 unsigned int address;
335 335 rtems_status_code status;
336 336 unsigned int freq;
337 337 unsigned int bin;
338 338 unsigned int coeff;
339 339 unsigned char *kCoeffPtr;
340 340 unsigned char *kCoeffDumpPtr;
341 341
342 342 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
343 343 // F0 => 11 bins
344 344 // F1 => 13 bins
345 345 // F2 => 12 bins
346 346 // 36 bins to dump in two packets (30 bins max per packet)
347 347
348 348 //*********
349 349 // PACKET 1
350 350 // 11 F0 bins, 13 F1 bins and 6 F2 bins
351 351 kcoefficients_dump_1.packetSequenceControl[0] = (unsigned char) (sequenceCounterParameterDump >> 8);
352 352 kcoefficients_dump_1.packetSequenceControl[1] = (unsigned char) (sequenceCounterParameterDump );
353 353 increment_seq_counter( &sequenceCounterParameterDump );
354 354 for( freq=0;
355 355 freq<NB_BINS_COMPRESSED_SM_F0;
356 356 freq++ )
357 357 {
358 358 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1] = freq;
359 359 bin = freq;
360 360 // printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
361 361 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
362 362 {
363 363 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
364 364 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
365 365 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
366 366 }
367 367 }
368 368 for( freq=NB_BINS_COMPRESSED_SM_F0;
369 369 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
370 370 freq++ )
371 371 {
372 372 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
373 373 bin = freq - NB_BINS_COMPRESSED_SM_F0;
374 374 // printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
375 375 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
376 376 {
377 377 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
378 378 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
379 379 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
380 380 }
381 381 }
382 382 for( freq=(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
383 383 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1+6);
384 384 freq++ )
385 385 {
386 386 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
387 387 bin = freq - (NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
388 388 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
389 389 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
390 390 {
391 391 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
392 392 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
393 393 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
394 394 }
395 395 }
396 396 kcoefficients_dump_1.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
397 397 kcoefficients_dump_1.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
398 398 kcoefficients_dump_1.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
399 399 kcoefficients_dump_1.time[3] = (unsigned char) (time_management_regs->coarse_time);
400 400 kcoefficients_dump_1.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
401 401 kcoefficients_dump_1.time[5] = (unsigned char) (time_management_regs->fine_time);
402 402 // SEND DATA
403 403 kcoefficient_node_1.status = 1;
404 404 address = (unsigned int) &kcoefficient_node_1;
405 405 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
406 406 if (status != RTEMS_SUCCESSFUL) {
407 407 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
408 408 }
409 409
410 410 //********
411 411 // PACKET 2
412 412 // 6 F2 bins
413 413 kcoefficients_dump_2.packetSequenceControl[0] = (unsigned char) (sequenceCounterParameterDump >> 8);
414 414 kcoefficients_dump_2.packetSequenceControl[1] = (unsigned char) (sequenceCounterParameterDump );
415 415 increment_seq_counter( &sequenceCounterParameterDump );
416 416 for( freq=0; freq<6; freq++ )
417 417 {
418 418 kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + 6 + freq;
419 419 bin = freq + 6;
420 420 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
421 421 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
422 422 {
423 423 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
424 424 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
425 425 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
426 426 }
427 427 }
428 428 kcoefficients_dump_2.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
429 429 kcoefficients_dump_2.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
430 430 kcoefficients_dump_2.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
431 431 kcoefficients_dump_2.time[3] = (unsigned char) (time_management_regs->coarse_time);
432 432 kcoefficients_dump_2.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
433 433 kcoefficients_dump_2.time[5] = (unsigned char) (time_management_regs->fine_time);
434 434 // SEND DATA
435 435 kcoefficient_node_2.status = 1;
436 436 address = (unsigned int) &kcoefficient_node_2;
437 437 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
438 438 if (status != RTEMS_SUCCESSFUL) {
439 439 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
440 440 }
441 441
442 442 return status;
443 443 }
444 444
445 445 int action_dump_par( rtems_id queue_id )
446 446 {
447 447 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
448 448 *
449 449 * @param queue_id is the id of the queue which handles TM related to this execution step.
450 450 *
451 451 * @return RTEMS directive status codes:
452 452 * - RTEMS_SUCCESSFUL - message sent successfully
453 453 * - RTEMS_INVALID_ID - invalid queue id
454 454 * - RTEMS_INVALID_SIZE - invalid message size
455 455 * - RTEMS_INVALID_ADDRESS - buffer is NULL
456 456 * - RTEMS_UNSATISFIED - out of message buffers
457 457 * - RTEMS_TOO_MANY - queue s limit has been reached
458 458 *
459 459 */
460 460
461 461 int status;
462 462
463 463 // UPDATE TIME
464 464 parameter_dump_packet.packetSequenceControl[0] = (unsigned char) (sequenceCounterParameterDump >> 8);
465 465 parameter_dump_packet.packetSequenceControl[1] = (unsigned char) (sequenceCounterParameterDump );
466 466 increment_seq_counter( &sequenceCounterParameterDump );
467 467
468 468 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
469 469 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
470 470 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
471 471 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
472 472 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
473 473 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
474 474 // SEND DATA
475 475 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
476 476 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
477 477 if (status != RTEMS_SUCCESSFUL) {
478 478 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
479 479 }
480 480
481 481 return status;
482 482 }
483 483
484 484 //***********************
485 485 // NORMAL MODE PARAMETERS
486 486
487 487 int check_common_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
488 488 {
489 489 unsigned char msb;
490 490 unsigned char lsb;
491 491 int flag;
492 492 float aux;
493 493 rtems_status_code status;
494 494
495 495 unsigned int sy_lfr_n_swf_l;
496 496 unsigned int sy_lfr_n_swf_p;
497 497 unsigned int sy_lfr_n_asm_p;
498 498 unsigned char sy_lfr_n_bp_p0;
499 499 unsigned char sy_lfr_n_bp_p1;
500 500 unsigned char sy_lfr_n_cwf_long_f3;
501 501
502 502 flag = LFR_SUCCESSFUL;
503 503
504 504 //***************
505 505 // get parameters
506 506 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
507 507 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
508 508 sy_lfr_n_swf_l = msb * 256 + lsb;
509 509
510 510 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
511 511 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
512 512 sy_lfr_n_swf_p = msb * 256 + lsb;
513 513
514 514 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
515 515 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
516 516 sy_lfr_n_asm_p = msb * 256 + lsb;
517 517
518 518 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
519 519
520 520 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
521 521
522 522 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
523 523
524 524 //******************
525 525 // check consistency
526 526 // sy_lfr_n_swf_l
527 527 if (sy_lfr_n_swf_l != 2048)
528 528 {
529 529 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L+10, sy_lfr_n_swf_l );
530 530 flag = WRONG_APP_DATA;
531 531 }
532 532 // sy_lfr_n_swf_p
533 533 if (flag == LFR_SUCCESSFUL)
534 534 {
535 535 if ( sy_lfr_n_swf_p < 16 )
536 536 {
537 537 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P+10, sy_lfr_n_swf_p );
538 538 flag = WRONG_APP_DATA;
539 539 }
540 540 }
541 541 // sy_lfr_n_bp_p0
542 542 if (flag == LFR_SUCCESSFUL)
543 543 {
544 544 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
545 545 {
546 546 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0+10, sy_lfr_n_bp_p0 );
547 547 flag = WRONG_APP_DATA;
548 548 }
549 549 }
550 550 // sy_lfr_n_asm_p
551 551 if (flag == LFR_SUCCESSFUL)
552 552 {
553 553 if (sy_lfr_n_asm_p == 0)
554 554 {
555 555 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
556 556 flag = WRONG_APP_DATA;
557 557 }
558 558 }
559 559 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
560 560 if (flag == LFR_SUCCESSFUL)
561 561 {
562 562 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
563 563 if (aux > FLOAT_EQUAL_ZERO)
564 564 {
565 565 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
566 566 flag = WRONG_APP_DATA;
567 567 }
568 568 }
569 569 // sy_lfr_n_bp_p1
570 570 if (flag == LFR_SUCCESSFUL)
571 571 {
572 572 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
573 573 {
574 574 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
575 575 flag = WRONG_APP_DATA;
576 576 }
577 577 }
578 578 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
579 579 if (flag == LFR_SUCCESSFUL)
580 580 {
581 581 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
582 582 if (aux > FLOAT_EQUAL_ZERO)
583 583 {
584 584 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
585 585 flag = LFR_DEFAULT;
586 586 }
587 587 }
588 588 // sy_lfr_n_cwf_long_f3
589 589
590 590 return flag;
591 591 }
592 592
593 593 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
594 594 {
595 595 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
596 596 *
597 597 * @param TC points to the TeleCommand packet that is being processed
598 598 * @param queue_id is the id of the queue which handles TM related to this execution step
599 599 *
600 600 */
601 601
602 602 int result;
603 603
604 604 result = LFR_SUCCESSFUL;
605 605
606 606 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
607 607 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
608 608
609 609 return result;
610 610 }
611 611
612 612 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
613 613 {
614 614 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
615 615 *
616 616 * @param TC points to the TeleCommand packet that is being processed
617 617 * @param queue_id is the id of the queue which handles TM related to this execution step
618 618 *
619 619 */
620 620
621 621 int result;
622 622
623 623 result = LFR_SUCCESSFUL;
624 624
625 625 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
626 626 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
627 627
628 628 return result;
629 629 }
630 630
631 631 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
632 632 {
633 633 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
634 634 *
635 635 * @param TC points to the TeleCommand packet that is being processed
636 636 * @param queue_id is the id of the queue which handles TM related to this execution step
637 637 *
638 638 */
639 639
640 640 int result;
641 641
642 642 result = LFR_SUCCESSFUL;
643 643
644 644 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
645 645 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
646 646
647 647 return result;
648 648 }
649 649
650 650 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
651 651 {
652 652 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
653 653 *
654 654 * @param TC points to the TeleCommand packet that is being processed
655 655 * @param queue_id is the id of the queue which handles TM related to this execution step
656 656 *
657 657 */
658 658
659 659 int status;
660 660
661 661 status = LFR_SUCCESSFUL;
662 662
663 663 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
664 664
665 665 return status;
666 666 }
667 667
668 668 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
669 669 {
670 670 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
671 671 *
672 672 * @param TC points to the TeleCommand packet that is being processed
673 673 * @param queue_id is the id of the queue which handles TM related to this execution step
674 674 *
675 675 */
676 676
677 677 int status;
678 678
679 679 status = LFR_SUCCESSFUL;
680 680
681 681 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
682 682
683 683 return status;
684 684 }
685 685
686 686 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
687 687 {
688 688 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
689 689 *
690 690 * @param TC points to the TeleCommand packet that is being processed
691 691 * @param queue_id is the id of the queue which handles TM related to this execution step
692 692 *
693 693 */
694 694
695 695 int status;
696 696
697 697 status = LFR_SUCCESSFUL;
698 698
699 699 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
700 700
701 701 return status;
702 702 }
703 703
704 704 //**********************
705 705 // BURST MODE PARAMETERS
706 706 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
707 707 {
708 708 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
709 709 *
710 710 * @param TC points to the TeleCommand packet that is being processed
711 711 * @param queue_id is the id of the queue which handles TM related to this execution step
712 712 *
713 713 */
714 714
715 715 int status;
716 716
717 717 status = LFR_SUCCESSFUL;
718 718
719 719 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
720 720
721 721 return status;
722 722 }
723 723
724 724 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
725 725 {
726 726 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
727 727 *
728 728 * @param TC points to the TeleCommand packet that is being processed
729 729 * @param queue_id is the id of the queue which handles TM related to this execution step
730 730 *
731 731 */
732 732
733 733 int status;
734 734
735 735 status = LFR_SUCCESSFUL;
736 736
737 737 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
738 738
739 739 return status;
740 740 }
741 741
742 742 //*********************
743 743 // SBM1 MODE PARAMETERS
744 744 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
745 745 {
746 746 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
747 747 *
748 748 * @param TC points to the TeleCommand packet that is being processed
749 749 * @param queue_id is the id of the queue which handles TM related to this execution step
750 750 *
751 751 */
752 752
753 753 int status;
754 754
755 755 status = LFR_SUCCESSFUL;
756 756
757 757 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
758 758
759 759 return status;
760 760 }
761 761
762 762 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
763 763 {
764 764 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
765 765 *
766 766 * @param TC points to the TeleCommand packet that is being processed
767 767 * @param queue_id is the id of the queue which handles TM related to this execution step
768 768 *
769 769 */
770 770
771 771 int status;
772 772
773 773 status = LFR_SUCCESSFUL;
774 774
775 775 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
776 776
777 777 return status;
778 778 }
779 779
780 780 //*********************
781 781 // SBM2 MODE PARAMETERS
782 782 int set_sy_lfr_s2_bp_p0(ccsdsTelecommandPacket_t *TC)
783 783 {
784 784 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
785 785 *
786 786 * @param TC points to the TeleCommand packet that is being processed
787 787 * @param queue_id is the id of the queue which handles TM related to this execution step
788 788 *
789 789 */
790 790
791 791 int status;
792 792
793 793 status = LFR_SUCCESSFUL;
794 794
795 795 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
796 796
797 797 return status;
798 798 }
799 799
800 800 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
801 801 {
802 802 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
803 803 *
804 804 * @param TC points to the TeleCommand packet that is being processed
805 805 * @param queue_id is the id of the queue which handles TM related to this execution step
806 806 *
807 807 */
808 808
809 809 int status;
810 810
811 811 status = LFR_SUCCESSFUL;
812 812
813 813 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
814 814
815 815 return status;
816 816 }
817 817
818 818 //*******************
819 819 // TC_LFR_UPDATE_INFO
820 820 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
821 821 {
822 822 unsigned int status;
823 823
824 824 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
825 825 || (mode == LFR_MODE_BURST)
826 826 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
827 827 {
828 828 status = LFR_SUCCESSFUL;
829 829 }
830 830 else
831 831 {
832 832 status = LFR_DEFAULT;
833 833 }
834 834
835 835 return status;
836 836 }
837 837
838 838 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
839 839 {
840 840 unsigned int status;
841 841
842 842 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
843 843 || (mode == TDS_MODE_BURST)
844 844 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
845 845 || (mode == TDS_MODE_LFM))
846 846 {
847 847 status = LFR_SUCCESSFUL;
848 848 }
849 849 else
850 850 {
851 851 status = LFR_DEFAULT;
852 852 }
853 853
854 854 return status;
855 855 }
856 856
857 857 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
858 858 {
859 859 unsigned int status;
860 860
861 861 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
862 862 || (mode == THR_MODE_BURST))
863 863 {
864 864 status = LFR_SUCCESSFUL;
865 865 }
866 866 else
867 867 {
868 868 status = LFR_DEFAULT;
869 869 }
870 870
871 871 return status;
872 872 }
873 873
874 874 //***********
875 875 // FBINS MASK
876 876
877 877 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
878 878 {
879 879 int status;
880 880 unsigned int k;
881 881 unsigned char *fbins_mask_dump;
882 882 unsigned char *fbins_mask_TC;
883 883
884 884 status = LFR_SUCCESSFUL;
885 885
886 886 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
887 887 fbins_mask_TC = TC->dataAndCRC;
888 888
889 889 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
890 890 {
891 891 fbins_mask_dump[k] = fbins_mask_TC[k];
892 892 }
893 893 for (k=0; k < NB_FBINS_MASKS; k++)
894 894 {
895 895 unsigned char *auxPtr;
896 896 auxPtr = &parameter_dump_packet.sy_lfr_fbins_f0_word1[k*NB_BYTES_PER_FBINS_MASK];
897 897 printf("%x %x %x %x\n", auxPtr[0], auxPtr[1], auxPtr[2], auxPtr[3]);
898 898 }
899 899
900 900
901 901 return status;
902 902 }
903 903
904 904 //**************
905 905 // KCOEFFICIENTS
906 906 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC )
907 907 {
908 908 unsigned int i;
909 909 unsigned short sy_lfr_kcoeff_frequency;
910 910 unsigned short bin;
911 911 unsigned short *freqPtr;
912 912 float *kcoeffPtr_norm;
913 913 float *kcoeffPtr_sbm;
914 914 int status;
915 915 unsigned char *kcoeffLoadPtr;
916 916 unsigned char *kcoeffNormPtr;
917 unsigned char *kcoeffSbmPtr_a;
918 unsigned char *kcoeffSbmPtr_b;
917 919
918 920 status = LFR_SUCCESSFUL;
919 921
920 922 kcoeffPtr_norm = NULL;
921 923 kcoeffPtr_sbm = NULL;
922 924 bin = 0;
923 925
924 926 freqPtr = (unsigned short *) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY];
925 927 sy_lfr_kcoeff_frequency = *freqPtr;
926 928
927 929 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
928 930 {
929 931 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
930 932 }
931 933 else
932 934 {
933 935 if ( ( sy_lfr_kcoeff_frequency >= 0 )
934 936 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
935 937 {
936 938 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
937 939 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
938 940 bin = sy_lfr_kcoeff_frequency;
939 941 }
940 942 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
941 943 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
942 944 {
943 945 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
944 946 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
945 947 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
946 948 }
947 949 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
948 950 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
949 951 {
950 952 kcoeffPtr_norm = k_coeff_intercalib_f2;
951 953 kcoeffPtr_sbm = NULL;
952 954 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
953 955 }
954 956 }
955 957
956 958 if (kcoeffPtr_norm != NULL )
957 959 {
958 960 printf("freq = %d, bin = %d\n", sy_lfr_kcoeff_frequency, bin);
959 961 for (i=0; i<NB_K_COEFF_PER_BIN; i++)
960 962 {
961 963 // destination
962 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + i ];
964 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + i ];
965 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + i) * 2 ];
966 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + i) * 2 + 1 ];
963 967 // source
964 968 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * i];
965 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
969 // copy source to destination
970 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
971 copyFloatByChar( kcoeffSbmPtr_a, kcoeffLoadPtr );
972 copyFloatByChar( kcoeffSbmPtr_b, kcoeffLoadPtr );
966 973 }
967 974 }
968 975
969 976 return status;
970 977 }
971 978
972 979 void copyFloatByChar( unsigned char *destination, unsigned char *source )
973 980 {
974 981 destination[0] = source[0];
975 982 destination[1] = source[1];
976 983 destination[2] = source[2];
977 984 destination[3] = source[3];
978 985 }
979 986
980 987 //**********
981 988 // init dump
982 989
983 990 void init_parameter_dump( void )
984 991 {
985 992 /** This function initialize the parameter_dump_packet global variable with default values.
986 993 *
987 994 */
988 995
989 996 unsigned int k;
990 997
991 998 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
992 999 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
993 1000 parameter_dump_packet.reserved = CCSDS_RESERVED;
994 1001 parameter_dump_packet.userApplication = CCSDS_USER_APP;
995 1002 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);
996 1003 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
997 1004 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
998 1005 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
999 1006 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> 8);
1000 1007 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
1001 1008 // DATA FIELD HEADER
1002 1009 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1003 1010 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
1004 1011 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
1005 1012 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
1006 1013 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
1007 1014 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
1008 1015 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
1009 1016 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
1010 1017 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
1011 1018 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
1012 1019 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
1013 1020
1014 1021 //******************
1015 1022 // COMMON PARAMETERS
1016 1023 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
1017 1024 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
1018 1025
1019 1026 //******************
1020 1027 // NORMAL PARAMETERS
1021 1028 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> 8);
1022 1029 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1023 1030 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> 8);
1024 1031 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1025 1032 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> 8);
1026 1033 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1027 1034 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1028 1035 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1029 1036 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1030 1037
1031 1038 //*****************
1032 1039 // BURST PARAMETERS
1033 1040 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1034 1041 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1035 1042
1036 1043 //****************
1037 1044 // SBM1 PARAMETERS
1038 1045 parameter_dump_packet.sy_lfr_s1_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P0; // min value is 0.25 s for the period
1039 1046 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1040 1047
1041 1048 //****************
1042 1049 // SBM2 PARAMETERS
1043 1050 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1044 1051 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1045 1052
1046 1053 //************
1047 1054 // FBINS MASKS
1048 1055 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
1049 1056 {
1050 1057 parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = 0xff;
1051 1058 }
1052 1059 }
1053 1060
1054 1061 void init_kcoefficients_dump( void )
1055 1062 {
1056 1063 init_kcoefficients_dump_packet( &kcoefficients_dump_1, 1, 30 );
1057 1064 init_kcoefficients_dump_packet( &kcoefficients_dump_2, 2, 6 );
1058 1065
1059 1066 kcoefficient_node_1.previous = NULL;
1060 1067 kcoefficient_node_1.next = NULL;
1061 1068 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1062 1069 kcoefficient_node_1.coarseTime = 0x00;
1063 1070 kcoefficient_node_1.fineTime = 0x00;
1064 1071 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1065 1072 kcoefficient_node_1.status = 0x00;
1066 1073
1067 1074 kcoefficient_node_2.previous = NULL;
1068 1075 kcoefficient_node_2.next = NULL;
1069 1076 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1070 1077 kcoefficient_node_2.coarseTime = 0x00;
1071 1078 kcoefficient_node_2.fineTime = 0x00;
1072 1079 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1073 1080 kcoefficient_node_2.status = 0x00;
1074 1081 }
1075 1082
1076 1083 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1077 1084 {
1078 1085 unsigned int k;
1079 1086 unsigned int packetLength;
1080 1087
1081 1088 packetLength = blk_nr * 130 + 20 - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1082 1089
1083 1090 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1084 1091 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1085 1092 kcoefficients_dump->reserved = CCSDS_RESERVED;
1086 1093 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1087 1094 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);;
1088 1095 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;;
1089 1096 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1090 1097 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1091 1098 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> 8);
1092 1099 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1093 1100 // DATA FIELD HEADER
1094 1101 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1095 1102 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1096 1103 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1097 1104 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1098 1105 kcoefficients_dump->time[0] = 0x00;
1099 1106 kcoefficients_dump->time[1] = 0x00;
1100 1107 kcoefficients_dump->time[2] = 0x00;
1101 1108 kcoefficients_dump->time[3] = 0x00;
1102 1109 kcoefficients_dump->time[4] = 0x00;
1103 1110 kcoefficients_dump->time[5] = 0x00;
1104 1111 kcoefficients_dump->sid = SID_K_DUMP;
1105 1112
1106 1113 kcoefficients_dump->pkt_cnt = 2;
1107 1114 kcoefficients_dump->pkt_nr = pkt_nr;
1108 1115 kcoefficients_dump->blk_nr = blk_nr;
1109 1116
1110 1117 //******************
1111 1118 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1112 1119 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1113 1120 for (k=0; k<3900; k++)
1114 1121 {
1115 1122 kcoefficients_dump->kcoeff_blks[k] = 0x00;
1116 1123 }
1117 1124 }
1118 1125
1119 1126
1120 1127
Show file before | Show file after | Hide comments
@@ -1,1370 +1,1370
1 1 /** Functions and tasks related to waveform packet generation.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle waveforms, in snapshot or continuous format.\n
7 7 *
8 8 */
9 9
10 10 #include "wf_handler.h"
11 11
12 12 //***************
13 13 // waveform rings
14 14 // F0
15 15 ring_node waveform_ring_f0[NB_RING_NODES_F0];
16 16 ring_node *current_ring_node_f0;
17 17 ring_node *ring_node_to_send_swf_f0;
18 18 // F1
19 19 ring_node waveform_ring_f1[NB_RING_NODES_F1];
20 20 ring_node *current_ring_node_f1;
21 21 ring_node *ring_node_to_send_swf_f1;
22 22 ring_node *ring_node_to_send_cwf_f1;
23 23 // F2
24 24 ring_node waveform_ring_f2[NB_RING_NODES_F2];
25 25 ring_node *current_ring_node_f2;
26 26 ring_node *ring_node_to_send_swf_f2;
27 27 ring_node *ring_node_to_send_cwf_f2;
28 28 // F3
29 29 ring_node waveform_ring_f3[NB_RING_NODES_F3];
30 30 ring_node *current_ring_node_f3;
31 31 ring_node *ring_node_to_send_cwf_f3;
32 32 char wf_cont_f3_light[ (NB_SAMPLES_PER_SNAPSHOT) * NB_BYTES_CWF3_LIGHT_BLK ];
33 33
34 34 bool extractSWF = false;
35 35 bool swf_f0_ready = false;
36 36 bool swf_f1_ready = false;
37 37 bool swf_f2_ready = false;
38 38
39 39 int wf_snap_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ];
40 40 ring_node ring_node_wf_snap_extracted;
41 41
42 42 //*********************
43 43 // Interrupt SubRoutine
44 44
45 45 ring_node * getRingNodeToSendCWF( unsigned char frequencyChannel)
46 46 {
47 47 ring_node *node;
48 48
49 49 node = NULL;
50 50 switch ( frequencyChannel ) {
51 51 case 1:
52 52 node = ring_node_to_send_cwf_f1;
53 53 break;
54 54 case 2:
55 55 node = ring_node_to_send_cwf_f2;
56 56 break;
57 57 case 3:
58 58 node = ring_node_to_send_cwf_f3;
59 59 break;
60 60 default:
61 61 break;
62 62 }
63 63
64 64 return node;
65 65 }
66 66
67 67 ring_node * getRingNodeToSendSWF( unsigned char frequencyChannel)
68 68 {
69 69 ring_node *node;
70 70
71 71 node = NULL;
72 72 switch ( frequencyChannel ) {
73 73 case 0:
74 74 node = ring_node_to_send_swf_f0;
75 75 break;
76 76 case 1:
77 77 node = ring_node_to_send_swf_f1;
78 78 break;
79 79 case 2:
80 80 node = ring_node_to_send_swf_f2;
81 81 break;
82 82 default:
83 83 break;
84 84 }
85 85
86 86 return node;
87 87 }
88 88
89 89 void reset_extractSWF( void )
90 90 {
91 91 extractSWF = false;
92 92 swf_f0_ready = false;
93 93 swf_f1_ready = false;
94 94 swf_f2_ready = false;
95 95 }
96 96
97 97 inline void waveforms_isr_f3( void )
98 98 {
99 99 rtems_status_code spare_status;
100 100
101 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
102 102 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
103 103 { // in modes other than STANDBY and BURST, send the CWF_F3 data
104 104 //***
105 105 // F3
106 106 if ( (waveform_picker_regs->status & 0xc0) != 0x00 ) { // [1100 0000] check the f3 full bits
107 107 ring_node_to_send_cwf_f3 = current_ring_node_f3->previous;
108 108 current_ring_node_f3 = current_ring_node_f3->next;
109 109 if ((waveform_picker_regs->status & 0x40) == 0x40){ // [0100 0000] f3 buffer 0 is full
110 110 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_0_coarse_time;
111 111 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_0_fine_time;
112 112 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->buffer_address;
113 113 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008840; // [1000 1000 0100 0000]
114 114 }
115 115 else if ((waveform_picker_regs->status & 0x80) == 0x80){ // [1000 0000] f3 buffer 1 is full
116 116 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_1_coarse_time;
117 117 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_1_fine_time;
118 118 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address;
119 119 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008880; // [1000 1000 1000 0000]
120 120 }
121 121 if (rtems_event_send( Task_id[TASKID_CWF3], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
122 122 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
123 123 }
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 207 status = (waveform_picker_regs->status & 0x30) >> 4; // [0011 0000] get the status bits for f2
208 208
209 209
210 210 switch(status)
211 211 {
212 212 case 1:
213 213 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
214 214 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
215 215 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
216 216 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
217 217 current_ring_node_f2 = current_ring_node_f2->next;
218 218 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
219 219 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
220 220 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
221 221 }
222 222 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
223 223 break;
224 224 case 2:
225 225 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
226 226 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
227 227 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
228 228 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
229 229 current_ring_node_f2 = current_ring_node_f2->next;
230 230 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
231 231 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
232 232 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
233 233 }
234 234 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
235 235 break;
236 236 default:
237 237 break;
238 238 }
239 239 }
240 240
241 241 inline void waveforms_isr_sbm1( void )
242 242 {
243 243 rtems_status_code status;
244 244
245 245 //***
246 246 // F1
247 247 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bits
248 248 // (1) change the receiving buffer for the waveform picker
249 249 ring_node_to_send_cwf_f1 = current_ring_node_f1->previous;
250 250 current_ring_node_f1 = current_ring_node_f1->next;
251 251 if ( (waveform_picker_regs->status & 0x04) == 0x04)
252 252 {
253 253 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
254 254 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
255 255 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
256 256 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
257 257 }
258 258 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
259 259 {
260 260 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
261 261 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
262 262 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
263 263 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
264 264 }
265 265 // (2) send an event for the the CWF1 task for transmission (and snapshot extraction if needed)
266 266 status = rtems_event_send( Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_SBM1 );
267 267 }
268 268
269 269 //***
270 270 // F0
271 271 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) { // [0000 0011] check the f0 full bits
272 272 swf_f0_ready = true;
273 273 // change f0 buffer
274 274 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
275 275 current_ring_node_f0 = current_ring_node_f0->next;
276 276 if ( (waveform_picker_regs->status & 0x01) == 0x01)
277 277 {
278 278
279 279 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
280 280 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
281 281 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
282 282 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
283 283 }
284 284 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
285 285 {
286 286 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
287 287 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
288 288 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
289 289 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
290 290 }
291 291 }
292 292
293 293 //***
294 294 // F2
295 295 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bits
296 296 swf_f2_ready = true;
297 297 // change f2 buffer
298 298 ring_node_to_send_swf_f2 = current_ring_node_f2->previous;
299 299 current_ring_node_f2 = current_ring_node_f2->next;
300 300 if ( (waveform_picker_regs->status & 0x10) == 0x10)
301 301 {
302 302 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
303 303 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
304 304 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
305 305 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
306 306 }
307 307 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
308 308 {
309 309 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
310 310 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
311 311 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
312 312 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
313 313 }
314 314 }
315 315 }
316 316
317 317 inline void waveforms_isr_sbm2( void )
318 318 {
319 319 rtems_status_code status;
320 320
321 321 //***
322 322 // F2
323 323 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bit
324 324 // (1) change the receiving buffer for the waveform picker
325 325 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
326 326 ring_node_to_send_cwf_f2->sid = SID_SBM2_CWF_F2;
327 327 current_ring_node_f2 = current_ring_node_f2->next;
328 328 if ( (waveform_picker_regs->status & 0x10) == 0x10)
329 329 {
330 330 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
331 331 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
332 332 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
333 333 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
334 334 }
335 335 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
336 336 {
337 337 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
338 338 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
339 339 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
340 340 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
341 341 }
342 342 // (2) send an event for the waveforms transmission
343 343 status = rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_SBM2 );
344 344 }
345 345
346 346 //***
347 347 // F0
348 348 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) { // [0000 0011] check the f0 full bit
349 349 swf_f0_ready = true;
350 350 // change f0 buffer
351 351 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
352 352 current_ring_node_f0 = current_ring_node_f0->next;
353 353 if ( (waveform_picker_regs->status & 0x01) == 0x01)
354 354 {
355 355
356 356 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
357 357 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
358 358 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
359 359 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
360 360 }
361 361 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
362 362 {
363 363 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
364 364 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
365 365 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
366 366 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
367 367 }
368 368 }
369 369
370 370 //***
371 371 // F1
372 372 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bit
373 373 swf_f1_ready = true;
374 374 ring_node_to_send_swf_f1 = current_ring_node_f1->previous;
375 375 current_ring_node_f1 = current_ring_node_f1->next;
376 376 if ( (waveform_picker_regs->status & 0x04) == 0x04)
377 377 {
378 378 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
379 379 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
380 380 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
381 381 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
382 382 }
383 383 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
384 384 {
385 385 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
386 386 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
387 387 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
388 388 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
389 389 }
390 390 }
391 391 }
392 392
393 393 rtems_isr waveforms_isr( rtems_vector_number vector )
394 394 {
395 395 /** This is the interrupt sub routine called by the waveform picker core.
396 396 *
397 397 * This ISR launch different actions depending mainly on two pieces of information:
398 398 * 1. the values read in the registers of the waveform picker.
399 399 * 2. the current LFR mode.
400 400 *
401 401 */
402 402
403 403 // STATUS
404 404 // new error error buffer full
405 405 // 15 14 13 12 11 10 9 8
406 406 // f3 f2 f1 f0 f3 f2 f1 f0
407 407 //
408 408 // ready buffer
409 409 // 7 6 5 4 3 2 1 0
410 410 // f3_1 f3_0 f2_1 f2_0 f1_1 f1_0 f0_1 f0_0
411 411
412 412 rtems_status_code spare_status;
413 413
414 414 waveforms_isr_f3();
415 415
416 416 if ( (waveform_picker_regs->status & 0xff00) != 0x00) // [1111 1111 0000 0000] check the error bits
417 417 {
418 418 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_10 );
419 419 }
420 420
421 421 switch(lfrCurrentMode)
422 422 {
423 423 //********
424 424 // STANDBY
425 425 case(LFR_MODE_STANDBY):
426 426 break;
427 427
428 428 //******
429 429 // NORMAL
430 430 case(LFR_MODE_NORMAL):
431 431 waveforms_isr_normal();
432 432 break;
433 433
434 434 //******
435 435 // BURST
436 436 case(LFR_MODE_BURST):
437 437 waveforms_isr_burst();
438 438 break;
439 439
440 440 //*****
441 441 // SBM1
442 442 case(LFR_MODE_SBM1):
443 443 waveforms_isr_sbm1();
444 444 break;
445 445
446 446 //*****
447 447 // SBM2
448 448 case(LFR_MODE_SBM2):
449 449 waveforms_isr_sbm2();
450 450 break;
451 451
452 452 //********
453 453 // DEFAULT
454 454 default:
455 455 break;
456 456 }
457 457 }
458 458
459 459 //************
460 460 // RTEMS TASKS
461 461
462 462 rtems_task wfrm_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
463 463 {
464 464 /** This RTEMS task is dedicated to the transmission of snapshots of the NORMAL mode.
465 465 *
466 466 * @param unused is the starting argument of the RTEMS task
467 467 *
468 468 * The following data packets are sent by this task:
469 469 * - TM_LFR_SCIENCE_NORMAL_SWF_F0
470 470 * - TM_LFR_SCIENCE_NORMAL_SWF_F1
471 471 * - TM_LFR_SCIENCE_NORMAL_SWF_F2
472 472 *
473 473 */
474 474
475 475 rtems_event_set event_out;
476 476 rtems_id queue_id;
477 477 rtems_status_code status;
478 478 bool resynchronisationEngaged;
479 479 ring_node *ring_node_wf_snap_extracted_ptr;
480 480
481 481 ring_node_wf_snap_extracted_ptr = (ring_node *) &ring_node_wf_snap_extracted;
482 482
483 483 resynchronisationEngaged = false;
484 484
485 485 status = get_message_queue_id_send( &queue_id );
486 486 if (status != RTEMS_SUCCESSFUL)
487 487 {
488 488 PRINTF1("in WFRM *** ERR get_message_queue_id_send %d\n", status)
489 489 }
490 490
491 491 BOOT_PRINTF("in WFRM ***\n")
492 492
493 493 while(1){
494 494 // wait for an RTEMS_EVENT
495 495 rtems_event_receive(RTEMS_EVENT_MODE_NORMAL | RTEMS_EVENT_MODE_SBM1
496 496 | RTEMS_EVENT_MODE_SBM2 | RTEMS_EVENT_MODE_SBM2_WFRM,
497 497 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
498 498 if(resynchronisationEngaged == false)
499 499 { // engage resynchronisation
500 500 snapshot_resynchronization( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
501 501 resynchronisationEngaged = true;
502 502 }
503 503 else
504 504 { // reset delta_snapshot to the nominal value
505 505 PRINTF("no resynchronisation, reset delta_snapshot to the nominal value\n")
506 506 set_wfp_delta_snapshot();
507 507 resynchronisationEngaged = false;
508 508 }
509 509 //
510 510
511 511 if (event_out == RTEMS_EVENT_MODE_NORMAL)
512 512 {
513 513 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_NORMAL\n")
514 514 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
515 515 ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1;
516 516 ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2;
517 517 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
518 518 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) );
519 519 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) );
520 520 }
521 521 if (event_out == RTEMS_EVENT_MODE_SBM1)
522 522 {
523 523 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM1\n")
524 524 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
525 525 ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F1;
526 526 ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2;
527 527 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
528 528 status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) );
529 529 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) );
530 530 }
531 531 if (event_out == RTEMS_EVENT_MODE_SBM2)
532 532 {
533 533 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM2\n")
534 534 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
535 535 ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1;
536 536 ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F2;
537 537 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
538 538 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) );
539 539 status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) );
540 540 }
541 541 }
542 542 }
543 543
544 544 rtems_task cwf3_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
545 545 {
546 546 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f3.
547 547 *
548 548 * @param unused is the starting argument of the RTEMS task
549 549 *
550 550 * The following data packet is sent by this task:
551 551 * - TM_LFR_SCIENCE_NORMAL_CWF_F3
552 552 *
553 553 */
554 554
555 555 rtems_event_set event_out;
556 556 rtems_id queue_id;
557 557 rtems_status_code status;
558 558 ring_node ring_node_cwf3_light;
559 559
560 560 status = get_message_queue_id_send( &queue_id );
561 561 if (status != RTEMS_SUCCESSFUL)
562 562 {
563 563 PRINTF1("in CWF3 *** ERR get_message_queue_id_send %d\n", status)
564 564 }
565 565
566 566 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
567 567
568 568 // init the ring_node_cwf3_light structure
569 569 ring_node_cwf3_light.buffer_address = (int) wf_cont_f3_light;
570 570 ring_node_cwf3_light.coarseTime = 0x00;
571 571 ring_node_cwf3_light.fineTime = 0x00;
572 572 ring_node_cwf3_light.next = NULL;
573 573 ring_node_cwf3_light.previous = NULL;
574 574 ring_node_cwf3_light.sid = SID_NORM_CWF_F3;
575 575 ring_node_cwf3_light.status = 0x00;
576 576
577 577 BOOT_PRINTF("in CWF3 ***\n")
578 578
579 579 while(1){
580 580 // wait for an RTEMS_EVENT
581 581 rtems_event_receive( RTEMS_EVENT_0,
582 582 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
583 583 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
584 584 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode==LFR_MODE_SBM2) )
585 585 {
586 586 if ( (parameter_dump_packet.sy_lfr_n_cwf_long_f3 & 0x01) == 0x01)
587 587 {
588 588 PRINTF("send CWF_LONG_F3\n")
589 589 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
590 590 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf_f3, sizeof( ring_node* ) );
591 591 }
592 592 else
593 593 {
594 594 PRINTF("send CWF_F3 (light)\n")
595 595 send_waveform_CWF3_light( ring_node_to_send_cwf_f3, &ring_node_cwf3_light, queue_id );
596 596 }
597 597
598 598 }
599 599 else
600 600 {
601 601 PRINTF1("in CWF3 *** lfrCurrentMode is %d, no data will be sent\n", lfrCurrentMode)
602 602 }
603 603 }
604 604 }
605 605
606 606 rtems_task cwf2_task(rtems_task_argument argument) // ONLY USED IN BURST AND SBM2
607 607 {
608 608 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f2.
609 609 *
610 610 * @param unused is the starting argument of the RTEMS task
611 611 *
612 612 * The following data packet is sent by this function:
613 613 * - TM_LFR_SCIENCE_BURST_CWF_F2
614 614 * - TM_LFR_SCIENCE_SBM2_CWF_F2
615 615 *
616 616 */
617 617
618 618 rtems_event_set event_out;
619 619 rtems_id queue_id;
620 620 rtems_status_code status;
621 621 ring_node *ring_node_to_send;
622 622 unsigned long long int acquisitionTimeF0_asLong;
623 623
624 624 acquisitionTimeF0_asLong = 0x00;
625 625
626 626 status = get_message_queue_id_send( &queue_id );
627 627 if (status != RTEMS_SUCCESSFUL)
628 628 {
629 629 PRINTF1("in CWF2 *** ERR get_message_queue_id_send %d\n", status)
630 630 }
631 631
632 632 BOOT_PRINTF("in CWF2 ***\n")
633 633
634 634 while(1){
635 635 // wait for an RTEMS_EVENT
636 636 rtems_event_receive( RTEMS_EVENT_MODE_BURST | RTEMS_EVENT_MODE_SBM2,
637 637 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
638 638 ring_node_to_send = getRingNodeToSendCWF( 2 );
639 639 if (event_out == RTEMS_EVENT_MODE_BURST)
640 640 {
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 645 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
646 646 // launch snapshot extraction if needed
647 647 if (extractSWF == true)
648 648 {
649 649 ring_node_to_send_swf_f2 = ring_node_to_send_cwf_f2;
650 650 // extract the snapshot
651 651 build_snapshot_from_ring( ring_node_to_send_swf_f2, 2, acquisitionTimeF0_asLong );
652 652 // send the snapshot when built
653 653 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM2 );
654 654 extractSWF = false;
655 655 }
656 656 if (swf_f0_ready && swf_f1_ready)
657 657 {
658 658 extractSWF = true;
659 659 // record the acquition time of the fΓ  snapshot to use to build the snapshot at f2
660 660 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
661 661 swf_f0_ready = false;
662 662 swf_f1_ready = false;
663 663 }
664 664 }
665 665 }
666 666 }
667 667
668 668 rtems_task cwf1_task(rtems_task_argument argument) // ONLY USED IN SBM1
669 669 {
670 670 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f1.
671 671 *
672 672 * @param unused is the starting argument of the RTEMS task
673 673 *
674 674 * The following data packet is sent by this function:
675 675 * - TM_LFR_SCIENCE_SBM1_CWF_F1
676 676 *
677 677 */
678 678
679 679 rtems_event_set event_out;
680 680 rtems_id queue_id;
681 681 rtems_status_code status;
682 682
683 683 ring_node *ring_node_to_send_cwf;
684 684
685 685 status = get_message_queue_id_send( &queue_id );
686 686 if (status != RTEMS_SUCCESSFUL)
687 687 {
688 688 PRINTF1("in CWF1 *** ERR get_message_queue_id_send %d\n", status)
689 689 }
690 690
691 691 BOOT_PRINTF("in CWF1 ***\n")
692 692
693 693 while(1){
694 694 // wait for an RTEMS_EVENT
695 695 rtems_event_receive( RTEMS_EVENT_MODE_SBM1,
696 696 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
697 697 ring_node_to_send_cwf = getRingNodeToSendCWF( 1 );
698 698 ring_node_to_send_cwf_f1->sid = SID_SBM1_CWF_F1;
699 699 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
700 700 if (status != 0)
701 701 printf("cwf sending failed\n");
702 702 // launch snapshot extraction if needed
703 703 if (extractSWF == true)
704 704 {
705 705 ring_node_to_send_swf_f1 = ring_node_to_send_cwf;
706 706 // launch the snapshot extraction
707 707 status = rtems_event_send( Task_id[TASKID_SWBD], RTEMS_EVENT_MODE_SBM1 );
708 708 extractSWF = false;
709 709 }
710 710 if (swf_f0_ready == true)
711 711 {
712 712 extractSWF = true;
713 713 swf_f0_ready = false; // this step shall be executed only one time
714 714 }
715 715 if ((swf_f1_ready == true) && (swf_f2_ready == true)) // swf_f1 is ready after the extraction
716 716 {
717 717 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM1 );
718 718 swf_f1_ready = false;
719 719 swf_f2_ready = false;
720 720 }
721 721 }
722 722 }
723 723
724 724 rtems_task swbd_task(rtems_task_argument argument)
725 725 {
726 726 /** This RTEMS task is dedicated to the building of snapshots from different continuous waveforms buffers.
727 727 *
728 728 * @param unused is the starting argument of the RTEMS task
729 729 *
730 730 */
731 731
732 732 rtems_event_set event_out;
733 733 unsigned long long int acquisitionTimeF0_asLong;
734 734
735 735 acquisitionTimeF0_asLong = 0x00;
736 736
737 737 BOOT_PRINTF("in SWBD ***\n")
738 738
739 739 while(1){
740 740 // wait for an RTEMS_EVENT
741 741 rtems_event_receive( RTEMS_EVENT_MODE_SBM1 | RTEMS_EVENT_MODE_SBM2,
742 742 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
743 743 if (event_out == RTEMS_EVENT_MODE_SBM1)
744 744 {
745 745 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
746 746 build_snapshot_from_ring( ring_node_to_send_swf_f1, 1, acquisitionTimeF0_asLong );
747 747 swf_f1_ready = true; // the snapshot has been extracted and is ready to be sent
748 748 }
749 749 else
750 750 {
751 751 PRINTF1("in SWBD *** unexpected rtems event received %x\n", (int) event_out)
752 752 }
753 753 }
754 754 }
755 755
756 756 //******************
757 757 // general functions
758 758
759 759 void WFP_init_rings( void )
760 760 {
761 761 // F0 RING
762 762 init_ring( waveform_ring_f0, NB_RING_NODES_F0, wf_buffer_f0, WFRM_BUFFER );
763 763 // F1 RING
764 764 init_ring( waveform_ring_f1, NB_RING_NODES_F1, wf_buffer_f1, WFRM_BUFFER );
765 765 // F2 RING
766 766 init_ring( waveform_ring_f2, NB_RING_NODES_F2, wf_buffer_f2, WFRM_BUFFER );
767 767 // F3 RING
768 768 init_ring( waveform_ring_f3, NB_RING_NODES_F3, wf_buffer_f3, WFRM_BUFFER );
769 769
770 770 ring_node_wf_snap_extracted.buffer_address = (int) wf_snap_extracted;
771 771
772 772 DEBUG_PRINTF1("waveform_ring_f0 @%x\n", (unsigned int) waveform_ring_f0)
773 773 DEBUG_PRINTF1("waveform_ring_f1 @%x\n", (unsigned int) waveform_ring_f1)
774 774 DEBUG_PRINTF1("waveform_ring_f2 @%x\n", (unsigned int) waveform_ring_f2)
775 775 DEBUG_PRINTF1("waveform_ring_f3 @%x\n", (unsigned int) waveform_ring_f3)
776 776 DEBUG_PRINTF1("wf_buffer_f0 @%x\n", (unsigned int) wf_buffer_f0)
777 777 DEBUG_PRINTF1("wf_buffer_f1 @%x\n", (unsigned int) wf_buffer_f1)
778 778 DEBUG_PRINTF1("wf_buffer_f2 @%x\n", (unsigned int) wf_buffer_f2)
779 779 DEBUG_PRINTF1("wf_buffer_f3 @%x\n", (unsigned int) wf_buffer_f3)
780 780
781 781 }
782 782
783 783 void WFP_reset_current_ring_nodes( void )
784 784 {
785 785 current_ring_node_f0 = waveform_ring_f0[0].next;
786 786 current_ring_node_f1 = waveform_ring_f1[0].next;
787 787 current_ring_node_f2 = waveform_ring_f2[0].next;
788 788 current_ring_node_f3 = waveform_ring_f3[0].next;
789 789
790 790 ring_node_to_send_swf_f0 = waveform_ring_f0;
791 791 ring_node_to_send_swf_f1 = waveform_ring_f1;
792 792 ring_node_to_send_swf_f2 = waveform_ring_f2;
793 793
794 794 ring_node_to_send_cwf_f1 = waveform_ring_f1;
795 795 ring_node_to_send_cwf_f2 = waveform_ring_f2;
796 796 ring_node_to_send_cwf_f3 = waveform_ring_f3;
797 797 }
798 798
799 799 int send_waveform_CWF3_light( ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id )
800 800 {
801 801 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
802 802 *
803 803 * @param waveform points to the buffer containing the data that will be send.
804 804 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
805 805 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
806 806 * contain information to setup the transmission of the data packets.
807 807 *
808 808 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
809 809 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
810 810 *
811 811 */
812 812
813 813 unsigned int i;
814 814 int ret;
815 815 rtems_status_code status;
816 816
817 817 char *sample;
818 818 int *dataPtr;
819 819
820 820 ret = LFR_DEFAULT;
821 821
822 822 dataPtr = (int*) ring_node_to_send->buffer_address;
823 823
824 824 ring_node_cwf3_light->coarseTime = ring_node_to_send->coarseTime;
825 825 ring_node_cwf3_light->fineTime = ring_node_to_send->fineTime;
826 826
827 827 //**********************
828 828 // BUILD CWF3_light DATA
829 829 for ( i=0; i< NB_SAMPLES_PER_SNAPSHOT; i++)
830 830 {
831 831 sample = (char*) &dataPtr[ (i * NB_WORDS_SWF_BLK) ];
832 832 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) ] = sample[ 0 ];
833 833 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 1 ] = sample[ 1 ];
834 834 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 2 ] = sample[ 2 ];
835 835 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 3 ] = sample[ 3 ];
836 836 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 4 ] = sample[ 4 ];
837 837 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 5 ] = sample[ 5 ];
838 838 }
839 839
840 840 // SEND PACKET
841 841 status = rtems_message_queue_send( queue_id, &ring_node_cwf3_light, sizeof( ring_node* ) );
842 842 if (status != RTEMS_SUCCESSFUL) {
843 843 printf("%d-%d, ERR %d\n", SID_NORM_CWF_F3, i, (int) status);
844 844 ret = LFR_DEFAULT;
845 845 }
846 846
847 847 return ret;
848 848 }
849 849
850 850 void compute_acquisition_time( unsigned int coarseTime, unsigned int fineTime,
851 851 unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char * acquisitionTime )
852 852 {
853 853 unsigned long long int acquisitionTimeAsLong;
854 854 unsigned char localAcquisitionTime[6];
855 855 double deltaT;
856 856
857 857 deltaT = 0.;
858 858
859 859 localAcquisitionTime[0] = (unsigned char) ( coarseTime >> 24 );
860 860 localAcquisitionTime[1] = (unsigned char) ( coarseTime >> 16 );
861 861 localAcquisitionTime[2] = (unsigned char) ( coarseTime >> 8 );
862 862 localAcquisitionTime[3] = (unsigned char) ( coarseTime );
863 863 localAcquisitionTime[4] = (unsigned char) ( fineTime >> 8 );
864 864 localAcquisitionTime[5] = (unsigned char) ( fineTime );
865 865
866 866 acquisitionTimeAsLong = ( (unsigned long long int) localAcquisitionTime[0] << 40 )
867 867 + ( (unsigned long long int) localAcquisitionTime[1] << 32 )
868 868 + ( (unsigned long long int) localAcquisitionTime[2] << 24 )
869 869 + ( (unsigned long long int) localAcquisitionTime[3] << 16 )
870 870 + ( (unsigned long long int) localAcquisitionTime[4] << 8 )
871 871 + ( (unsigned long long int) localAcquisitionTime[5] );
872 872
873 873 switch( sid )
874 874 {
875 875 case SID_NORM_SWF_F0:
876 876 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 24576. ;
877 877 break;
878 878
879 879 case SID_NORM_SWF_F1:
880 880 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 4096. ;
881 881 break;
882 882
883 883 case SID_NORM_SWF_F2:
884 884 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 256. ;
885 885 break;
886 886
887 887 case SID_SBM1_CWF_F1:
888 888 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 4096. ;
889 889 break;
890 890
891 891 case SID_SBM2_CWF_F2:
892 892 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
893 893 break;
894 894
895 895 case SID_BURST_CWF_F2:
896 896 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
897 897 break;
898 898
899 899 case SID_NORM_CWF_F3:
900 900 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF_SHORT_F3 * 65536. / 16. ;
901 901 break;
902 902
903 903 case SID_NORM_CWF_LONG_F3:
904 904 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 16. ;
905 905 break;
906 906
907 907 default:
908 908 PRINTF1("in compute_acquisition_time *** ERR unexpected sid %d\n", sid)
909 909 deltaT = 0.;
910 910 break;
911 911 }
912 912
913 913 acquisitionTimeAsLong = acquisitionTimeAsLong + (unsigned long long int) deltaT;
914 914 //
915 915 acquisitionTime[0] = (unsigned char) (acquisitionTimeAsLong >> 40);
916 916 acquisitionTime[1] = (unsigned char) (acquisitionTimeAsLong >> 32);
917 917 acquisitionTime[2] = (unsigned char) (acquisitionTimeAsLong >> 24);
918 918 acquisitionTime[3] = (unsigned char) (acquisitionTimeAsLong >> 16);
919 919 acquisitionTime[4] = (unsigned char) (acquisitionTimeAsLong >> 8 );
920 920 acquisitionTime[5] = (unsigned char) (acquisitionTimeAsLong );
921 921
922 922 }
923 923
924 924 void build_snapshot_from_ring( ring_node *ring_node_to_send, unsigned char frequencyChannel, unsigned long long int acquisitionTimeF0_asLong )
925 925 {
926 926 unsigned int i;
927 927 unsigned long long int centerTime_asLong;
928 928 unsigned long long int acquisitionTime_asLong;
929 929 unsigned long long int bufferAcquisitionTime_asLong;
930 930 unsigned char *ptr1;
931 931 unsigned char *ptr2;
932 932 unsigned char *timeCharPtr;
933 933 unsigned char nb_ring_nodes;
934 934 unsigned long long int frequency_asLong;
935 935 unsigned long long int nbTicksPerSample_asLong;
936 936 unsigned long long int nbSamplesPart1_asLong;
937 937 unsigned long long int sampleOffset_asLong;
938 938
939 939 unsigned int deltaT_F0;
940 940 unsigned int deltaT_F1;
941 941 unsigned long long int deltaT_F2;
942 942
943 943 deltaT_F0 = 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
944 944 deltaT_F1 = 16384; // (2048. / 4096. / 2.) * 65536. = 16384;
945 945 deltaT_F2 = 262144; // (2048. / 256. / 2.) * 65536. = 262144;
946 946 sampleOffset_asLong = 0x00;
947 947
948 948 // (1) get the f0 acquisition time => the value is passed in argument
949 949
950 950 // (2) compute the central reference time
951 951 centerTime_asLong = acquisitionTimeF0_asLong + deltaT_F0;
952 952
953 953 // (3) compute the acquisition time of the current snapshot
954 954 switch(frequencyChannel)
955 955 {
956 956 case 1: // 1 is for F1 = 4096 Hz
957 957 acquisitionTime_asLong = centerTime_asLong - deltaT_F1;
958 958 nb_ring_nodes = NB_RING_NODES_F1;
959 959 frequency_asLong = 4096;
960 960 nbTicksPerSample_asLong = 16; // 65536 / 4096;
961 961 break;
962 962 case 2: // 2 is for F2 = 256 Hz
963 963 acquisitionTime_asLong = centerTime_asLong - deltaT_F2;
964 964 nb_ring_nodes = NB_RING_NODES_F2;
965 965 frequency_asLong = 256;
966 966 nbTicksPerSample_asLong = 256; // 65536 / 256;
967 967 break;
968 968 default:
969 969 acquisitionTime_asLong = centerTime_asLong;
970 970 frequency_asLong = 256;
971 971 nbTicksPerSample_asLong = 256;
972 972 break;
973 973 }
974 974
975 975 //****************************************************************************
976 976 // (4) search the ring_node with the acquisition time <= acquisitionTime_asLong
977 977 for (i=0; i<nb_ring_nodes; i++)
978 978 {
979 979 PRINTF1("%d ... ", i)
980 980 bufferAcquisitionTime_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send->coarseTime );
981 981 if (bufferAcquisitionTime_asLong <= acquisitionTime_asLong)
982 982 {
983 983 PRINTF1("buffer found with acquisition time = %llx\n", bufferAcquisitionTime_asLong)
984 984 break;
985 985 }
986 986 ring_node_to_send = ring_node_to_send->previous;
987 987 }
988 988
989 989 // (5) compute the number of samples to take in the current buffer
990 990 sampleOffset_asLong = ((acquisitionTime_asLong - bufferAcquisitionTime_asLong) * frequency_asLong ) >> 16;
991 991 nbSamplesPart1_asLong = NB_SAMPLES_PER_SNAPSHOT - sampleOffset_asLong;
992 992 PRINTF2("sampleOffset_asLong = %lld, nbSamplesPart1_asLong = %lld\n", sampleOffset_asLong, nbSamplesPart1_asLong)
993 993
994 994 // (6) compute the final acquisition time
995 995 acquisitionTime_asLong = bufferAcquisitionTime_asLong +
996 996 sampleOffset_asLong * nbTicksPerSample_asLong;
997 997
998 998 // (7) copy the acquisition time at the beginning of the extrated snapshot
999 999 ptr1 = (unsigned char*) &acquisitionTime_asLong;
1000 1000 // fine time
1001 1001 ptr2 = (unsigned char*) &ring_node_wf_snap_extracted.fineTime;
1002 1002 ptr2[2] = ptr1[ 4 + 2 ];
1003 1003 ptr2[3] = ptr1[ 5 + 2 ];
1004 1004 // coarse time
1005 1005 ptr2 = (unsigned char*) &ring_node_wf_snap_extracted.coarseTime;
1006 1006 ptr2[0] = ptr1[ 0 + 2 ];
1007 1007 ptr2[1] = ptr1[ 1 + 2 ];
1008 1008 ptr2[2] = ptr1[ 2 + 2 ];
1009 1009 ptr2[3] = ptr1[ 3 + 2 ];
1010 1010
1011 1011 // re set the synchronization bit
1012 1012 timeCharPtr = (unsigned char*) &ring_node_to_send->coarseTime;
1013 1013 ptr2[0] = ptr2[0] | (timeCharPtr[0] & 0x80); // [1000 0000]
1014 1014
1015 1015 if ( (nbSamplesPart1_asLong >= NB_SAMPLES_PER_SNAPSHOT) | (nbSamplesPart1_asLong < 0) )
1016 1016 {
1017 1017 nbSamplesPart1_asLong = 0;
1018 1018 }
1019 1019 // copy the part 1 of the snapshot in the extracted buffer
1020 1020 for ( i = 0; i < (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i++ )
1021 1021 {
1022 1022 wf_snap_extracted[i] =
1023 1023 ((int*) ring_node_to_send->buffer_address)[ i + (sampleOffset_asLong * NB_WORDS_SWF_BLK) ];
1024 1024 }
1025 1025 // copy the part 2 of the snapshot in the extracted buffer
1026 1026 ring_node_to_send = ring_node_to_send->next;
1027 1027 for ( i = (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i < (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK); i++ )
1028 1028 {
1029 1029 wf_snap_extracted[i] =
1030 1030 ((int*) ring_node_to_send->buffer_address)[ (i-(nbSamplesPart1_asLong * NB_WORDS_SWF_BLK)) ];
1031 1031 }
1032 1032 }
1033 1033
1034 1034 void snapshot_resynchronization( unsigned char *timePtr )
1035 1035 {
1036 1036 unsigned long long int acquisitionTime;
1037 1037 unsigned long long int centerTime;
1038 1038 unsigned long long int previousTick;
1039 1039 unsigned long long int nextTick;
1040 1040 unsigned long long int deltaPreviousTick;
1041 1041 unsigned long long int deltaNextTick;
1042 1042 unsigned int deltaTickInF2;
1043 1043 double deltaPrevious;
1044 1044 double deltaNext;
1045 1045
1046 1046 acquisitionTime = get_acquisition_time( timePtr );
1047 1047
1048 1048 // compute center time
1049 1049 centerTime = acquisitionTime + 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
1050 1050 previousTick = centerTime - (centerTime & 0xffff);
1051 1051 nextTick = previousTick + 65536;
1052 1052
1053 1053 deltaPreviousTick = centerTime - previousTick;
1054 1054 deltaNextTick = nextTick - centerTime;
1055 1055
1056 1056 deltaPrevious = ((double) deltaPreviousTick) / 65536. * 1000.;
1057 1057 deltaNext = ((double) deltaNextTick) / 65536. * 1000.;
1058 1058
1059 1059 PRINTF2("delta previous = %f ms, delta next = %f ms\n", deltaPrevious, deltaNext)
1060 1060 PRINTF2("delta previous = %llu, delta next = %llu\n", deltaPreviousTick, deltaNextTick)
1061 1061
1062 1062 // which tick is the closest
1063 1063 if (deltaPreviousTick > deltaNextTick)
1064 1064 {
1065 1065 deltaTickInF2 = floor( (deltaNext * 256. / 1000.) ); // the division by 2 is important here
1066 1066 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot + deltaTickInF2;
1067 1067 printf("correction of = + %u\n", deltaTickInF2);
1068 1068 }
1069 1069 else
1070 1070 {
1071 1071 deltaTickInF2 = floor( (deltaPrevious * 256. / 1000.) ); // the division by 2 is important here
1072 1072 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot - deltaTickInF2;
1073 1073 printf("correction of = - %u\n", deltaTickInF2);
1074 1074 }
1075 1075 }
1076 1076
1077 1077 //**************
1078 1078 // wfp registers
1079 1079 void reset_wfp_burst_enable( void )
1080 1080 {
1081 1081 /** This function resets the waveform picker burst_enable register.
1082 1082 *
1083 1083 * The burst bits [f2 f1 f0] and the enable bits [f3 f2 f1 f0] are set to 0.
1084 1084 *
1085 1085 */
1086 1086
1087 1087 // [1000 000] burst f2, f1, f0 enable f3, f2, f1, f0
1088 1088 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable & 0x80;
1089 1089 }
1090 1090
1091 1091 void reset_wfp_status( void )
1092 1092 {
1093 1093 /** This function resets the waveform picker status register.
1094 1094 *
1095 1095 * All status bits are set to 0 [new_err full_err full].
1096 1096 *
1097 1097 */
1098 1098
1099 1099 waveform_picker_regs->status = 0xffff;
1100 1100 }
1101 1101
1102 1102 void reset_wfp_buffer_addresses( void )
1103 1103 {
1104 1104 // F0
1105 1105 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->previous->buffer_address; // 0x08
1106 1106 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address; // 0x0c
1107 1107 // F1
1108 1108 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->previous->buffer_address; // 0x10
1109 1109 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address; // 0x14
1110 1110 // F2
1111 1111 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->previous->buffer_address; // 0x18
1112 1112 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; // 0x1c
1113 1113 // F3
1114 1114 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->previous->buffer_address; // 0x20
1115 1115 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address; // 0x24
1116 1116 }
1117 1117
1118 1118 void reset_waveform_picker_regs( void )
1119 1119 {
1120 1120 /** This function resets the waveform picker module registers.
1121 1121 *
1122 1122 * The registers affected by this function are located at the following offset addresses:
1123 1123 * - 0x00 data_shaping
1124 1124 * - 0x04 run_burst_enable
1125 1125 * - 0x08 addr_data_f0
1126 1126 * - 0x0C addr_data_f1
1127 1127 * - 0x10 addr_data_f2
1128 1128 * - 0x14 addr_data_f3
1129 1129 * - 0x18 status
1130 1130 * - 0x1C delta_snapshot
1131 1131 * - 0x20 delta_f0
1132 1132 * - 0x24 delta_f0_2
1133 1133 * - 0x28 delta_f1
1134 1134 * - 0x2c delta_f2
1135 1135 * - 0x30 nb_data_by_buffer
1136 1136 * - 0x34 nb_snapshot_param
1137 1137 * - 0x38 start_date
1138 1138 * - 0x3c nb_word_in_buffer
1139 1139 *
1140 1140 */
1141 1141
1142 1142 set_wfp_data_shaping(); // 0x00 *** R1 R0 SP1 SP0 BW
1143 1143
1144 1144 reset_wfp_burst_enable(); // 0x04 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
1145 1145
1146 1146 reset_wfp_buffer_addresses();
1147 1147
1148 1148 reset_wfp_status(); // 0x18
1149 1149
1150 1150 set_wfp_delta_snapshot(); // 0x1c *** 300 s => 0x12bff
1151 1151
1152 1152 set_wfp_delta_f0_f0_2(); // 0x20, 0x24
1153 1153
1154 1154 set_wfp_delta_f1(); // 0x28
1155 1155
1156 1156 set_wfp_delta_f2(); // 0x2c
1157 1157
1158 1158 DEBUG_PRINTF1("delta_snapshot %x\n", waveform_picker_regs->delta_snapshot)
1159 1159 DEBUG_PRINTF1("delta_f0 %x\n", waveform_picker_regs->delta_f0)
1160 1160 DEBUG_PRINTF1("delta_f0_2 %x\n", waveform_picker_regs->delta_f0_2)
1161 1161 DEBUG_PRINTF1("delta_f1 %x\n", waveform_picker_regs->delta_f1)
1162 1162 DEBUG_PRINTF1("delta_f2 %x\n", waveform_picker_regs->delta_f2)
1163 1163 // 2688 = 8 * 336
1164 1164 waveform_picker_regs->nb_data_by_buffer = 0xa7f; // 0x30 *** 2688 - 1 => nb samples -1
1165 1165 waveform_picker_regs->snapshot_param = 0xa80; // 0x34 *** 2688 => nb samples
1166 1166 waveform_picker_regs->start_date = 0x7fffffff; // 0x38
1167 1167 //
1168 1168 // coarse time and fine time registers are not initialized, they are volatile
1169 1169 //
1170 1170 waveform_picker_regs->buffer_length = 0x1f8;// buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
1171 1171 }
1172 1172
1173 1173 void set_wfp_data_shaping( void )
1174 1174 {
1175 1175 /** This function sets the data_shaping register of the waveform picker module.
1176 1176 *
1177 1177 * The value is read from one field of the parameter_dump_packet structure:\n
1178 1178 * bw_sp0_sp1_r0_r1
1179 1179 *
1180 1180 */
1181 1181
1182 1182 unsigned char data_shaping;
1183 1183
1184 1184 // get the parameters for the data shaping [BW SP0 SP1 R0 R1] in sy_lfr_common1 and configure the register
1185 1185 // waveform picker : [R1 R0 SP1 SP0 BW]
1186 1186
1187 1187 data_shaping = parameter_dump_packet.sy_lfr_common_parameters;
1188 1188
1189 1189 waveform_picker_regs->data_shaping =
1190 ( (data_shaping & 0x10) >> 4 ) // BW
1191 + ( (data_shaping & 0x08) >> 2 ) // SP0
1192 + ( (data_shaping & 0x04) ) // SP1
1193 + ( (data_shaping & 0x02) << 2 ) // R0
1194 + ( (data_shaping & 0x01) << 4 ) // R1
1190 ( (data_shaping & 0x20) >> 5 ) // BW
1191 + ( (data_shaping & 0x10) >> 3 ) // SP0
1192 + ( (data_shaping & 0x08) >> 1 ) // SP1
1193 + ( (data_shaping & 0x04) ) // R0
1194 + ( (data_shaping & 0x02) << 3 ) // R1
1195 1195 + ( (data_shaping & 0x01) << 5 ); // R2
1196 1196 }
1197 1197
1198 1198 void set_wfp_burst_enable_register( unsigned char mode )
1199 1199 {
1200 1200 /** This function sets the waveform picker burst_enable register depending on the mode.
1201 1201 *
1202 1202 * @param mode is the LFR mode to launch.
1203 1203 *
1204 1204 * The burst bits shall be before the enable bits.
1205 1205 *
1206 1206 */
1207 1207
1208 1208 // [0000 0000] burst f2, f1, f0 enable f3 f2 f1 f0
1209 1209 // the burst bits shall be set first, before the enable bits
1210 1210 switch(mode) {
1211 1211 case(LFR_MODE_NORMAL):
1212 1212 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enable
1213 1213 waveform_picker_regs->run_burst_enable = 0x0f; // [0000 1111] enable f3 f2 f1 f0
1214 1214 break;
1215 1215 case(LFR_MODE_BURST):
1216 1216 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1217 1217 // waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x04; // [0100] enable f2
1218 1218 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0c; // [1100] enable f3 AND f2
1219 1219 break;
1220 1220 case(LFR_MODE_SBM1):
1221 1221 waveform_picker_regs->run_burst_enable = 0x20; // [0010 0000] f1 burst enabled
1222 1222 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1223 1223 break;
1224 1224 case(LFR_MODE_SBM2):
1225 1225 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1226 1226 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1227 1227 break;
1228 1228 default:
1229 1229 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enabled, no waveform enabled
1230 1230 break;
1231 1231 }
1232 1232 }
1233 1233
1234 1234 void set_wfp_delta_snapshot( void )
1235 1235 {
1236 1236 /** This function sets the delta_snapshot register of the waveform picker module.
1237 1237 *
1238 1238 * The value is read from two (unsigned char) of the parameter_dump_packet structure:
1239 1239 * - sy_lfr_n_swf_p[0]
1240 1240 * - sy_lfr_n_swf_p[1]
1241 1241 *
1242 1242 */
1243 1243
1244 1244 unsigned int delta_snapshot;
1245 1245 unsigned int delta_snapshot_in_T2;
1246 1246
1247 1247 delta_snapshot = parameter_dump_packet.sy_lfr_n_swf_p[0]*256
1248 1248 + parameter_dump_packet.sy_lfr_n_swf_p[1];
1249 1249
1250 1250 delta_snapshot_in_T2 = delta_snapshot * 256;
1251 1251 waveform_picker_regs->delta_snapshot = delta_snapshot_in_T2 - 1; // max 4 bytes
1252 1252 }
1253 1253
1254 1254 void set_wfp_delta_f0_f0_2( void )
1255 1255 {
1256 1256 unsigned int delta_snapshot;
1257 1257 unsigned int nb_samples_per_snapshot;
1258 1258 float delta_f0_in_float;
1259 1259
1260 1260 delta_snapshot = waveform_picker_regs->delta_snapshot;
1261 1261 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1262 1262 delta_f0_in_float =nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 24576.) * 256.;
1263 1263
1264 1264 waveform_picker_regs->delta_f0 = delta_snapshot - floor( delta_f0_in_float );
1265 1265 waveform_picker_regs->delta_f0_2 = 0x30; // 48 = 11 0000, max 7 bits
1266 1266 }
1267 1267
1268 1268 void set_wfp_delta_f1( void )
1269 1269 {
1270 1270 unsigned int delta_snapshot;
1271 1271 unsigned int nb_samples_per_snapshot;
1272 1272 float delta_f1_in_float;
1273 1273
1274 1274 delta_snapshot = waveform_picker_regs->delta_snapshot;
1275 1275 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1276 1276 delta_f1_in_float = nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 4096.) * 256.;
1277 1277
1278 1278 waveform_picker_regs->delta_f1 = delta_snapshot - floor( delta_f1_in_float );
1279 1279 }
1280 1280
1281 1281 void set_wfp_delta_f2()
1282 1282 {
1283 1283 unsigned int delta_snapshot;
1284 1284 unsigned int nb_samples_per_snapshot;
1285 1285
1286 1286 delta_snapshot = waveform_picker_regs->delta_snapshot;
1287 1287 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1288 1288
1289 1289 waveform_picker_regs->delta_f2 = delta_snapshot - nb_samples_per_snapshot / 2;
1290 1290 }
1291 1291
1292 1292 //*****************
1293 1293 // local parameters
1294 1294
1295 1295 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid )
1296 1296 {
1297 1297 /** This function increments the parameter "sequence_cnt" depending on the sid passed in argument.
1298 1298 *
1299 1299 * @param packet_sequence_control is a pointer toward the parameter sequence_cnt to update.
1300 1300 * @param sid is the source identifier of the packet being updated.
1301 1301 *
1302 1302 * REQ-LFR-SRS-5240 / SSS-CP-FS-590
1303 1303 * The sequence counters shall wrap around from 2^14 to zero.
1304 1304 * The sequence counter shall start at zero at startup.
1305 1305 *
1306 1306 * REQ-LFR-SRS-5239 / SSS-CP-FS-580
1307 1307 * All TM_LFR_SCIENCE_ packets are sent to ground, i.e. destination id = 0
1308 1308 *
1309 1309 */
1310 1310
1311 1311 unsigned short *sequence_cnt;
1312 1312 unsigned short segmentation_grouping_flag;
1313 1313 unsigned short new_packet_sequence_control;
1314 1314 rtems_mode initial_mode_set;
1315 1315 rtems_mode current_mode_set;
1316 1316 rtems_status_code status;
1317 1317
1318 1318 //******************************************
1319 1319 // CHANGE THE MODE OF THE CALLING RTEMS TASK
1320 1320 status = rtems_task_mode( RTEMS_NO_PREEMPT, RTEMS_PREEMPT_MASK, &initial_mode_set );
1321 1321
1322 1322 if ( (sid == SID_NORM_SWF_F0) || (sid == SID_NORM_SWF_F1) || (sid == SID_NORM_SWF_F2)
1323 1323 || (sid == SID_NORM_CWF_F3) || (sid == SID_NORM_CWF_LONG_F3)
1324 1324 || (sid == SID_BURST_CWF_F2)
1325 1325 || (sid == SID_NORM_ASM_F0) || (sid == SID_NORM_ASM_F1) || (sid == SID_NORM_ASM_F2)
1326 1326 || (sid == SID_NORM_BP1_F0) || (sid == SID_NORM_BP1_F1) || (sid == SID_NORM_BP1_F2)
1327 1327 || (sid == SID_NORM_BP2_F0) || (sid == SID_NORM_BP2_F1) || (sid == SID_NORM_BP2_F2)
1328 1328 || (sid == SID_BURST_BP1_F0) || (sid == SID_BURST_BP2_F0)
1329 1329 || (sid == SID_BURST_BP1_F1) || (sid == SID_BURST_BP2_F1) )
1330 1330 {
1331 1331 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_NORMAL_BURST;
1332 1332 }
1333 1333 else if ( (sid ==SID_SBM1_CWF_F1) || (sid ==SID_SBM2_CWF_F2)
1334 1334 || (sid == SID_SBM1_BP1_F0) || (sid == SID_SBM1_BP2_F0)
1335 1335 || (sid == SID_SBM2_BP1_F0) || (sid == SID_SBM2_BP2_F0)
1336 1336 || (sid == SID_SBM2_BP1_F1) || (sid == SID_SBM2_BP2_F1) )
1337 1337 {
1338 1338 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_SBM1_SBM2;
1339 1339 }
1340 1340 else
1341 1341 {
1342 1342 sequence_cnt = (unsigned short *) NULL;
1343 1343 PRINTF1("in increment_seq_counter_source_id *** ERR apid_destid %d not known\n", sid)
1344 1344 }
1345 1345
1346 1346 if (sequence_cnt != NULL)
1347 1347 {
1348 1348 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
1349 1349 *sequence_cnt = (*sequence_cnt) & 0x3fff;
1350 1350
1351 1351 new_packet_sequence_control = segmentation_grouping_flag | (*sequence_cnt) ;
1352 1352
1353 1353 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
1354 1354 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1355 1355
1356 1356 // increment the sequence counter
1357 1357 if ( *sequence_cnt < SEQ_CNT_MAX)
1358 1358 {
1359 1359 *sequence_cnt = *sequence_cnt + 1;
1360 1360 }
1361 1361 else
1362 1362 {
1363 1363 *sequence_cnt = 0;
1364 1364 }
1365 1365 }
1366 1366
1367 1367 //***********************************
1368 1368 // RESET THE MODE OF THE CALLING TASK
1369 1369 status = rtems_task_mode( initial_mode_set, RTEMS_PREEMPT_MASK, &current_mode_set );
1370 1370 }
General Comments 0
You need to be logged in to leave comments. Login now