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

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

Bug 117
paul -
r143:f2e3909b2300 VHDLib206
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,10 +1,11
1 1 syntax: glob
2 2 *.pdf
3 3 *~
4 4 *.o
5 *.zip
5 6 tests/*.err
6 7 doc
7 8 *.srec
8 9 FSW-qt/bin/fsw
9 10 src/LFR_basic-parameters
10 11
Show file before | Show file after | Hide comments
@@ -1,586 +1,586
1 1 /** General usage functions and RTEMS tasks.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 */
7 7
8 8 #include "fsw_misc.h"
9 9
10 10 void configure_timer(gptimer_regs_t *gptimer_regs, unsigned char timer, unsigned int clock_divider,
11 11 unsigned char interrupt_level, rtems_isr (*timer_isr)() )
12 12 {
13 13 /** This function configures a GPTIMER timer instantiated in the VHDL design.
14 14 *
15 15 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
16 16 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
17 17 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
18 18 * @param interrupt_level is the interrupt level that the timer drives.
19 19 * @param timer_isr is the interrupt subroutine that will be attached to the IRQ driven by the timer.
20 20 *
21 21 * Interrupt levels are described in the SPARC documentation sparcv8.pdf p.76
22 22 *
23 23 */
24 24
25 25 rtems_status_code status;
26 26 rtems_isr_entry old_isr_handler;
27 27
28 28 gptimer_regs->timer[timer].ctrl = 0x00; // reset the control register
29 29
30 30 status = rtems_interrupt_catch( timer_isr, interrupt_level, &old_isr_handler) ; // see sparcv8.pdf p.76 for interrupt levels
31 31 if (status!=RTEMS_SUCCESSFUL)
32 32 {
33 33 PRINTF("in configure_timer *** ERR rtems_interrupt_catch\n")
34 34 }
35 35
36 36 timer_set_clock_divider( gptimer_regs, timer, clock_divider);
37 37 }
38 38
39 39 void timer_start(gptimer_regs_t *gptimer_regs, unsigned char timer)
40 40 {
41 41 /** This function starts a GPTIMER timer.
42 42 *
43 43 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
44 44 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
45 45 *
46 46 */
47 47
48 48 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000010; // clear pending IRQ if any
49 49 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000004; // LD load value from the reload register
50 50 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000001; // EN enable the timer
51 51 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000002; // RS restart
52 52 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000008; // IE interrupt enable
53 53 }
54 54
55 55 void timer_stop(gptimer_regs_t *gptimer_regs, unsigned char timer)
56 56 {
57 57 /** This function stops a GPTIMER timer.
58 58 *
59 59 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
60 60 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
61 61 *
62 62 */
63 63
64 64 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & 0xfffffffe; // EN enable the timer
65 65 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & 0xffffffef; // IE interrupt enable
66 66 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000010; // clear pending IRQ if any
67 67 }
68 68
69 69 void timer_set_clock_divider(gptimer_regs_t *gptimer_regs, unsigned char timer, unsigned int clock_divider)
70 70 {
71 71 /** This function sets the clock divider of a GPTIMER timer.
72 72 *
73 73 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
74 74 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
75 75 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
76 76 *
77 77 */
78 78
79 79 gptimer_regs->timer[timer].reload = clock_divider; // base clock frequency is 1 MHz
80 80 }
81 81
82 82 int send_console_outputs_on_apbuart_port( void ) // Send the console outputs on the apbuart port
83 83 {
84 84 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
85 85
86 86 apbuart_regs->ctrl = APBUART_CTRL_REG_MASK_TE;
87 87
88 88 return 0;
89 89 }
90 90
91 91 int enable_apbuart_transmitter( void ) // set the bit 1, TE Transmitter Enable to 1 in the APBUART control register
92 92 {
93 93 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
94 94
95 95 apbuart_regs->ctrl = apbuart_regs->ctrl | APBUART_CTRL_REG_MASK_TE;
96 96
97 97 return 0;
98 98 }
99 99
100 100 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value)
101 101 {
102 102 /** This function sets the scaler reload register of the apbuart module
103 103 *
104 104 * @param regs is the address of the apbuart registers in memory
105 105 * @param value is the value that will be stored in the scaler register
106 106 *
107 107 * The value shall be set by the software to get data on the serial interface.
108 108 *
109 109 */
110 110
111 111 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) regs;
112 112
113 113 apbuart_regs->scaler = value;
114 114 BOOT_PRINTF1("OK *** apbuart port scaler reload register set to 0x%x\n", value)
115 115 }
116 116
117 117 //************
118 118 // RTEMS TASKS
119 119
120 120 rtems_task stat_task(rtems_task_argument argument)
121 121 {
122 122 int i;
123 123 int j;
124 124 i = 0;
125 125 j = 0;
126 126 BOOT_PRINTF("in STAT *** \n")
127 127 while(1){
128 128 rtems_task_wake_after(1000);
129 129 PRINTF1("%d\n", j)
130 130 if (i == CPU_USAGE_REPORT_PERIOD) {
131 131 // #ifdef PRINT_TASK_STATISTICS
132 132 // rtems_cpu_usage_report();
133 133 // rtems_cpu_usage_reset();
134 134 // #endif
135 135 i = 0;
136 136 }
137 137 else i++;
138 138 j++;
139 139 }
140 140 }
141 141
142 142 rtems_task hous_task(rtems_task_argument argument)
143 143 {
144 144 rtems_status_code status;
145 145 rtems_id queue_id;
146 146 rtems_rate_monotonic_period_status period_status;
147 147
148 148 status = get_message_queue_id_send( &queue_id );
149 149 if (status != RTEMS_SUCCESSFUL)
150 150 {
151 151 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
152 152 }
153 153
154 154 BOOT_PRINTF("in HOUS ***\n")
155 155
156 156 if (rtems_rate_monotonic_ident( name_hk_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
157 157 status = rtems_rate_monotonic_create( name_hk_rate_monotonic, &HK_id );
158 158 if( status != RTEMS_SUCCESSFUL ) {
159 159 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status )
160 160 }
161 161 }
162 162
163 163 housekeeping_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
164 164 housekeeping_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
165 165 housekeeping_packet.reserved = DEFAULT_RESERVED;
166 166 housekeeping_packet.userApplication = CCSDS_USER_APP;
167 167 housekeeping_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
168 168 housekeeping_packet.packetID[1] = (unsigned char) (APID_TM_HK);
169 169 housekeeping_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
170 170 housekeeping_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
171 171 housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
172 172 housekeeping_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
173 173 housekeeping_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
174 174 housekeeping_packet.serviceType = TM_TYPE_HK;
175 175 housekeeping_packet.serviceSubType = TM_SUBTYPE_HK;
176 176 housekeeping_packet.destinationID = TM_DESTINATION_ID_GROUND;
177 177 housekeeping_packet.sid = SID_HK;
178 178
179 179 status = rtems_rate_monotonic_cancel(HK_id);
180 180 if( status != RTEMS_SUCCESSFUL ) {
181 181 PRINTF1( "ERR *** in HOUS *** rtems_rate_monotonic_cancel(HK_id) ***code: %d\n", status )
182 182 }
183 183 else {
184 184 DEBUG_PRINTF("OK *** in HOUS *** rtems_rate_monotonic_cancel(HK_id)\n")
185 185 }
186 186
187 187 // startup phase
188 188 status = rtems_rate_monotonic_period( HK_id, SY_LFR_TIME_SYN_TIMEOUT_in_ticks );
189 189 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
190 190 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
191 191 while(period_status.state != RATE_MONOTONIC_EXPIRED ) // after SY_LFR_TIME_SYN_TIMEOUT ms, starts HK anyway
192 192 {
193 193 if ((time_management_regs->coarse_time & 0x80000000) == 0x00000000) // check time synchronization
194 194 {
195 195 break; // break if LFR is synchronized
196 196 }
197 197 else
198 198 {
199 199 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
200 200 // sched_yield();
201 201 status = rtems_task_wake_after( 10 ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 100 ms = 10 * 10 ms
202 202 }
203 203 }
204 204 status = rtems_rate_monotonic_cancel(HK_id);
205 205 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
206 206
207 207 while(1){ // launch the rate monotonic task
208 208 status = rtems_rate_monotonic_period( HK_id, HK_PERIOD );
209 209 if ( status != RTEMS_SUCCESSFUL ) {
210 210 PRINTF1( "in HOUS *** ERR period: %d\n", status);
211 211 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_6 );
212 212 }
213 213 else {
214 214 increment_seq_counter( housekeeping_packet.packetSequenceControl );
215 215 housekeeping_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
216 216 housekeeping_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
217 217 housekeeping_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
218 218 housekeeping_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
219 219 housekeeping_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
220 220 housekeeping_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
221 221
222 222 spacewire_update_statistics();
223 223
224 224 get_v_e1_e2_f3( housekeeping_packet.hk_lfr_sc_v_f3 );
225 225 get_cpu_load( (unsigned char *) &housekeeping_packet.hk_lfr_cpu_load );
226 226
227 227 // SEND PACKET
228 228 status = rtems_message_queue_urgent( queue_id, &housekeeping_packet,
229 229 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
230 230 if (status != RTEMS_SUCCESSFUL) {
231 231 PRINTF1("in HOUS *** ERR send: %d\n", status)
232 232 }
233 233 }
234 234 }
235 235
236 236 PRINTF("in HOUS *** deleting task\n")
237 237
238 238 status = rtems_task_delete( RTEMS_SELF ); // should not return
239 239 printf( "rtems_task_delete returned with status of %d.\n", status );
240 240 return;
241 241 }
242 242
243 243 rtems_task dumb_task( rtems_task_argument unused )
244 244 {
245 245 /** This RTEMS taks is used to print messages without affecting the general behaviour of the software.
246 246 *
247 247 * @param unused is the starting argument of the RTEMS task
248 248 *
249 249 * The DUMB taks waits for RTEMS events and print messages depending on the incoming events.
250 250 *
251 251 */
252 252
253 253 unsigned int i;
254 254 unsigned int intEventOut;
255 255 unsigned int coarse_time = 0;
256 256 unsigned int fine_time = 0;
257 257 rtems_event_set event_out;
258 258
259 259 char *DumbMessages[10] = {"in DUMB *** default", // RTEMS_EVENT_0
260 260 "in DUMB *** timecode_irq_handler", // RTEMS_EVENT_1
261 261 "in DUMB *** f3 buffer changed", // RTEMS_EVENT_2
262 262 "in DUMB *** in SMIQ *** Error sending event to AVF0", // RTEMS_EVENT_3
263 263 "in DUMB *** spectral_matrices_isr *** Error sending event to SMIQ", // RTEMS_EVENT_4
264 264 "in DUMB *** waveforms_simulator_isr", // RTEMS_EVENT_5
265 265 "ERR HK", // RTEMS_EVENT_6
266 266 "ready for dump", // RTEMS_EVENT_7
267 267 "in DUMB *** spectral_matrices_isr", // RTEMS_EVENT_8
268 268 "tick" // RTEMS_EVENT_9
269 269 };
270 270
271 271 BOOT_PRINTF("in DUMB *** \n")
272 272
273 273 while(1){
274 274 rtems_event_receive(RTEMS_EVENT_0 | RTEMS_EVENT_1 | RTEMS_EVENT_2 | RTEMS_EVENT_3
275 275 | RTEMS_EVENT_4 | RTEMS_EVENT_5 | RTEMS_EVENT_6 | RTEMS_EVENT_7
276 276 | RTEMS_EVENT_8 | RTEMS_EVENT_9,
277 277 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT
278 278 intEventOut = (unsigned int) event_out;
279 279 for ( i=0; i<32; i++)
280 280 {
281 281 if ( ((intEventOut >> i) & 0x0001) != 0)
282 282 {
283 283 coarse_time = time_management_regs->coarse_time;
284 284 fine_time = time_management_regs->fine_time;
285 285 printf("in DUMB *** coarse: %x, fine: %x, %s\n", coarse_time, fine_time, DumbMessages[i]);
286 286 if (i==8)
287 287 {
288 288 PRINTF1("status = %x\n", spectral_matrix_regs->status)
289 289 }
290 290 }
291 291 }
292 292 }
293 293 }
294 294
295 295 //*****************************
296 296 // init housekeeping parameters
297 297
298 298 void init_housekeeping_parameters( void )
299 299 {
300 300 /** This function initialize the housekeeping_packet global variable with default values.
301 301 *
302 302 */
303 303
304 304 unsigned int i = 0;
305 305 unsigned char *parameters;
306 306
307 307 parameters = (unsigned char*) &housekeeping_packet.lfr_status_word;
308 308 for(i = 0; i< SIZE_HK_PARAMETERS; i++)
309 309 {
310 310 parameters[i] = 0x00;
311 311 }
312 312 // init status word
313 313 housekeeping_packet.lfr_status_word[0] = DEFAULT_STATUS_WORD_BYTE0;
314 314 housekeeping_packet.lfr_status_word[1] = DEFAULT_STATUS_WORD_BYTE1;
315 315 // init software version
316 316 housekeeping_packet.lfr_sw_version[0] = SW_VERSION_N1;
317 317 housekeeping_packet.lfr_sw_version[1] = SW_VERSION_N2;
318 318 housekeeping_packet.lfr_sw_version[2] = SW_VERSION_N3;
319 319 housekeeping_packet.lfr_sw_version[3] = SW_VERSION_N4;
320 320 // init fpga version
321 321 parameters = (unsigned char *) (REGS_ADDR_WAVEFORM_PICKER + 0xb0);
322 322 housekeeping_packet.lfr_fpga_version[0] = parameters[1]; // n1
323 323 housekeeping_packet.lfr_fpga_version[1] = parameters[2]; // n2
324 324 housekeeping_packet.lfr_fpga_version[2] = parameters[3]; // n3
325 325 }
326 326
327 327 void increment_seq_counter( unsigned char *packet_sequence_control)
328 328 {
329 329 /** This function increment the sequence counter psased in argument.
330 330 *
331 331 * The increment does not affect the grouping flag. In case of an overflow, the counter is reset to 0.
332 332 *
333 333 */
334 334
335 335 unsigned short sequence_cnt;
336 336 unsigned short segmentation_grouping_flag;
337 337 unsigned short new_packet_sequence_control;
338 338
339 339 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8; // keep bits 7 downto 6
340 340 sequence_cnt = (unsigned short) (
341 341 ( (packet_sequence_control[0] & 0x3f) << 8 ) // keep bits 5 downto 0
342 342 + packet_sequence_control[1]
343 343 );
344 344
345 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
346
347 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
348 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
349
345 350 if ( sequence_cnt < SEQ_CNT_MAX)
346 351 {
347 352 sequence_cnt = sequence_cnt + 1;
348 353 }
349 354 else
350 355 {
351 356 sequence_cnt = 0;
352 357 }
353
354 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
355
356 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
357 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
358 358 }
359 359
360 360 void getTime( unsigned char *time)
361 361 {
362 362 /** This function write the current local time in the time buffer passed in argument.
363 363 *
364 364 */
365 365
366 366 time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
367 367 time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
368 368 time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
369 369 time[3] = (unsigned char) (time_management_regs->coarse_time);
370 370 time[4] = (unsigned char) (time_management_regs->fine_time>>8);
371 371 time[5] = (unsigned char) (time_management_regs->fine_time);
372 372 }
373 373
374 374 unsigned long long int getTimeAsUnsignedLongLongInt( )
375 375 {
376 376 /** This function write the current local time in the time buffer passed in argument.
377 377 *
378 378 */
379 379 unsigned long long int time;
380 380
381 381 time = ( (unsigned long long int) (time_management_regs->coarse_time & 0x7fffffff) << 16 )
382 382 + time_management_regs->fine_time;
383 383
384 384 return time;
385 385 }
386 386
387 387 void send_dumb_hk( void )
388 388 {
389 389 Packet_TM_LFR_HK_t dummy_hk_packet;
390 390 unsigned char *parameters;
391 391 unsigned int i;
392 392 rtems_id queue_id;
393 393
394 394 dummy_hk_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
395 395 dummy_hk_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
396 396 dummy_hk_packet.reserved = DEFAULT_RESERVED;
397 397 dummy_hk_packet.userApplication = CCSDS_USER_APP;
398 398 dummy_hk_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
399 399 dummy_hk_packet.packetID[1] = (unsigned char) (APID_TM_HK);
400 400 dummy_hk_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
401 401 dummy_hk_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
402 402 dummy_hk_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
403 403 dummy_hk_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
404 404 dummy_hk_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
405 405 dummy_hk_packet.serviceType = TM_TYPE_HK;
406 406 dummy_hk_packet.serviceSubType = TM_SUBTYPE_HK;
407 407 dummy_hk_packet.destinationID = TM_DESTINATION_ID_GROUND;
408 408 dummy_hk_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
409 409 dummy_hk_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
410 410 dummy_hk_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
411 411 dummy_hk_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
412 412 dummy_hk_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
413 413 dummy_hk_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
414 414 dummy_hk_packet.sid = SID_HK;
415 415
416 416 // init status word
417 417 dummy_hk_packet.lfr_status_word[0] = 0xff;
418 418 dummy_hk_packet.lfr_status_word[1] = 0xff;
419 419 // init software version
420 420 dummy_hk_packet.lfr_sw_version[0] = SW_VERSION_N1;
421 421 dummy_hk_packet.lfr_sw_version[1] = SW_VERSION_N2;
422 422 dummy_hk_packet.lfr_sw_version[2] = SW_VERSION_N3;
423 423 dummy_hk_packet.lfr_sw_version[3] = SW_VERSION_N4;
424 424 // init fpga version
425 425 parameters = (unsigned char *) (REGS_ADDR_WAVEFORM_PICKER + 0xb0);
426 426 dummy_hk_packet.lfr_fpga_version[0] = parameters[1]; // n1
427 427 dummy_hk_packet.lfr_fpga_version[1] = parameters[2]; // n2
428 428 dummy_hk_packet.lfr_fpga_version[2] = parameters[3]; // n3
429 429
430 430 parameters = (unsigned char *) &dummy_hk_packet.hk_lfr_cpu_load;
431 431
432 432 for (i=0; i<100; i++)
433 433 {
434 434 parameters[i] = 0xff;
435 435 }
436 436
437 437 get_message_queue_id_send( &queue_id );
438 438
439 439 rtems_message_queue_urgent( queue_id, &dummy_hk_packet,
440 440 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
441 441 }
442 442
443 443 void get_v_e1_e2_f3_old( unsigned char *spacecraft_potential )
444 444 {
445 445 unsigned int coarseTime;
446 446 unsigned int acquisitionTime;
447 447 unsigned int deltaT = 0;
448 448 unsigned char *bufferPtr;
449 449
450 450 unsigned int offset_in_samples;
451 451 unsigned int offset_in_bytes;
452 452 unsigned char f3 = 16; // v, e1 and e2 will be picked up each second, f3 = 16 Hz
453 453
454 454 if (lfrCurrentMode == LFR_MODE_STANDBY)
455 455 {
456 456 spacecraft_potential[0] = 0x00;
457 457 spacecraft_potential[1] = 0x00;
458 458 spacecraft_potential[2] = 0x00;
459 459 spacecraft_potential[3] = 0x00;
460 460 spacecraft_potential[4] = 0x00;
461 461 spacecraft_potential[5] = 0x00;
462 462 }
463 463 else
464 464 {
465 465 coarseTime = time_management_regs->coarse_time & 0x7fffffff;
466 466 bufferPtr = (unsigned char*) current_ring_node_f3->buffer_address;
467 467 acquisitionTime = (unsigned int) ( ( bufferPtr[2] & 0x7f ) << 24 )
468 468 + (unsigned int) ( bufferPtr[3] << 16 )
469 469 + (unsigned int) ( bufferPtr[0] << 8 )
470 470 + (unsigned int) ( bufferPtr[1] );
471 471 if ( coarseTime > acquisitionTime )
472 472 {
473 473 deltaT = coarseTime - acquisitionTime;
474 474 offset_in_samples = (deltaT-1) * f3 ;
475 475 }
476 476 else if( coarseTime == acquisitionTime )
477 477 {
478 478 bufferPtr = (unsigned char*) current_ring_node_f3->previous->buffer_address; // pick up v e1 and e2 in the previous f3 buffer
479 479 offset_in_samples = NB_SAMPLES_PER_SNAPSHOT-1;
480 480 }
481 481 else
482 482 {
483 483 offset_in_samples = 0;
484 484 PRINTF2("ERR *** in get_v_e1_e2_f3 *** coarseTime = %x, acquisitionTime = %x\n", coarseTime, acquisitionTime)
485 485 }
486 486
487 487 if ( offset_in_samples > (NB_SAMPLES_PER_SNAPSHOT - 1) )
488 488 {
489 489 PRINTF1("ERR *** in get_v_e1_e2_f3 *** trying to read out of the buffer, counter = %d\n", offset_in_samples)
490 490 offset_in_samples = NB_SAMPLES_PER_SNAPSHOT -1;
491 491 }
492 492 offset_in_bytes = TIME_OFFSET_IN_BYTES + offset_in_samples * NB_WORDS_SWF_BLK * 4;
493 493 spacecraft_potential[0] = bufferPtr[ offset_in_bytes + 0];
494 494 spacecraft_potential[1] = bufferPtr[ offset_in_bytes + 1];
495 495 spacecraft_potential[2] = bufferPtr[ offset_in_bytes + 2];
496 496 spacecraft_potential[3] = bufferPtr[ offset_in_bytes + 3];
497 497 spacecraft_potential[4] = bufferPtr[ offset_in_bytes + 4];
498 498 spacecraft_potential[5] = bufferPtr[ offset_in_bytes + 5];
499 499 }
500 500 }
501 501
502 502 void get_v_e1_e2_f3( unsigned char *spacecraft_potential )
503 503 {
504 504 unsigned int coarseTime;
505 505 unsigned int acquisitionTime;
506 506 unsigned int deltaT = 0;
507 507 unsigned char *bufferPtr;
508 508
509 509 unsigned int offset_in_samples;
510 510 unsigned int offset_in_bytes;
511 511 unsigned char f3 = 16; // v, e1 and e2 will be picked up each second, f3 = 16 Hz
512 512
513 513 if (lfrCurrentMode == LFR_MODE_STANDBY)
514 514 {
515 515 spacecraft_potential[0] = 0x00;
516 516 spacecraft_potential[1] = 0x00;
517 517 spacecraft_potential[2] = 0x00;
518 518 spacecraft_potential[3] = 0x00;
519 519 spacecraft_potential[4] = 0x00;
520 520 spacecraft_potential[5] = 0x00;
521 521 }
522 522 else
523 523 {
524 524 coarseTime = time_management_regs->coarse_time & 0x7fffffff;
525 525 bufferPtr = (unsigned char*) current_ring_node_f3->buffer_address;
526 526 acquisitionTime = (unsigned int) ( ( bufferPtr[0] & 0x7f ) << 24 )
527 527 + (unsigned int) ( bufferPtr[1] << 16 )
528 528 + (unsigned int) ( bufferPtr[2] << 8 )
529 529 + (unsigned int) ( bufferPtr[3] );
530 530 if ( coarseTime > acquisitionTime )
531 531 {
532 532 deltaT = coarseTime - acquisitionTime;
533 533 offset_in_samples = (deltaT-1) * f3 ;
534 534 }
535 535 else if( coarseTime == acquisitionTime )
536 536 {
537 537 bufferPtr = (unsigned char*) current_ring_node_f3->previous->buffer_address; // pick up v e1 and e2 in the previous f3 buffer
538 538 offset_in_samples = NB_SAMPLES_PER_SNAPSHOT-1;
539 539 }
540 540 else
541 541 {
542 542 offset_in_samples = 0;
543 543 PRINTF2("ERR *** in get_v_e1_e2_f3 *** coarseTime = %x, acquisitionTime = %x\n", coarseTime, acquisitionTime)
544 544 }
545 545
546 546 if ( offset_in_samples > (NB_SAMPLES_PER_SNAPSHOT - 1) )
547 547 {
548 548 PRINTF1("ERR *** in get_v_e1_e2_f3 *** trying to read out of the buffer, counter = %d\n", offset_in_samples)
549 549 offset_in_samples = NB_SAMPLES_PER_SNAPSHOT -1;
550 550 }
551 551 offset_in_bytes = TIME_OFFSET_IN_BYTES + offset_in_samples * NB_WORDS_SWF_BLK * 4;
552 552 spacecraft_potential[0] = bufferPtr[ offset_in_bytes + 0];
553 553 spacecraft_potential[1] = bufferPtr[ offset_in_bytes + 1];
554 554 spacecraft_potential[2] = bufferPtr[ offset_in_bytes + 2];
555 555 spacecraft_potential[3] = bufferPtr[ offset_in_bytes + 3];
556 556 spacecraft_potential[4] = bufferPtr[ offset_in_bytes + 4];
557 557 spacecraft_potential[5] = bufferPtr[ offset_in_bytes + 5];
558 558 }
559 559 }
560 560
561 561 void get_cpu_load( unsigned char *resource_statistics )
562 562 {
563 563 unsigned char cpu_load;
564 564
565 565 cpu_load = lfr_rtems_cpu_usage_report();
566 566
567 567 // HK_LFR_CPU_LOAD
568 568 resource_statistics[0] = cpu_load;
569 569
570 570 // HK_LFR_CPU_LOAD_MAX
571 571 if (cpu_load > resource_statistics[1])
572 572 {
573 573 resource_statistics[1] = cpu_load;
574 574 }
575 575
576 576 // CPU_LOAD_AVE
577 577 resource_statistics[2] = 0;
578 578
579 579 #ifndef PRINT_TASK_STATISTICS
580 580 rtems_cpu_usage_reset();
581 581 #endif
582 582
583 583 }
584 584
585 585
586 586
Show file before | Show file after | Hide comments
@@ -1,511 +1,510
1 1 /** Functions to send TM packets related to TC parsing and execution.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to send appropriate TM packets after parsing and execution:
7 7 * - TM_LFR_TC_EXE_SUCCESS
8 8 * - TM_LFR_TC_EXE_INCONSISTENT
9 9 * - TM_LFR_TC_EXE_NOT_EXECUTABLE
10 10 * - TM_LFR_TC_EXE_NOT_IMPLEMENTED
11 11 * - TM_LFR_TC_EXE_ERROR
12 12 * - TM_LFR_TC_EXE_CORRUPTED
13 13 *
14 14 */
15 15
16 16 #include "tm_lfr_tc_exe.h"
17 17
18 18 int send_tm_lfr_tc_exe_success( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
19 19 {
20 20 /** This function sends a TM_LFR_TC_EXE_SUCCESS packet in the dedicated RTEMS message queue.
21 21 *
22 22 * @param TC points to the TeleCommand packet that is being processed
23 23 * @param queue_id is the id of the queue which handles TM
24 24 *
25 25 * @return RTEMS directive status code:
26 26 * - RTEMS_SUCCESSFUL - message sent successfully
27 27 * - RTEMS_INVALID_ID - invalid queue id
28 28 * - RTEMS_INVALID_SIZE - invalid message size
29 29 * - RTEMS_INVALID_ADDRESS - buffer is NULL
30 30 * - RTEMS_UNSATISFIED - out of message buffers
31 31 * - RTEMS_TOO_MANY - queue s limit has been reached
32 32 *
33 33 */
34 34
35 35 rtems_status_code status;
36 36 Packet_TM_LFR_TC_EXE_SUCCESS_t TM;
37 37 unsigned char messageSize;
38 38
39 39 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
40 40 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
41 41 TM.reserved = DEFAULT_RESERVED;
42 42 TM.userApplication = CCSDS_USER_APP;
43 43 // PACKET HEADER
44 44 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
45 45 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
46 46 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
47 47 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_SUCCESS >> 8);
48 48 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_SUCCESS );
49 49 // DATA FIELD HEADER
50 50 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
51 51 TM.serviceType = TM_TYPE_TC_EXE;
52 52 TM.serviceSubType = TM_SUBTYPE_EXE_OK;
53 53 TM.destinationID = TC->sourceID;
54 54 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
55 55 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
56 56 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
57 57 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
58 58 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
59 59 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
60 60 //
61 61 TM.telecommand_pkt_id[0] = TC->packetID[0];
62 62 TM.telecommand_pkt_id[1] = TC->packetID[1];
63 63 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
64 64 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
65 65
66 66 messageSize = PACKET_LENGTH_TC_EXE_SUCCESS + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
67 67
68 68 // SEND DATA
69 69 status = rtems_message_queue_send( queue_id, &TM, messageSize);
70 70 if (status != RTEMS_SUCCESSFUL) {
71 71 PRINTF("in send_tm_lfr_tc_exe_success *** ERR\n")
72 72 }
73 73
74 74 // UPDATE HK FIELDS
75 75 update_last_TC_exe( TC, TM.time );
76 76
77 77 return status;
78 78 }
79 79
80 80 int send_tm_lfr_tc_exe_inconsistent( ccsdsTelecommandPacket_t *TC, rtems_id queue_id,
81 81 unsigned char byte_position, unsigned char rcv_value )
82 82 {
83 83 /** This function sends a TM_LFR_TC_EXE_INCONSISTENT packet in the dedicated RTEMS message queue.
84 84 *
85 85 * @param TC points to the TeleCommand packet that is being processed
86 86 * @param queue_id is the id of the queue which handles TM
87 87 * @param byte_position is the byte position of the MSB of the parameter that has been seen as inconsistent
88 88 * @param rcv_value is the value of the LSB of the parameter that has been deteced as inconsistent
89 89 *
90 90 * @return RTEMS directive status code:
91 91 * - RTEMS_SUCCESSFUL - message sent successfully
92 92 * - RTEMS_INVALID_ID - invalid queue id
93 93 * - RTEMS_INVALID_SIZE - invalid message size
94 94 * - RTEMS_INVALID_ADDRESS - buffer is NULL
95 95 * - RTEMS_UNSATISFIED - out of message buffers
96 96 * - RTEMS_TOO_MANY - queue s limit has been reached
97 97 *
98 98 */
99 99
100 100 rtems_status_code status;
101 101 Packet_TM_LFR_TC_EXE_INCONSISTENT_t TM;
102 102 unsigned char messageSize;
103 103
104 104 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
105 105 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
106 106 TM.reserved = DEFAULT_RESERVED;
107 107 TM.userApplication = CCSDS_USER_APP;
108 108 // PACKET HEADER
109 109 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
110 110 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
111 111 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
112 112 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_INCONSISTENT >> 8);
113 113 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_INCONSISTENT );
114 114 // DATA FIELD HEADER
115 115 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
116 116 TM.serviceType = TM_TYPE_TC_EXE;
117 117 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
118 118 TM.destinationID = TC->sourceID;
119 119 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
120 120 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
121 121 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
122 122 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
123 123 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
124 124 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
125 125 //
126 126 TM.tc_failure_code[0] = (char) (WRONG_APP_DATA >> 8);
127 127 TM.tc_failure_code[1] = (char) (WRONG_APP_DATA );
128 128 TM.telecommand_pkt_id[0] = TC->packetID[0];
129 129 TM.telecommand_pkt_id[1] = TC->packetID[1];
130 130 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
131 131 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
132 132 TM.tc_service = TC->serviceType; // type of the rejected TC
133 133 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
134 134 TM.byte_position = byte_position;
135 135 TM.rcv_value = rcv_value;
136 136
137 137 messageSize = PACKET_LENGTH_TC_EXE_INCONSISTENT + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
138 138
139 139 // SEND DATA
140 140 status = rtems_message_queue_send( queue_id, &TM, messageSize);
141 141 if (status != RTEMS_SUCCESSFUL) {
142 142 PRINTF("in send_tm_lfr_tc_exe_inconsistent *** ERR\n")
143 143 }
144 144
145 145 // UPDATE HK FIELDS
146 146 update_last_TC_rej( TC, TM.time );
147 147
148 148 return status;
149 149 }
150 150
151 151 int send_tm_lfr_tc_exe_not_executable( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
152 152 {
153 153 /** This function sends a TM_LFR_TC_EXE_NOT_EXECUTABLE packet in the dedicated RTEMS message queue.
154 154 *
155 155 * @param TC points to the TeleCommand packet that is being processed
156 156 * @param queue_id is the id of the queue which handles TM
157 157 *
158 158 * @return RTEMS directive status code:
159 159 * - RTEMS_SUCCESSFUL - message sent successfully
160 160 * - RTEMS_INVALID_ID - invalid queue id
161 161 * - RTEMS_INVALID_SIZE - invalid message size
162 162 * - RTEMS_INVALID_ADDRESS - buffer is NULL
163 163 * - RTEMS_UNSATISFIED - out of message buffers
164 164 * - RTEMS_TOO_MANY - queue s limit has been reached
165 165 *
166 166 */
167 167
168 168 rtems_status_code status;
169 169 Packet_TM_LFR_TC_EXE_NOT_EXECUTABLE_t TM;
170 170 unsigned char messageSize;
171 171
172 172 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
173 173 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
174 174 TM.reserved = DEFAULT_RESERVED;
175 175 TM.userApplication = CCSDS_USER_APP;
176 176 // PACKET HEADER
177 177 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
178 178 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
179 179 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
180 180 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE >> 8);
181 181 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE );
182 182 // DATA FIELD HEADER
183 183 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
184 184 TM.serviceType = TM_TYPE_TC_EXE;
185 185 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
186 186 TM.destinationID = TC->sourceID; // default destination id
187 187 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
188 188 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
189 189 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
190 190 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
191 191 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
192 192 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
193 193 //
194 194 TM.tc_failure_code[0] = (char) (TC_NOT_EXE >> 8);
195 195 TM.tc_failure_code[1] = (char) (TC_NOT_EXE );
196 196 TM.telecommand_pkt_id[0] = TC->packetID[0];
197 197 TM.telecommand_pkt_id[1] = TC->packetID[1];
198 198 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
199 199 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
200 200 TM.tc_service = TC->serviceType; // type of the rejected TC
201 201 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
202 202 TM.lfr_status_word[0] = housekeeping_packet.lfr_status_word[0];
203 203 TM.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1];
204 204
205 205 messageSize = PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
206 206
207 207 // SEND DATA
208 208 status = rtems_message_queue_send( queue_id, &TM, messageSize);
209 209 if (status != RTEMS_SUCCESSFUL) {
210 210 PRINTF("in send_tm_lfr_tc_exe_not_executable *** ERR\n")
211 211 }
212 212
213 213 // UPDATE HK FIELDS
214 214 update_last_TC_rej( TC, TM.time );
215 215
216 216 return status;
217 217 }
218 218
219 219 int send_tm_lfr_tc_exe_not_implemented( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time )
220 220 {
221 221 /** This function sends a TM_LFR_TC_EXE_NOT_IMPLEMENTED packet in the dedicated RTEMS message queue.
222 222 *
223 223 * @param TC points to the TeleCommand packet that is being processed
224 224 * @param queue_id is the id of the queue which handles TM
225 225 *
226 226 * @return RTEMS directive status code:
227 227 * - RTEMS_SUCCESSFUL - message sent successfully
228 228 * - RTEMS_INVALID_ID - invalid queue id
229 229 * - RTEMS_INVALID_SIZE - invalid message size
230 230 * - RTEMS_INVALID_ADDRESS - buffer is NULL
231 231 * - RTEMS_UNSATISFIED - out of message buffers
232 232 * - RTEMS_TOO_MANY - queue s limit has been reached
233 233 *
234 234 */
235 235
236 236 rtems_status_code status;
237 237 Packet_TM_LFR_TC_EXE_NOT_IMPLEMENTED_t TM;
238 238 unsigned char messageSize;
239 239
240 240 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
241 241 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
242 242 TM.reserved = DEFAULT_RESERVED;
243 243 TM.userApplication = CCSDS_USER_APP;
244 244 // PACKET HEADER
245 245 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
246 246 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
247 247 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
248 248 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED >> 8);
249 249 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED );
250 250 // DATA FIELD HEADER
251 251 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
252 252 TM.serviceType = TM_TYPE_TC_EXE;
253 253 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
254 254 TM.destinationID = TC->sourceID; // default destination id
255 255 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
256 256 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
257 257 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
258 258 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
259 259 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
260 260 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
261 261 //
262 262 TM.tc_failure_code[0] = (char) (FUNCT_NOT_IMPL >> 8);
263 263 TM.tc_failure_code[1] = (char) (FUNCT_NOT_IMPL );
264 264 TM.telecommand_pkt_id[0] = TC->packetID[0];
265 265 TM.telecommand_pkt_id[1] = TC->packetID[1];
266 266 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
267 267 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
268 268 TM.tc_service = TC->serviceType; // type of the rejected TC
269 269 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
270 270
271 271 messageSize = PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
272 272
273 273 // SEND DATA
274 274 status = rtems_message_queue_send( queue_id, &TM, messageSize);
275 275 if (status != RTEMS_SUCCESSFUL) {
276 276 PRINTF("in send_tm_lfr_tc_exe_not_implemented *** ERR\n")
277 277 }
278 278
279 279 // UPDATE HK FIELDS
280 280 update_last_TC_rej( TC, TM.time );
281 281
282 282 return status;
283 283 }
284 284
285 285 int send_tm_lfr_tc_exe_error( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
286 286 {
287 287 /** This function sends a TM_LFR_TC_EXE_ERROR packet in the dedicated RTEMS message queue.
288 288 *
289 289 * @param TC points to the TeleCommand packet that is being processed
290 290 * @param queue_id is the id of the queue which handles TM
291 291 *
292 292 * @return RTEMS directive status code:
293 293 * - RTEMS_SUCCESSFUL - message sent successfully
294 294 * - RTEMS_INVALID_ID - invalid queue id
295 295 * - RTEMS_INVALID_SIZE - invalid message size
296 296 * - RTEMS_INVALID_ADDRESS - buffer is NULL
297 297 * - RTEMS_UNSATISFIED - out of message buffers
298 298 * - RTEMS_TOO_MANY - queue s limit has been reached
299 299 *
300 300 */
301 301
302 302 rtems_status_code status;
303 303 Packet_TM_LFR_TC_EXE_ERROR_t TM;
304 304 unsigned char messageSize;
305 305
306 306 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
307 307 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
308 308 TM.reserved = DEFAULT_RESERVED;
309 309 TM.userApplication = CCSDS_USER_APP;
310 310 // PACKET HEADER
311 311 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
312 312 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
313 313 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
314 314 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_ERROR >> 8);
315 315 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_ERROR );
316 316 // DATA FIELD HEADER
317 317 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
318 318 TM.serviceType = TM_TYPE_TC_EXE;
319 319 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
320 320 TM.destinationID = TC->sourceID; // default destination id
321 321 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
322 322 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
323 323 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
324 324 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
325 325 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
326 326 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
327 327 //
328 328 TM.tc_failure_code[0] = (char) (FAIL_DETECTED >> 8);
329 329 TM.tc_failure_code[1] = (char) (FAIL_DETECTED );
330 330 TM.telecommand_pkt_id[0] = TC->packetID[0];
331 331 TM.telecommand_pkt_id[1] = TC->packetID[1];
332 332 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
333 333 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
334 334 TM.tc_service = TC->serviceType; // type of the rejected TC
335 335 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
336 336
337 337 messageSize = PACKET_LENGTH_TC_EXE_ERROR + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
338 338
339 339 // SEND DATA
340 340 status = rtems_message_queue_send( queue_id, &TM, messageSize);
341 341 if (status != RTEMS_SUCCESSFUL) {
342 342 PRINTF("in send_tm_lfr_tc_exe_error *** ERR\n")
343 343 }
344 344
345 345 // UPDATE HK FIELDS
346 346 update_last_TC_rej( TC, TM.time );
347 347
348 348 return status;
349 349 }
350 350
351 351 int send_tm_lfr_tc_exe_corrupted(ccsdsTelecommandPacket_t *TC, rtems_id queue_id,
352 352 unsigned char *computed_CRC, unsigned char *currentTC_LEN_RCV,
353 353 unsigned char destinationID )
354 354 {
355 355 /** This function sends a TM_LFR_TC_EXE_CORRUPTED packet in the dedicated RTEMS message queue.
356 356 *
357 357 * @param TC points to the TeleCommand packet that is being processed
358 358 * @param queue_id is the id of the queue which handles TM
359 359 * @param computed_CRC points to a buffer of two bytes containing the CRC computed during the parsing of the TeleCommand
360 360 * @param currentTC_LEN_RCV points to a buffer of two bytes containing a packet size field computed on the received data
361 361 *
362 362 * @return RTEMS directive status code:
363 363 * - RTEMS_SUCCESSFUL - message sent successfully
364 364 * - RTEMS_INVALID_ID - invalid queue id
365 365 * - RTEMS_INVALID_SIZE - invalid message size
366 366 * - RTEMS_INVALID_ADDRESS - buffer is NULL
367 367 * - RTEMS_UNSATISFIED - out of message buffers
368 368 * - RTEMS_TOO_MANY - queue s limit has been reached
369 369 *
370 370 */
371 371
372 372 rtems_status_code status;
373 373 Packet_TM_LFR_TC_EXE_CORRUPTED_t TM;
374 374 unsigned char messageSize;
375 375 unsigned int packetLength;
376 376 unsigned char *packetDataField;
377 377
378 378 packetLength = (TC->packetLength[0] * 256) + TC->packetLength[1]; // compute the packet length parameter
379 379 packetDataField = (unsigned char *) &TC->headerFlag_pusVersion_Ack; // get the beginning of the data field
380 380
381 381 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
382 382 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
383 383 TM.reserved = DEFAULT_RESERVED;
384 384 TM.userApplication = CCSDS_USER_APP;
385 385 // PACKET HEADER
386 386 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
387 387 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
388 388 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
389 389 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_CORRUPTED >> 8);
390 390 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_CORRUPTED );
391 391 // DATA FIELD HEADER
392 392 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
393 393 TM.serviceType = TM_TYPE_TC_EXE;
394 394 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
395 395 TM.destinationID = destinationID;
396 396 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
397 397 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
398 398 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
399 399 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
400 400 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
401 401 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
402 402 //
403 403 TM.tc_failure_code[0] = (unsigned char) (CORRUPTED >> 8);
404 404 TM.tc_failure_code[1] = (unsigned char) (CORRUPTED );
405 405 TM.telecommand_pkt_id[0] = TC->packetID[0];
406 406 TM.telecommand_pkt_id[1] = TC->packetID[1];
407 407 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
408 408 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
409 409 TM.tc_service = TC->serviceType; // type of the rejected TC
410 410 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
411 411 TM.pkt_len_rcv_value[0] = TC->packetLength[0];
412 412 TM.pkt_len_rcv_value[1] = TC->packetLength[1];
413 413 TM.pkt_datafieldsize_cnt[0] = currentTC_LEN_RCV[0];
414 414 TM.pkt_datafieldsize_cnt[1] = currentTC_LEN_RCV[1];
415 415 TM.rcv_crc[0] = packetDataField[ packetLength - 1 ];
416 416 TM.rcv_crc[1] = packetDataField[ packetLength ];
417 417 TM.computed_crc[0] = computed_CRC[0];
418 418 TM.computed_crc[1] = computed_CRC[1];
419 419
420 420 messageSize = PACKET_LENGTH_TC_EXE_CORRUPTED + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
421 421
422 422 // SEND DATA
423 423 status = rtems_message_queue_send( queue_id, &TM, messageSize);
424 424 if (status != RTEMS_SUCCESSFUL) {
425 425 PRINTF("in send_tm_lfr_tc_exe_error *** ERR\n")
426 426 }
427 427
428 428 // UPDATE HK FIELDS
429 429 update_last_TC_rej( TC, TM.time );
430 430
431 431 return status;
432 432 }
433 433
434 434 void increment_seq_counter_destination_id( unsigned char *packet_sequence_control, unsigned char destination_id )
435 435 {
436 436 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
437 437 *
438 438 * @param packet_sequence_control points to the packet sequence control which will be incremented
439 439 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
440 440 *
441 441 * If the destination ID is not known, a dedicated counter is incremented.
442 442 *
443 443 */
444 444
445 445 unsigned short sequence_cnt;
446 446 unsigned short segmentation_grouping_flag;
447 447 unsigned short new_packet_sequence_control;
448 448 unsigned char i;
449 449
450 450 switch (destination_id)
451 451 {
452 452 case SID_TC_GROUND:
453 453 i = GROUND;
454 454 break;
455 455 case SID_TC_MISSION_TIMELINE:
456 456 i = MISSION_TIMELINE;
457 457 break;
458 458 case SID_TC_TC_SEQUENCES:
459 459 i = TC_SEQUENCES;
460 460 break;
461 461 case SID_TC_RECOVERY_ACTION_CMD:
462 462 i = RECOVERY_ACTION_CMD;
463 463 break;
464 464 case SID_TC_BACKUP_MISSION_TIMELINE:
465 465 i = BACKUP_MISSION_TIMELINE;
466 466 break;
467 467 case SID_TC_DIRECT_CMD:
468 468 i = DIRECT_CMD;
469 469 break;
470 470 case SID_TC_SPARE_GRD_SRC1:
471 471 i = SPARE_GRD_SRC1;
472 472 break;
473 473 case SID_TC_SPARE_GRD_SRC2:
474 474 i = SPARE_GRD_SRC2;
475 475 break;
476 476 case SID_TC_OBCP:
477 477 i = OBCP;
478 478 break;
479 479 case SID_TC_SYSTEM_CONTROL:
480 480 i = SYSTEM_CONTROL;
481 481 break;
482 482 case SID_TC_AOCS:
483 483 i = AOCS;
484 484 break;
485 485 case SID_TC_RPW_INTERNAL:
486 486 i = RPW_INTERNAL;
487 487 break;
488 488 default:
489 489 i = GROUND;
490 490 break;
491 491 }
492 492
493 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
494 sequence_cnt = sequenceCounters_TC_EXE[ i ] & 0x3fff;
495
496 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
497
498 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
499 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
500
493 501 // increment the sequence counter
494 502 if ( sequenceCounters_TC_EXE[ i ] < SEQ_CNT_MAX )
495 503 {
496 504 sequenceCounters_TC_EXE[ i ] = sequenceCounters_TC_EXE[ i ] + 1;
497 505 }
498 506 else
499 507 {
500 508 sequenceCounters_TC_EXE[ i ] = 0;
501 509 }
502
503 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
504 sequence_cnt = sequenceCounters_TC_EXE[ i ] & 0x3fff;
505
506 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
507
508 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
509 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
510
511 510 }
General Comments 0
You need to be logged in to leave comments. Login now