@@ -1,6 +1,6 | |||
|
1 | 1 | ############################################################################# |
|
2 | 2 | # Makefile for building: bin/fsw |
|
3 |
# Generated by qmake (2.01a) (Qt 4.8.6) on: Thu Nov |
|
|
3 | # Generated by qmake (2.01a) (Qt 4.8.6) on: Thu Nov 13 07:59:00 2014 | |
|
4 | 4 | # Project: fsw-qt.pro |
|
5 | 5 | # Template: app |
|
6 | 6 | # Command: /usr/bin/qmake-qt4 -spec /usr/lib64/qt4/mkspecs/linux-g++ -o Makefile fsw-qt.pro |
@@ -10,7 +10,7 | |||
|
10 | 10 | |
|
11 | 11 | CC = sparc-rtems-gcc |
|
12 | 12 | CXX = sparc-rtems-g++ |
|
13 |
DEFINES = -DSW_VERSION_N1=2 -DSW_VERSION_N2=0 -DSW_VERSION_N3=1 -DSW_VERSION_N4=1 -DLPP_DPU_DESTID -DPRINT_MESSAGES_ON_CONSOLE |
|
|
13 | DEFINES = -DSW_VERSION_N1=2 -DSW_VERSION_N2=0 -DSW_VERSION_N3=1 -DSW_VERSION_N4=1 -DLPP_DPU_DESTID -DPRINT_MESSAGES_ON_CONSOLE | |
|
14 | 14 | CFLAGS = -pipe -O3 -Wall $(DEFINES) |
|
15 | 15 | CXXFLAGS = -pipe -O3 -Wall $(DEFINES) |
|
16 | 16 | INCPATH = -I/usr/lib64/qt4/mkspecs/linux-g++ -I. -I../src -I../header -I../header/processing -I../src/LFR_basic-parameters |
@@ -2,7 +2,7 TEMPLATE = app | |||
|
2 | 2 | # CONFIG += console v8 sim |
|
3 | 3 | # CONFIG options = verbose *** boot_messages *** debug_messages *** cpu_usage_report *** stack_report *** vhdl_dev *** debug_tch |
|
4 | 4 | # lpp_dpu_destid |
|
5 |
CONFIG += console verbose lpp_dpu_destid |
|
|
5 | CONFIG += console verbose lpp_dpu_destid | |
|
6 | 6 | CONFIG -= qt |
|
7 | 7 | |
|
8 | 8 | include(./sparc.pri) |
@@ -1,6 +1,6 | |||
|
1 | 1 | <?xml version="1.0" encoding="UTF-8"?> |
|
2 | 2 | <!DOCTYPE QtCreatorProject> |
|
3 |
<!-- Written by QtCreator 3.2.1, 2014-11- |
|
|
3 | <!-- Written by QtCreator 3.2.1, 2014-11-12T16:01:52. --> | |
|
4 | 4 | <qtcreator> |
|
5 | 5 | <data> |
|
6 | 6 | <variable>EnvironmentId</variable> |
@@ -34,7 +34,6 void getTime( unsigned char *time); | |||
|
34 | 34 | unsigned long long int getTimeAsUnsignedLongLongInt( ); |
|
35 | 35 | void send_dumb_hk( void ); |
|
36 | 36 | void get_v_e1_e2_f3(unsigned char *spacecraft_potential); |
|
37 | void get_v_e1_e2_f3_alt(unsigned char *spacecraft_potential); | |
|
38 | 37 | void get_cpu_load( unsigned char *resource_statistics ); |
|
39 | 38 | |
|
40 | 39 | extern int sched_yield( void ); |
@@ -14,6 +14,7 typedef struct ring_node | |||
|
14 | 14 | { |
|
15 | 15 | struct ring_node *previous; |
|
16 | 16 | struct ring_node *next; |
|
17 | unsigned int sid; | |
|
17 | 18 | unsigned int coarseTime; |
|
18 | 19 | unsigned int fineTime; |
|
19 | 20 | int buffer_address; |
@@ -12,6 +12,7 | |||
|
12 | 12 | #define NB_RING_NODES_SM_F0 12 // AT LEAST 8 due to the way the averaging is done |
|
13 | 13 | #define NB_RING_NODES_ASM_BURST_SBM_F0 10 // AT LEAST 3 |
|
14 | 14 | #define NB_RING_NODES_ASM_NORM_F0 10 // AT LEAST 3 |
|
15 | #define NB_RING_NODES_ASM_F0 3 // AT LEAST 3 | |
|
15 | 16 | #define NB_RING_NODES_SM_F1 12 // AT LEAST 8 due to the way the averaging is done |
|
16 | 17 | #define NB_RING_NODES_ASM_BURST_SBM_F1 5 // AT LEAST 3 |
|
17 | 18 | #define NB_RING_NODES_ASM_NORM_F1 5 // AT LEAST 3 |
@@ -31,6 +31,14 void spacewire_set_RE( unsigned char val | |||
|
31 | 31 | void spacewire_compute_stats_offsets( void ); |
|
32 | 32 | void spacewire_update_statistics( void ); |
|
33 | 33 | |
|
34 | void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header ); | |
|
35 | void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header ); | |
|
36 | void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header ); | |
|
37 | int spw_send_waveform_CWF( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_CWF_t *header ); | |
|
38 | int spw_send_waveform_SWF( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_SWF_t *header ); | |
|
39 | int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_CWF_t *header ); | |
|
40 | void spw_send_asm( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_ASM_t *header ); | |
|
41 | ||
|
34 | 42 | void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc ); |
|
35 | 43 | rtems_timer_service_routine user_routine( rtems_id timer_id, void *user_data ); |
|
36 | 44 |
@@ -18,7 +18,7 rtems_task prc2_task( rtems_task_argumen | |||
|
18 | 18 | // FUNCTIONS |
|
19 | 19 | |
|
20 | 20 | void reset_nb_sm_f2( void ); |
|
21 |
void SM_average_f2( |
|
|
21 | void SM_average_f2(float *averaged_spec_mat_f2, ring_node_sm *ring_node, unsigned int nbAverageNormF2 ); | |
|
22 | 22 | |
|
23 | 23 | //******* |
|
24 | 24 | // EXTERN |
@@ -13,12 +13,12 | |||
|
13 | 13 | |
|
14 | 14 | typedef struct ring_node_sm |
|
15 | 15 | { |
|
16 |
|
|
|
17 |
|
|
|
18 |
|
|
|
19 |
|
|
|
20 |
|
|
|
21 |
|
|
|
16 | struct ring_node_sm *previous; | |
|
17 | struct ring_node_sm *next; | |
|
18 | int buffer_address; | |
|
19 | unsigned int status; | |
|
20 | unsigned int coarseTime; | |
|
21 | unsigned int fineTime; | |
|
22 | 22 | } ring_node_sm; |
|
23 | 23 | |
|
24 | 24 | typedef struct ring_node_asm |
@@ -73,7 +73,6 void reset_nb_sm( void ); | |||
|
73 | 73 | // SM |
|
74 | 74 | void SM_init_rings( void ); |
|
75 | 75 | void SM_reset_current_ring_nodes( void ); |
|
76 | void SM_generic_init_ring(ring_node_sm *ring, unsigned char nbNodes, volatile int sm_f[] ); | |
|
77 | 76 | // ASM |
|
78 | 77 | void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes ); |
|
79 | 78 | void ASM_init_header( Header_TM_LFR_SCIENCE_ASM_t *header); |
@@ -100,8 +99,8 void reset_sm_status( void ); | |||
|
100 | 99 | void reset_spectral_matrix_regs( void ); |
|
101 | 100 | void set_time(unsigned char *time, unsigned char *timeInBuffer ); |
|
102 | 101 | unsigned long long int get_acquisition_time( unsigned char *timePtr ); |
|
103 | void close_matrix_actions(unsigned int *nb_sm, unsigned int nb_sm_before_avf, rtems_id avf_task_id, | |
|
104 | ring_node_sm *node_for_averaging, ring_node_sm *ringNode, unsigned long long int time); | |
|
102 | void close_matrix_actions( unsigned int *nb_sm, unsigned int nb_sm_before_avf, rtems_id avf_task_id, | |
|
103 | ring_node_sm *node_for_averaging, ring_node_sm *ringNode, unsigned long long int time ); | |
|
105 | 104 | unsigned char getSID( rtems_event_set event ); |
|
106 | 105 | |
|
107 | 106 | extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id ); |
@@ -109,7 +108,10 extern rtems_status_code get_message_que | |||
|
109 | 108 | |
|
110 | 109 | //*************************************** |
|
111 | 110 | // DEFINITIONS OF STATIC INLINE FUNCTIONS |
|
112 |
static inline void SM_average( |
|
|
111 | static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM, | |
|
112 | ring_node_sm *ring_node_tab[], | |
|
113 | unsigned int nbAverageNORM, unsigned int nbAverageSBM ); | |
|
114 | static inline void SM_average_debug( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM, | |
|
113 | 115 | ring_node_sm *ring_node_tab[], |
|
114 | 116 | unsigned int nbAverageNORM, unsigned int nbAverageSBM ); |
|
115 | 117 | static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized, |
@@ -119,7 +121,7 static inline void ASM_compress_reorgani | |||
|
119 | 121 | unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart); |
|
120 | 122 | static inline void ASM_convert(volatile float *input_matrix, char *output_matrix); |
|
121 | 123 | |
|
122 | void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM, | |
|
124 | void SM_average_debug( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM, | |
|
123 | 125 | ring_node_sm *ring_node_tab[], |
|
124 | 126 | unsigned int nbAverageNORM, unsigned int nbAverageSBM ) |
|
125 | 127 | { |
@@ -159,6 +161,39 void SM_average( float *averaged_spec_ma | |||
|
159 | 161 | } |
|
160 | 162 | } |
|
161 | 163 | |
|
164 | void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM, | |
|
165 | ring_node_sm *ring_node_tab[], | |
|
166 | unsigned int nbAverageNORM, unsigned int nbAverageSBM ) | |
|
167 | { | |
|
168 | float sum; | |
|
169 | unsigned int i; | |
|
170 | ||
|
171 | for(i=0; i<TOTAL_SIZE_SM; i++) | |
|
172 | { | |
|
173 | sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ]; | |
|
174 | ||
|
175 | if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) ) | |
|
176 | { | |
|
177 | averaged_spec_mat_NORM[ i ] = sum; | |
|
178 | averaged_spec_mat_SBM[ i ] = sum; | |
|
179 | } | |
|
180 | else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) ) | |
|
181 | { | |
|
182 | averaged_spec_mat_NORM[ i ] = sum; | |
|
183 | averaged_spec_mat_SBM[ i ] = sum; | |
|
184 | } | |
|
185 | else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) ) | |
|
186 | { | |
|
187 | averaged_spec_mat_NORM[ i ] = sum; | |
|
188 | averaged_spec_mat_SBM[ i ] = sum; | |
|
189 | } | |
|
190 | else | |
|
191 | { | |
|
192 | PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM) | |
|
193 | } | |
|
194 | } | |
|
195 | } | |
|
196 | ||
|
162 | 197 | void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider ) |
|
163 | 198 | { |
|
164 | 199 | int frequencyBin; |
@@ -17,10 +17,10 extern int fdSPW; | |||
|
17 | 17 | |
|
18 | 18 | //***************** |
|
19 | 19 | // waveform buffers |
|
20 |
extern volatile int wf_ |
|
|
21 |
extern volatile int wf_ |
|
|
22 |
extern volatile int wf_ |
|
|
23 |
extern volatile int wf_ |
|
|
20 | extern volatile int wf_buffer_f0[ ]; | |
|
21 | extern volatile int wf_buffer_f1[ ]; | |
|
22 | extern volatile int wf_buffer_f2[ ]; | |
|
23 | extern volatile int wf_buffer_f3[ ]; | |
|
24 | 24 | extern char wf_cont_f3_light[ ]; |
|
25 | 25 | |
|
26 | 26 | extern waveform_picker_regs_0_1_18_t *waveform_picker_regs; |
@@ -52,17 +52,12 rtems_task swbd_task( rtems_task_argumen | |||
|
52 | 52 | //****************** |
|
53 | 53 | // general functions |
|
54 | 54 | void WFP_init_rings( void ); |
|
55 |
void init_ |
|
|
55 | void init_ring( ring_node ring[], unsigned char nbNodes, volatile int buffer[] , unsigned int bufferSize ); | |
|
56 | 56 | void WFP_reset_current_ring_nodes( void ); |
|
57 | 57 | // |
|
58 | int init_header_snapshot_wf_table( unsigned int sid, Header_TM_LFR_SCIENCE_SWF_t *headerSWF ); | |
|
59 | int init_header_continuous_wf_table( unsigned int sid, Header_TM_LFR_SCIENCE_CWF_t *headerCWF ); | |
|
60 | 58 | int init_header_continuous_cwf3_light_table( Header_TM_LFR_SCIENCE_CWF_t *headerCWF ); |
|
61 | 59 | // |
|
62 |
int send_waveform_ |
|
|
63 | int send_waveform_CWF( ring_node *ring_node_to_send, unsigned int sid, Header_TM_LFR_SCIENCE_CWF_t *headerCWF, rtems_id queue_id ); | |
|
64 | int send_waveform_CWF3_light( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_CWF_t *headerCWF, rtems_id queue_id ); | |
|
65 | int send_ring_node_CWF( ring_node *ring_node_to_send ); | |
|
60 | int send_waveform_CWF3_light(ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id ); | |
|
66 | 61 | // |
|
67 | 62 | void compute_acquisition_time(unsigned int coarseTime, unsigned int fineTime, |
|
68 | 63 | unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char *acquisitionTime ); |
@@ -37,10 +37,10 unsigned char lfrCurrentMode; | |||
|
37 | 37 | // WAVEFORMS GLOBAL VARIABLES // 2688 * 3 * 4 + 2 * 4 = 32256 + 8 bytes = 32264 |
|
38 | 38 | // 127 * 256 = 32512 => delta = 248 bytes = 62 words |
|
39 | 39 | // F0 F1 F2 F3 |
|
40 |
volatile int wf_ |
|
|
41 |
volatile int wf_ |
|
|
42 |
volatile int wf_ |
|
|
43 |
volatile int wf_ |
|
|
40 | volatile int wf_buffer_f0[ NB_RING_NODES_F0 * WFRM_BUFFER ] __attribute__((aligned(0x100))); | |
|
41 | volatile int wf_buffer_f1[ NB_RING_NODES_F1 * WFRM_BUFFER ] __attribute__((aligned(0x100))); | |
|
42 | volatile int wf_buffer_f2[ NB_RING_NODES_F2 * WFRM_BUFFER ] __attribute__((aligned(0x100))); | |
|
43 | volatile int wf_buffer_f3[ NB_RING_NODES_F3 * WFRM_BUFFER ] __attribute__((aligned(0x100))); | |
|
44 | 44 | char wf_cont_f3_light[ (NB_SAMPLES_PER_SNAPSHOT) * NB_BYTES_CWF3_LIGHT_BLK + TIME_OFFSET_IN_BYTES ] __attribute__((aligned(0x100))); |
|
45 | 45 | |
|
46 | 46 | //*********************************** |
@@ -321,7 +321,7 int create_all_tasks( void ) // create a | |||
|
321 | 321 | status = rtems_task_create( |
|
322 | 322 | Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE, |
|
323 | 323 | RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT, |
|
324 | RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SEND] | |
|
324 | RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND] | |
|
325 | 325 | ); |
|
326 | 326 | } |
|
327 | 327 | if (status == RTEMS_SUCCESSFUL) // WTDG |
@@ -225,11 +225,11 rtems_task hous_task(rtems_task_argument | |||
|
225 | 225 | |
|
226 | 226 | spacewire_update_statistics(); |
|
227 | 227 | |
|
228 |
// get_v_e1_e2_f3 |
|
|
228 | // get_v_e1_e2_f3( housekeeping_packet.hk_lfr_sc_v_f3 ); | |
|
229 | 229 | get_cpu_load( (unsigned char *) &housekeeping_packet.hk_lfr_cpu_load ); |
|
230 | 230 | |
|
231 | 231 | // SEND PACKET |
|
232 |
status = rtems_message_queue_ |
|
|
232 | status = rtems_message_queue_send( queue_id, &housekeeping_packet, | |
|
233 | 233 | PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES); |
|
234 | 234 | if (status != RTEMS_SUCCESSFUL) { |
|
235 | 235 | PRINTF1("in HOUS *** ERR send: %d\n", status) |
@@ -437,73 +437,12 void send_dumb_hk( void ) | |||
|
437 | 437 | |
|
438 | 438 | get_message_queue_id_send( &queue_id ); |
|
439 | 439 | |
|
440 |
rtems_message_queue_ |
|
|
440 | rtems_message_queue_send( queue_id, &dummy_hk_packet, | |
|
441 | 441 | PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES); |
|
442 | 442 | } |
|
443 | 443 | |
|
444 | 444 | void get_v_e1_e2_f3( unsigned char *spacecraft_potential ) |
|
445 | 445 | { |
|
446 | unsigned int coarseTime; | |
|
447 | unsigned int acquisitionTime; | |
|
448 | unsigned int deltaT = 0; | |
|
449 | unsigned char *bufferPtr; | |
|
450 | ||
|
451 | unsigned int offset_in_samples; | |
|
452 | unsigned int offset_in_bytes; | |
|
453 | unsigned char f3 = 16; // v, e1 and e2 will be picked up each second, f3 = 16 Hz | |
|
454 | ||
|
455 | bufferPtr = NULL; | |
|
456 | ||
|
457 | if (lfrCurrentMode == LFR_MODE_STANDBY) | |
|
458 | { | |
|
459 | spacecraft_potential[0] = 0x00; | |
|
460 | spacecraft_potential[1] = 0x00; | |
|
461 | spacecraft_potential[2] = 0x00; | |
|
462 | spacecraft_potential[3] = 0x00; | |
|
463 | spacecraft_potential[4] = 0x00; | |
|
464 | spacecraft_potential[5] = 0x00; | |
|
465 | } | |
|
466 | else | |
|
467 | { | |
|
468 | coarseTime = time_management_regs->coarse_time & 0x7fffffff; | |
|
469 | bufferPtr = (unsigned char*) current_ring_node_f3->buffer_address; | |
|
470 | acquisitionTime = (unsigned int) ( ( bufferPtr[0] & 0x7f ) << 24 ) | |
|
471 | + (unsigned int) ( bufferPtr[1] << 16 ) | |
|
472 | + (unsigned int) ( bufferPtr[2] << 8 ) | |
|
473 | + (unsigned int) ( bufferPtr[3] ); | |
|
474 | if ( coarseTime > acquisitionTime ) | |
|
475 | { | |
|
476 | deltaT = coarseTime - acquisitionTime; | |
|
477 | offset_in_samples = (deltaT-1) * f3 ; | |
|
478 | } | |
|
479 | else if( coarseTime == acquisitionTime ) | |
|
480 | { | |
|
481 | bufferPtr = (unsigned char*) current_ring_node_f3->previous->buffer_address; // pick up v e1 and e2 in the previous f3 buffer | |
|
482 | offset_in_samples = NB_SAMPLES_PER_SNAPSHOT-1; | |
|
483 | } | |
|
484 | else | |
|
485 | { | |
|
486 | offset_in_samples = 0; | |
|
487 | // PRINTF2("ERR *** in get_v_e1_e2_f3 *** coarseTime = %x, acquisitionTime = %x\n", coarseTime, acquisitionTime) | |
|
488 | } | |
|
489 | ||
|
490 | if ( offset_in_samples > (NB_SAMPLES_PER_SNAPSHOT - 1) ) | |
|
491 | { | |
|
492 | // PRINTF1("ERR *** in get_v_e1_e2_f3 *** trying to read out of the buffer, counter = %d\n", offset_in_samples) | |
|
493 | offset_in_samples = NB_SAMPLES_PER_SNAPSHOT -1; | |
|
494 | } | |
|
495 | offset_in_bytes = TIME_OFFSET_IN_BYTES + offset_in_samples * NB_WORDS_SWF_BLK * 4; | |
|
496 | spacecraft_potential[0] = bufferPtr[ offset_in_bytes + 0]; | |
|
497 | spacecraft_potential[1] = bufferPtr[ offset_in_bytes + 1]; | |
|
498 | spacecraft_potential[2] = bufferPtr[ offset_in_bytes + 2]; | |
|
499 | spacecraft_potential[3] = bufferPtr[ offset_in_bytes + 3]; | |
|
500 | spacecraft_potential[4] = bufferPtr[ offset_in_bytes + 4]; | |
|
501 | spacecraft_potential[5] = bufferPtr[ offset_in_bytes + 5]; | |
|
502 | } | |
|
503 | } | |
|
504 | ||
|
505 | void get_v_e1_e2_f3_alt( unsigned char *spacecraft_potential ) | |
|
506 | { | |
|
507 | 446 | unsigned long long int localTime_asLong; |
|
508 | 447 | unsigned long long int f3_0_AcquisitionTime_asLong; |
|
509 | 448 | unsigned long long int f3_1_AcquisitionTime_asLong; |
@@ -16,6 +16,12 | |||
|
16 | 16 | rtems_name semq_name; |
|
17 | 17 | rtems_id semq_id; |
|
18 | 18 | |
|
19 | //***************** | |
|
20 | // waveform headers | |
|
21 | Header_TM_LFR_SCIENCE_CWF_t headerCWF; | |
|
22 | Header_TM_LFR_SCIENCE_SWF_t headerSWF; | |
|
23 | Header_TM_LFR_SCIENCE_ASM_t headerASM; | |
|
24 | ||
|
19 | 25 | //*********** |
|
20 | 26 | // RTEMS TASK |
|
21 | 27 | rtems_task spiq_task(rtems_task_argument unused) |
@@ -204,10 +210,23 rtems_task send_task( rtems_task_argumen | |||
|
204 | 210 | |
|
205 | 211 | rtems_status_code status; // RTEMS status code |
|
206 | 212 | char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer |
|
213 | ring_node *incomingRingNodePtr; | |
|
214 | int ring_node_address; | |
|
215 | char *charPtr; | |
|
207 | 216 | spw_ioctl_pkt_send *spw_ioctl_send; |
|
208 | 217 | size_t size; // size of the incoming TC packet |
|
209 | 218 | u_int32_t count; |
|
210 | 219 | rtems_id queue_id; |
|
220 | unsigned char sid; | |
|
221 | ||
|
222 | incomingRingNodePtr = NULL; | |
|
223 | ring_node_address = 0; | |
|
224 | charPtr = (char *) &ring_node_address; | |
|
225 | sid = 0; | |
|
226 | ||
|
227 | init_header_cwf( &headerCWF ); | |
|
228 | init_header_swf( &headerSWF ); | |
|
229 | init_header_asm( &headerASM ); | |
|
211 | 230 | |
|
212 | 231 | status = get_message_queue_id_send( &queue_id ); |
|
213 | 232 | if (status != RTEMS_SUCCESSFUL) |
@@ -228,7 +247,40 rtems_task send_task( rtems_task_argumen | |||
|
228 | 247 | } |
|
229 | 248 | else |
|
230 | 249 | { |
|
231 | if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet | |
|
250 | if ( size == sizeof(ring_node*) ) | |
|
251 | { | |
|
252 | charPtr[0] = incomingData[0]; | |
|
253 | charPtr[1] = incomingData[1]; | |
|
254 | charPtr[2] = incomingData[2]; | |
|
255 | charPtr[3] = incomingData[3]; | |
|
256 | incomingRingNodePtr = (ring_node*) ring_node_address; | |
|
257 | sid = incomingRingNodePtr->sid; | |
|
258 | // printf("sid = %d\n", incomingRingNodePtr->sid); | |
|
259 | if ( (sid==SID_NORM_CWF_LONG_F3) | |
|
260 | || (sid==SID_BURST_CWF_F2 ) | |
|
261 | || (sid==SID_SBM1_CWF_F1 ) | |
|
262 | || (sid==SID_SBM2_CWF_F2 )) | |
|
263 | { | |
|
264 | spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF ); | |
|
265 | } | |
|
266 | else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) ) | |
|
267 | { | |
|
268 | spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF ); | |
|
269 | } | |
|
270 | else if ( (sid==SID_NORM_CWF_F3) ) | |
|
271 | { | |
|
272 | spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF ); | |
|
273 | } | |
|
274 | else if ( (sid==SID_NORM_ASM_F0) || (SID_NORM_ASM_F1) || (SID_NORM_ASM_F2) ) | |
|
275 | { | |
|
276 | spw_send_asm( incomingRingNodePtr, &headerASM ); | |
|
277 | } | |
|
278 | else | |
|
279 | { | |
|
280 | printf("unexpected sid = %d\n", sid); | |
|
281 | } | |
|
282 | } | |
|
283 | else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet | |
|
232 | 284 | { |
|
233 | 285 | status = write( fdSPW, incomingData, size ); |
|
234 | 286 | if (status == -1){ |
@@ -240,6 +292,13 rtems_task send_task( rtems_task_argumen | |||
|
240 | 292 | spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData; |
|
241 | 293 | status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send ); |
|
242 | 294 | if (status == -1){ |
|
295 | printf("size = %d, %x, %x, %x, %x, %x\n", | |
|
296 | size, | |
|
297 | incomingData[0], | |
|
298 | incomingData[1], | |
|
299 | incomingData[2], | |
|
300 | incomingData[3], | |
|
301 | incomingData[4]); | |
|
243 | 302 | PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status) |
|
244 | 303 | } |
|
245 | 304 | } |
@@ -393,7 +452,7 int spacewire_configure_link( int fd ) | |||
|
393 | 452 | PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n") |
|
394 | 453 | } |
|
395 | 454 | // |
|
396 |
status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, |
|
|
455 | status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks | |
|
397 | 456 | if (status!=RTEMS_SUCCESSFUL) { |
|
398 | 457 | PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n") |
|
399 | 458 | } |
@@ -622,3 +681,423 rtems_timer_service_routine user_routine | |||
|
622 | 681 | status = RTEMS_SUCCESSFUL; |
|
623 | 682 | } |
|
624 | 683 | } |
|
684 | ||
|
685 | void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header ) | |
|
686 | { | |
|
687 | header->targetLogicalAddress = CCSDS_DESTINATION_ID; | |
|
688 | header->protocolIdentifier = CCSDS_PROTOCOLE_ID; | |
|
689 | header->reserved = DEFAULT_RESERVED; | |
|
690 | header->userApplication = CCSDS_USER_APP; | |
|
691 | header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; | |
|
692 | header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT; | |
|
693 | header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8); | |
|
694 | header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 ); | |
|
695 | // DATA FIELD HEADER | |
|
696 | header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2; | |
|
697 | header->serviceType = TM_TYPE_LFR_SCIENCE; // service type | |
|
698 | header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype | |
|
699 | header->destinationID = TM_DESTINATION_ID_GROUND; | |
|
700 | header->time[0] = 0x00; | |
|
701 | header->time[0] = 0x00; | |
|
702 | header->time[0] = 0x00; | |
|
703 | header->time[0] = 0x00; | |
|
704 | header->time[0] = 0x00; | |
|
705 | header->time[0] = 0x00; | |
|
706 | // AUXILIARY DATA HEADER | |
|
707 | header->sid = 0x00; | |
|
708 | header->hkBIA = DEFAULT_HKBIA; | |
|
709 | header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8); | |
|
710 | header->blkNr[1] = (unsigned char) (BLK_NR_CWF ); | |
|
711 | } | |
|
712 | ||
|
713 | void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header ) | |
|
714 | { | |
|
715 | header->targetLogicalAddress = CCSDS_DESTINATION_ID; | |
|
716 | header->protocolIdentifier = CCSDS_PROTOCOLE_ID; | |
|
717 | header->reserved = DEFAULT_RESERVED; | |
|
718 | header->userApplication = CCSDS_USER_APP; | |
|
719 | header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8); | |
|
720 | header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); | |
|
721 | header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; | |
|
722 | header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT; | |
|
723 | header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8); | |
|
724 | header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 ); | |
|
725 | // DATA FIELD HEADER | |
|
726 | header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2; | |
|
727 | header->serviceType = TM_TYPE_LFR_SCIENCE; // service type | |
|
728 | header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype | |
|
729 | header->destinationID = TM_DESTINATION_ID_GROUND; | |
|
730 | header->time[0] = 0x00; | |
|
731 | header->time[0] = 0x00; | |
|
732 | header->time[0] = 0x00; | |
|
733 | header->time[0] = 0x00; | |
|
734 | header->time[0] = 0x00; | |
|
735 | header->time[0] = 0x00; | |
|
736 | // AUXILIARY DATA HEADER | |
|
737 | header->sid = 0x00; | |
|
738 | header->hkBIA = DEFAULT_HKBIA; | |
|
739 | header->pktCnt = DEFAULT_PKTCNT; // PKT_CNT | |
|
740 | header->pktNr = 0x00; | |
|
741 | header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8); | |
|
742 | header->blkNr[1] = (unsigned char) (BLK_NR_CWF ); | |
|
743 | } | |
|
744 | ||
|
745 | void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header ) | |
|
746 | { | |
|
747 | header->targetLogicalAddress = CCSDS_DESTINATION_ID; | |
|
748 | header->protocolIdentifier = CCSDS_PROTOCOLE_ID; | |
|
749 | header->reserved = DEFAULT_RESERVED; | |
|
750 | header->userApplication = CCSDS_USER_APP; | |
|
751 | header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8); | |
|
752 | header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); | |
|
753 | header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; | |
|
754 | header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT; | |
|
755 | header->packetLength[0] = 0x00; | |
|
756 | header->packetLength[1] = 0x00; | |
|
757 | // DATA FIELD HEADER | |
|
758 | header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2; | |
|
759 | header->serviceType = TM_TYPE_LFR_SCIENCE; // service type | |
|
760 | header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype | |
|
761 | header->destinationID = TM_DESTINATION_ID_GROUND; | |
|
762 | header->time[0] = 0x00; | |
|
763 | header->time[0] = 0x00; | |
|
764 | header->time[0] = 0x00; | |
|
765 | header->time[0] = 0x00; | |
|
766 | header->time[0] = 0x00; | |
|
767 | header->time[0] = 0x00; | |
|
768 | // AUXILIARY DATA HEADER | |
|
769 | header->sid = 0x00; | |
|
770 | header->biaStatusInfo = 0x00; | |
|
771 | header->pa_lfr_pkt_cnt_asm = 0x00; | |
|
772 | header->pa_lfr_pkt_nr_asm = 0x00; | |
|
773 | header->pa_lfr_asm_blk_nr[0] = 0x00; | |
|
774 | header->pa_lfr_asm_blk_nr[1] = 0x00; | |
|
775 | } | |
|
776 | ||
|
777 | int spw_send_waveform_CWF( ring_node *ring_node_to_send, | |
|
778 | Header_TM_LFR_SCIENCE_CWF_t *header ) | |
|
779 | { | |
|
780 | /** This function sends CWF CCSDS packets (F2, F1 or F0). | |
|
781 | * | |
|
782 | * @param waveform points to the buffer containing the data that will be send. | |
|
783 | * @param sid is the source identifier of the data that will be sent. | |
|
784 | * @param headerCWF points to a table of headers that have been prepared for the data transmission. | |
|
785 | * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures | |
|
786 | * contain information to setup the transmission of the data packets. | |
|
787 | * | |
|
788 | * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks. | |
|
789 | * | |
|
790 | */ | |
|
791 | ||
|
792 | unsigned int i; | |
|
793 | int ret; | |
|
794 | unsigned int coarseTime; | |
|
795 | unsigned int fineTime; | |
|
796 | rtems_status_code status; | |
|
797 | spw_ioctl_pkt_send spw_ioctl_send_CWF; | |
|
798 | int *dataPtr; | |
|
799 | unsigned char sid; | |
|
800 | ||
|
801 | spw_ioctl_send_CWF.hlen = TM_HEADER_LEN + 4 + 10; // + 4 is for the protocole extra header, + 10 is for the auxiliary header | |
|
802 | spw_ioctl_send_CWF.options = 0; | |
|
803 | ||
|
804 | ret = LFR_DEFAULT; | |
|
805 | sid = (unsigned char) ring_node_to_send->sid; | |
|
806 | ||
|
807 | coarseTime = ring_node_to_send->coarseTime; | |
|
808 | fineTime = ring_node_to_send->fineTime; | |
|
809 | dataPtr = (int*) ring_node_to_send->buffer_address; | |
|
810 | ||
|
811 | for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform | |
|
812 | { | |
|
813 | spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ]; | |
|
814 | spw_ioctl_send_CWF.hdr = (char*) header; | |
|
815 | // BUILD THE DATA | |
|
816 | spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK; | |
|
817 | ||
|
818 | // SET PACKET SEQUENCE CONTROL | |
|
819 | increment_seq_counter_source_id( header->packetSequenceControl, sid ); | |
|
820 | ||
|
821 | // SET SID | |
|
822 | header->sid = sid; | |
|
823 | ||
|
824 | // SET PACKET TIME | |
|
825 | compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime); | |
|
826 | // | |
|
827 | header->time[0] = header->acquisitionTime[0]; | |
|
828 | header->time[1] = header->acquisitionTime[1]; | |
|
829 | header->time[2] = header->acquisitionTime[2]; | |
|
830 | header->time[3] = header->acquisitionTime[3]; | |
|
831 | header->time[4] = header->acquisitionTime[4]; | |
|
832 | header->time[5] = header->acquisitionTime[5]; | |
|
833 | ||
|
834 | // SET PACKET ID | |
|
835 | if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) ) | |
|
836 | { | |
|
837 | header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8); | |
|
838 | header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2); | |
|
839 | } | |
|
840 | else | |
|
841 | { | |
|
842 | header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8); | |
|
843 | header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); | |
|
844 | } | |
|
845 | ||
|
846 | status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF ); | |
|
847 | if (status != RTEMS_SUCCESSFUL) { | |
|
848 | printf("%d-%d, ERR %d\n", sid, i, (int) status); | |
|
849 | ret = LFR_DEFAULT; | |
|
850 | } | |
|
851 | } | |
|
852 | ||
|
853 | return ret; | |
|
854 | } | |
|
855 | ||
|
856 | int spw_send_waveform_SWF( ring_node *ring_node_to_send, | |
|
857 | Header_TM_LFR_SCIENCE_SWF_t *header ) | |
|
858 | { | |
|
859 | /** This function sends SWF CCSDS packets (F2, F1 or F0). | |
|
860 | * | |
|
861 | * @param waveform points to the buffer containing the data that will be send. | |
|
862 | * @param sid is the source identifier of the data that will be sent. | |
|
863 | * @param headerSWF points to a table of headers that have been prepared for the data transmission. | |
|
864 | * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures | |
|
865 | * contain information to setup the transmission of the data packets. | |
|
866 | * | |
|
867 | * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks. | |
|
868 | * | |
|
869 | */ | |
|
870 | ||
|
871 | unsigned int i; | |
|
872 | int ret; | |
|
873 | unsigned int coarseTime; | |
|
874 | unsigned int fineTime; | |
|
875 | rtems_status_code status; | |
|
876 | spw_ioctl_pkt_send spw_ioctl_send_SWF; | |
|
877 | int *dataPtr; | |
|
878 | unsigned char sid; | |
|
879 | ||
|
880 | spw_ioctl_send_SWF.hlen = TM_HEADER_LEN + 4 + 12; // + 4 is for the protocole extra header, + 12 is for the auxiliary header | |
|
881 | spw_ioctl_send_SWF.options = 0; | |
|
882 | ||
|
883 | ret = LFR_DEFAULT; | |
|
884 | ||
|
885 | coarseTime = ring_node_to_send->coarseTime; | |
|
886 | fineTime = ring_node_to_send->fineTime; | |
|
887 | dataPtr = (int*) ring_node_to_send->buffer_address; | |
|
888 | sid = ring_node_to_send->sid; | |
|
889 | ||
|
890 | for (i=0; i<7; i++) // send waveform | |
|
891 | { | |
|
892 | spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ]; | |
|
893 | spw_ioctl_send_SWF.hdr = (char*) header; | |
|
894 | ||
|
895 | // SET PACKET SEQUENCE CONTROL | |
|
896 | increment_seq_counter_source_id( header->packetSequenceControl, sid ); | |
|
897 | ||
|
898 | // SET PACKET LENGTH AND BLKNR | |
|
899 | if (i == 6) | |
|
900 | { | |
|
901 | spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK; | |
|
902 | header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8); | |
|
903 | header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 ); | |
|
904 | header->blkNr[0] = (unsigned char) (BLK_NR_224 >> 8); | |
|
905 | header->blkNr[1] = (unsigned char) (BLK_NR_224 ); | |
|
906 | } | |
|
907 | else | |
|
908 | { | |
|
909 | spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK; | |
|
910 | header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8); | |
|
911 | header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 ); | |
|
912 | header->blkNr[0] = (unsigned char) (BLK_NR_304 >> 8); | |
|
913 | header->blkNr[1] = (unsigned char) (BLK_NR_304 ); | |
|
914 | } | |
|
915 | ||
|
916 | // SET PACKET TIME | |
|
917 | compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime ); | |
|
918 | // | |
|
919 | header->time[0] = header->acquisitionTime[0]; | |
|
920 | header->time[1] = header->acquisitionTime[1]; | |
|
921 | header->time[2] = header->acquisitionTime[2]; | |
|
922 | header->time[3] = header->acquisitionTime[3]; | |
|
923 | header->time[4] = header->acquisitionTime[4]; | |
|
924 | header->time[5] = header->acquisitionTime[5]; | |
|
925 | ||
|
926 | // SET SID | |
|
927 | header->sid = sid; | |
|
928 | ||
|
929 | // SET PKTNR | |
|
930 | header->pktNr = i+1; // PKT_NR | |
|
931 | ||
|
932 | // SEND PACKET | |
|
933 | status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF ); | |
|
934 | if (status != RTEMS_SUCCESSFUL) { | |
|
935 | printf("%d-%d, ERR %d\n", sid, i, (int) status); | |
|
936 | ret = LFR_DEFAULT; | |
|
937 | } | |
|
938 | } | |
|
939 | ||
|
940 | return ret; | |
|
941 | } | |
|
942 | ||
|
943 | int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send, | |
|
944 | Header_TM_LFR_SCIENCE_CWF_t *header ) | |
|
945 | { | |
|
946 | /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data. | |
|
947 | * | |
|
948 | * @param waveform points to the buffer containing the data that will be send. | |
|
949 | * @param headerCWF points to a table of headers that have been prepared for the data transmission. | |
|
950 | * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures | |
|
951 | * contain information to setup the transmission of the data packets. | |
|
952 | * | |
|
953 | * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer | |
|
954 | * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks. | |
|
955 | * | |
|
956 | */ | |
|
957 | ||
|
958 | unsigned int i; | |
|
959 | int ret; | |
|
960 | unsigned int coarseTime; | |
|
961 | unsigned int fineTime; | |
|
962 | rtems_status_code status; | |
|
963 | spw_ioctl_pkt_send spw_ioctl_send_CWF; | |
|
964 | char *dataPtr; | |
|
965 | unsigned char sid; | |
|
966 | ||
|
967 | spw_ioctl_send_CWF.hlen = TM_HEADER_LEN + 4 + 10; // + 4 is for the protocole extra header, + 10 is for the auxiliary header | |
|
968 | spw_ioctl_send_CWF.options = 0; | |
|
969 | ||
|
970 | ret = LFR_DEFAULT; | |
|
971 | sid = ring_node_to_send->sid; | |
|
972 | ||
|
973 | coarseTime = ring_node_to_send->coarseTime; | |
|
974 | fineTime = ring_node_to_send->fineTime; | |
|
975 | dataPtr = (char*) ring_node_to_send->buffer_address; | |
|
976 | ||
|
977 | //********************* | |
|
978 | // SEND CWF3_light DATA | |
|
979 | for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform | |
|
980 | { | |
|
981 | spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ]; | |
|
982 | spw_ioctl_send_CWF.hdr = (char*) header; | |
|
983 | // BUILD THE DATA | |
|
984 | spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK; | |
|
985 | ||
|
986 | // SET PACKET SEQUENCE COUNTER | |
|
987 | increment_seq_counter_source_id( header->packetSequenceControl, sid ); | |
|
988 | ||
|
989 | // SET SID | |
|
990 | header->sid = sid; | |
|
991 | ||
|
992 | // SET PACKET TIME | |
|
993 | compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime ); | |
|
994 | // | |
|
995 | header->time[0] = header->acquisitionTime[0]; | |
|
996 | header->time[1] = header->acquisitionTime[1]; | |
|
997 | header->time[2] = header->acquisitionTime[2]; | |
|
998 | header->time[3] = header->acquisitionTime[3]; | |
|
999 | header->time[4] = header->acquisitionTime[4]; | |
|
1000 | header->time[5] = header->acquisitionTime[5]; | |
|
1001 | ||
|
1002 | // SET PACKET ID | |
|
1003 | header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8); | |
|
1004 | header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); | |
|
1005 | ||
|
1006 | // SEND PACKET | |
|
1007 | status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF ); | |
|
1008 | if (status != RTEMS_SUCCESSFUL) { | |
|
1009 | printf("%d-%d, ERR %d\n", sid, i, (int) status); | |
|
1010 | ret = LFR_DEFAULT; | |
|
1011 | } | |
|
1012 | } | |
|
1013 | ||
|
1014 | return ret; | |
|
1015 | } | |
|
1016 | ||
|
1017 | void spw_send_asm( ring_node *ring_node_to_send, | |
|
1018 | Header_TM_LFR_SCIENCE_ASM_t *header ) | |
|
1019 | { | |
|
1020 | unsigned int i; | |
|
1021 | unsigned int length = 0; | |
|
1022 | rtems_status_code status; | |
|
1023 | unsigned int sid; | |
|
1024 | char *spectral_matrix; | |
|
1025 | int coarseTime; | |
|
1026 | int fineTime; | |
|
1027 | spw_ioctl_pkt_send spw_ioctl_send_ASM; | |
|
1028 | ||
|
1029 | sid = ring_node_to_send->sid; | |
|
1030 | spectral_matrix = (char*) ring_node_to_send->buffer_address; | |
|
1031 | coarseTime = ring_node_to_send->coarseTime; | |
|
1032 | fineTime = ring_node_to_send->fineTime; | |
|
1033 | ||
|
1034 | for (i=0; i<2; i++) | |
|
1035 | { | |
|
1036 | // (1) BUILD THE DATA | |
|
1037 | switch(sid) | |
|
1038 | { | |
|
1039 | case SID_NORM_ASM_F0: | |
|
1040 | spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F0_IN_BYTES / 2; // 2 packets will be sent | |
|
1041 | spw_ioctl_send_ASM.data = &spectral_matrix[ | |
|
1042 | ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0) ) * NB_VALUES_PER_SM ) * 2 | |
|
1043 | ]; | |
|
1044 | length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0; | |
|
1045 | header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0) >> 8 ); // BLK_NR MSB | |
|
1046 | header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0); // BLK_NR LSB | |
|
1047 | break; | |
|
1048 | case SID_NORM_ASM_F1: | |
|
1049 | spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F1_IN_BYTES / 2; // 2 packets will be sent | |
|
1050 | spw_ioctl_send_ASM.data = &spectral_matrix[ | |
|
1051 | ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1) ) * NB_VALUES_PER_SM ) * 2 | |
|
1052 | ]; | |
|
1053 | length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1; | |
|
1054 | header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1) >> 8 ); // BLK_NR MSB | |
|
1055 | header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1); // BLK_NR LSB | |
|
1056 | break; | |
|
1057 | case SID_NORM_ASM_F2: | |
|
1058 | spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F2_IN_BYTES / 2; // 2 packets will be sent | |
|
1059 | spw_ioctl_send_ASM.data = &spectral_matrix[ | |
|
1060 | ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM ) * 2 | |
|
1061 | ]; | |
|
1062 | length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2; | |
|
1063 | header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB | |
|
1064 | header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB | |
|
1065 | break; | |
|
1066 | default: | |
|
1067 | PRINTF1("ERR *** in spw_send_asm *** unexpected sid %d\n", sid) | |
|
1068 | break; | |
|
1069 | } | |
|
1070 | spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM + CCSDS_PROTOCOLE_EXTRA_BYTES; | |
|
1071 | spw_ioctl_send_ASM.hdr = (char *) header; | |
|
1072 | spw_ioctl_send_ASM.options = 0; | |
|
1073 | ||
|
1074 | // (2) BUILD THE HEADER | |
|
1075 | increment_seq_counter_source_id( header->packetSequenceControl, sid ); | |
|
1076 | header->packetLength[0] = (unsigned char) (length>>8); | |
|
1077 | header->packetLength[1] = (unsigned char) (length); | |
|
1078 | header->sid = (unsigned char) sid; // SID | |
|
1079 | header->pa_lfr_pkt_cnt_asm = 2; | |
|
1080 | header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1); | |
|
1081 | ||
|
1082 | // (3) SET PACKET TIME | |
|
1083 | header->time[0] = (unsigned char) (coarseTime>>24); | |
|
1084 | header->time[1] = (unsigned char) (coarseTime>>16); | |
|
1085 | header->time[2] = (unsigned char) (coarseTime>>8); | |
|
1086 | header->time[3] = (unsigned char) (coarseTime); | |
|
1087 | header->time[4] = (unsigned char) (fineTime>>8); | |
|
1088 | header->time[5] = (unsigned char) (fineTime); | |
|
1089 | // | |
|
1090 | header->acquisitionTime[0] = header->time[0]; | |
|
1091 | header->acquisitionTime[1] = header->time[1]; | |
|
1092 | header->acquisitionTime[2] = header->time[2]; | |
|
1093 | header->acquisitionTime[3] = header->time[3]; | |
|
1094 | header->acquisitionTime[4] = header->time[4]; | |
|
1095 | header->acquisitionTime[5] = header->time[5]; | |
|
1096 | ||
|
1097 | // (4) SEND PACKET | |
|
1098 | status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM ); | |
|
1099 | if (status != RTEMS_SUCCESSFUL) { | |
|
1100 | printf("in ASM_send *** ERR %d\n", (int) status); | |
|
1101 | } | |
|
1102 | } | |
|
1103 | } |
@@ -14,8 +14,11 nb_sm_before_bp_asm_f0 nb_sm_before_f0; | |||
|
14 | 14 | |
|
15 | 15 | //*** |
|
16 | 16 | // F0 |
|
17 | ring_node_asm asm_ring_norm_f0 [ NB_RING_NODES_ASM_NORM_F0 ]; | |
|
18 | ring_node_asm asm_ring_burst_sbm_f0[ NB_RING_NODES_ASM_BURST_SBM_F0 ]; | |
|
17 | ring_node_asm asm_ring_norm_f0 [ NB_RING_NODES_ASM_NORM_F0 ]; | |
|
18 | ring_node_asm asm_ring_burst_sbm_f0 [ NB_RING_NODES_ASM_BURST_SBM_F0 ]; | |
|
19 | ||
|
20 | ring_node ring_to_send_asm_f0 [ NB_RING_NODES_ASM_F0 ]; | |
|
21 | char buffer_asm_f0 [ NB_RING_NODES_ASM_F0 * TOTAL_SIZE_SM ]; | |
|
19 | 22 | |
|
20 | 23 | float asm_f0_reorganized [ TOTAL_SIZE_SM ]; |
|
21 | 24 | char asm_f0_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ]; |
@@ -188,11 +191,16 rtems_task prc0_task( rtems_task_argumen | |||
|
188 | 191 | bp_packet packet_norm_bp2_f0; |
|
189 | 192 | bp_packet packet_sbm_bp1_f0; |
|
190 | 193 | bp_packet packet_sbm_bp2_f0; |
|
194 | ring_node *current_ring_node_to_send_asm_f0; | |
|
191 | 195 | |
|
192 | 196 | unsigned long long int localTime; |
|
193 | 197 | |
|
194 | 198 | ASM_init_header( &headerASM ); |
|
195 | 199 | |
|
200 | // init the ring of the averaged spectral matrices which will be transmitted to the DPU | |
|
201 | init_ring( ring_to_send_asm_f0, NB_RING_NODES_ASM_F0, (volatile int*) buffer_asm_f0, TOTAL_SIZE_SM ); | |
|
202 | current_ring_node_to_send_asm_f0 = ring_to_send_asm_f0; | |
|
203 | ||
|
196 | 204 | //************* |
|
197 | 205 | // NORM headers |
|
198 | 206 | BP_init_header_with_spare( &packet_norm_bp1_f0.header, |
@@ -327,14 +335,27 rtems_task prc0_task( rtems_task_argumen | |||
|
327 | 335 | |
|
328 | 336 | if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F0) |
|
329 | 337 | { |
|
338 | // // 1) reorganize the ASM and divide | |
|
339 | // ASM_reorganize_and_divide( incomingMsg->norm->matrix, | |
|
340 | // asm_f0_reorganized, | |
|
341 | // nb_sm_before_f0.norm_bp1 ); | |
|
342 | // // 2) convert the float array in a char array | |
|
343 | // ASM_convert( asm_f0_reorganized, (char*) current_ring_node_to_send_asm_f0->buffer_address ); | |
|
344 | // current_ring_node_to_send_asm_f0->coarseTime = incomingMsg->coarseTime; | |
|
345 | // current_ring_node_to_send_asm_f0->fineTime = incomingMsg->fineTime; | |
|
346 | // current_ring_node_to_send_asm_f0->sid = SID_NORM_ASM_F0; | |
|
347 | // // 3) send the spectral matrix packets | |
|
348 | // status = rtems_message_queue_send( queue_id, ¤t_ring_node_to_send_asm_f0, sizeof( ring_node* ) ); | |
|
349 | // // change asm ring node | |
|
350 | // current_ring_node_to_send_asm_f0 = current_ring_node_to_send_asm_f0->next; | |
|
330 | 351 | // 1) reorganize the ASM and divide |
|
331 | 352 | ASM_reorganize_and_divide( incomingMsg->norm->matrix, |
|
332 |
|
|
|
333 |
|
|
|
353 | asm_f0_reorganized, | |
|
354 | nb_sm_before_f0.norm_bp1 ); | |
|
334 | 355 | // 2) convert the float array in a char array |
|
335 | 356 | ASM_convert( asm_f0_reorganized, asm_f0_char); |
|
336 | 357 | // 3) send the spectral matrix packets |
|
337 |
set_time( headerASM.time |
|
|
358 | set_time( headerASM.time , (unsigned char *) &incomingMsg->coarseTime ); | |
|
338 | 359 | set_time( headerASM.acquisitionTime, (unsigned char *) &incomingMsg->coarseTime ); |
|
339 | 360 | ASM_send( &headerASM, asm_f0_char, SID_NORM_ASM_F0, &spw_ioctl_send_ASM, queue_id); |
|
340 | 361 | } |
@@ -261,102 +261,78 void reset_nb_sm( void ) | |||
|
261 | 261 | nb_sm_f1 = 0; |
|
262 | 262 | } |
|
263 | 263 | |
|
264 | //void SM_init_rings_alt( void ) | |
|
265 | //{ | |
|
266 | // init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM ); | |
|
267 | // init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f0, TOTAL_SIZE_SM ); | |
|
268 | // init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f0, TOTAL_SIZE_SM ); | |
|
269 | ||
|
270 | // DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0) | |
|
271 | // DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1) | |
|
272 | // DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2) | |
|
273 | // DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0) | |
|
274 | // DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1) | |
|
275 | // DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2) | |
|
276 | //} | |
|
277 | ||
|
264 | 278 | void SM_init_rings( void ) |
|
265 | 279 | { |
|
266 | 280 | unsigned char i; |
|
267 | ||
|
268 | 281 | // F0 RING |
|
269 |
sm_ring_f0[0].next |
|
|
270 |
sm_ring_f0[0].previous |
|
|
271 |
sm_ring_f0[0].buffer_address |
|
|
282 | sm_ring_f0[0].next = (ring_node_sm*) &sm_ring_f0[1]; | |
|
283 | sm_ring_f0[0].previous = (ring_node_sm*) &sm_ring_f0[NB_RING_NODES_SM_F0-1]; | |
|
284 | sm_ring_f0[0].buffer_address = | |
|
272 | 285 | (int) &sm_f0[ 0 ]; |
|
273 | ||
|
274 |
sm_ring_f0[NB_RING_NODES_SM_F0-1]. |
|
|
275 | sm_ring_f0[NB_RING_NODES_SM_F0-1].previous = (ring_node_sm*) &sm_ring_f0[NB_RING_NODES_SM_F0-2]; | |
|
286 | sm_ring_f0[NB_RING_NODES_SM_F0-1].next = (ring_node_sm*) &sm_ring_f0[0]; | |
|
287 | sm_ring_f0[NB_RING_NODES_SM_F0-1].previous = (ring_node_sm*) &sm_ring_f0[NB_RING_NODES_SM_F0-2]; | |
|
276 | 288 | sm_ring_f0[NB_RING_NODES_SM_F0-1].buffer_address = |
|
277 | 289 | (int) &sm_f0[ (NB_RING_NODES_SM_F0-1) * TOTAL_SIZE_SM ]; |
|
278 | ||
|
279 | 290 | for(i=1; i<NB_RING_NODES_SM_F0-1; i++) |
|
280 | 291 | { |
|
281 |
sm_ring_f0[i].next |
|
|
282 |
sm_ring_f0[i].previous |
|
|
283 |
sm_ring_f0[i].buffer_address |
|
|
292 | sm_ring_f0[i].next = (ring_node_sm*) &sm_ring_f0[i+1]; | |
|
293 | sm_ring_f0[i].previous = (ring_node_sm*) &sm_ring_f0[i-1]; | |
|
294 | sm_ring_f0[i].buffer_address = | |
|
284 | 295 | (int) &sm_f0[ i * TOTAL_SIZE_SM ]; |
|
285 | 296 | } |
|
286 | ||
|
287 | 297 | // F1 RING |
|
288 |
sm_ring_f1[0].next |
|
|
289 |
sm_ring_f1[0].previous |
|
|
290 |
sm_ring_f1[0].buffer_address |
|
|
298 | sm_ring_f1[0].next = (ring_node_sm*) &sm_ring_f1[1]; | |
|
299 | sm_ring_f1[0].previous = (ring_node_sm*) &sm_ring_f1[NB_RING_NODES_SM_F1-1]; | |
|
300 | sm_ring_f1[0].buffer_address = | |
|
291 | 301 | (int) &sm_f1[ 0 ]; |
|
292 | ||
|
293 |
sm_ring_f1[NB_RING_NODES_SM_F1-1]. |
|
|
294 | sm_ring_f1[NB_RING_NODES_SM_F1-1].previous = (ring_node_sm*) &sm_ring_f1[NB_RING_NODES_SM_F1-2]; | |
|
302 | sm_ring_f1[NB_RING_NODES_SM_F1-1].next = (ring_node_sm*) &sm_ring_f1[0]; | |
|
303 | sm_ring_f1[NB_RING_NODES_SM_F1-1].previous = (ring_node_sm*) &sm_ring_f1[NB_RING_NODES_SM_F1-2]; | |
|
295 | 304 | sm_ring_f1[NB_RING_NODES_SM_F1-1].buffer_address = |
|
296 | 305 | (int) &sm_f1[ (NB_RING_NODES_SM_F1-1) * TOTAL_SIZE_SM ]; |
|
297 | ||
|
298 | 306 | for(i=1; i<NB_RING_NODES_SM_F1-1; i++) |
|
299 | 307 | { |
|
300 |
sm_ring_f1[i].next |
|
|
301 |
sm_ring_f1[i].previous |
|
|
302 |
sm_ring_f1[i].buffer_address |
|
|
308 | sm_ring_f1[i].next = (ring_node_sm*) &sm_ring_f1[i+1]; | |
|
309 | sm_ring_f1[i].previous = (ring_node_sm*) &sm_ring_f1[i-1]; | |
|
310 | sm_ring_f1[i].buffer_address = | |
|
303 | 311 | (int) &sm_f1[ i * TOTAL_SIZE_SM ]; |
|
304 | 312 | } |
|
305 | ||
|
306 | 313 | // F2 RING |
|
307 |
sm_ring_f2[0].next |
|
|
308 |
sm_ring_f2[0].previous |
|
|
309 |
sm_ring_f2[0].buffer_address |
|
|
314 | sm_ring_f2[0].next = (ring_node_sm*) &sm_ring_f2[1]; | |
|
315 | sm_ring_f2[0].previous = (ring_node_sm*) &sm_ring_f2[NB_RING_NODES_SM_F2-1]; | |
|
316 | sm_ring_f2[0].buffer_address = | |
|
310 | 317 | (int) &sm_f2[ 0 ]; |
|
311 | ||
|
312 |
sm_ring_f2[NB_RING_NODES_SM_F2-1]. |
|
|
313 | sm_ring_f2[NB_RING_NODES_SM_F2-1].previous = (ring_node_sm*) &sm_ring_f2[NB_RING_NODES_SM_F2-2]; | |
|
318 | sm_ring_f2[NB_RING_NODES_SM_F2-1].next = (ring_node_sm*) &sm_ring_f2[0]; | |
|
319 | sm_ring_f2[NB_RING_NODES_SM_F2-1].previous = (ring_node_sm*) &sm_ring_f2[NB_RING_NODES_SM_F2-2]; | |
|
314 | 320 | sm_ring_f2[NB_RING_NODES_SM_F2-1].buffer_address = |
|
315 | 321 | (int) &sm_f2[ (NB_RING_NODES_SM_F2-1) * TOTAL_SIZE_SM ]; |
|
316 | ||
|
317 | 322 | for(i=1; i<NB_RING_NODES_SM_F2-1; i++) |
|
318 | 323 | { |
|
319 |
sm_ring_f2[i].next |
|
|
320 |
sm_ring_f2[i].previous |
|
|
321 |
sm_ring_f2[i].buffer_address |
|
|
324 | sm_ring_f2[i].next = (ring_node_sm*) &sm_ring_f2[i+1]; | |
|
325 | sm_ring_f2[i].previous = (ring_node_sm*) &sm_ring_f2[i-1]; | |
|
326 | sm_ring_f2[i].buffer_address = | |
|
322 | 327 | (int) &sm_f2[ i * TOTAL_SIZE_SM ]; |
|
323 | 328 | } |
|
324 | ||
|
325 | 329 | DEBUG_PRINTF1("asm_ring_f0 @%x\n", (unsigned int) sm_ring_f0) |
|
326 | DEBUG_PRINTF1("asm_ring_f1 @%x\n", (unsigned int) sm_ring_f1) | |
|
327 | DEBUG_PRINTF1("asm_ring_f2 @%x\n", (unsigned int) sm_ring_f2) | |
|
328 | ||
|
329 | spectral_matrix_regs->f0_0_address = sm_ring_f0[0].buffer_address; | |
|
330 | DEBUG_PRINTF1("asm_ring_f1 @%x\n", (unsigned int) sm_ring_f1) | |
|
331 | DEBUG_PRINTF1("asm_ring_f2 @%x\n", (unsigned int) sm_ring_f2) | |
|
332 | spectral_matrix_regs->f0_0_address = sm_ring_f0[0].buffer_address; | |
|
330 | 333 | DEBUG_PRINTF1("spectral_matrix_regs->matrixF0_Address0 @%x\n", spectral_matrix_regs->f0_0_address) |
|
331 | 334 | } |
|
332 | 335 | |
|
333 | void SM_generic_init_ring( ring_node_sm *ring, unsigned char nbNodes, volatile int sm_f[] ) | |
|
334 | { | |
|
335 | unsigned char i; | |
|
336 | ||
|
337 | //*************** | |
|
338 | // BUFFER ADDRESS | |
|
339 | for(i=0; i<nbNodes; i++) | |
|
340 | { | |
|
341 | ring[ i ].buffer_address = (int) &sm_f[ i * TOTAL_SIZE_SM ]; | |
|
342 | } | |
|
343 | ||
|
344 | //***** | |
|
345 | // NEXT | |
|
346 | ring[ nbNodes - 1 ].next = (ring_node_sm*) &ring[ 0 ]; | |
|
347 | for(i=0; i<nbNodes-1; i++) | |
|
348 | { | |
|
349 | ring[ i ].next = (ring_node_sm*) &ring[ i + 1 ]; | |
|
350 | } | |
|
351 | ||
|
352 | //********* | |
|
353 | // PREVIOUS | |
|
354 | ring[ 0 ].previous = (ring_node_sm*) &ring[ nbNodes -1 ]; | |
|
355 | for(i=1; i<nbNodes; i++) | |
|
356 | { | |
|
357 | ring[ i ].previous = (ring_node_sm*) &ring[ i - 1 ]; | |
|
358 | } | |
|
359 | } | |
|
360 | 336 | |
|
361 | 337 | void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes ) |
|
362 | 338 | { |
@@ -399,17 +375,17 void ASM_init_header( Header_TM_LFR_SCIE | |||
|
399 | 375 | header->serviceType = TM_TYPE_LFR_SCIENCE; // service type |
|
400 | 376 | header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype |
|
401 | 377 | header->destinationID = TM_DESTINATION_ID_GROUND; |
|
402 | // AUXILIARY DATA HEADER | |
|
403 | header->sid = 0x00; | |
|
404 | header->biaStatusInfo = 0x00; | |
|
405 | header->pa_lfr_pkt_cnt_asm = 0x00; | |
|
406 | header->pa_lfr_pkt_nr_asm = 0x00; | |
|
407 | 378 | header->time[0] = 0x00; |
|
408 | 379 | header->time[0] = 0x00; |
|
409 | 380 | header->time[0] = 0x00; |
|
410 | 381 | header->time[0] = 0x00; |
|
411 | 382 | header->time[0] = 0x00; |
|
412 | 383 | header->time[0] = 0x00; |
|
384 | // AUXILIARY DATA HEADER | |
|
385 | header->sid = 0x00; | |
|
386 | header->biaStatusInfo = 0x00; | |
|
387 | header->pa_lfr_pkt_cnt_asm = 0x00; | |
|
388 | header->pa_lfr_pkt_nr_asm = 0x00; | |
|
413 | 389 | header->pa_lfr_asm_blk_nr[0] = 0x00; // BLK_NR MSB |
|
414 | 390 | header->pa_lfr_asm_blk_nr[1] = 0x00; // BLK_NR LSB |
|
415 | 391 | } |
@@ -9,19 +9,6 | |||
|
9 | 9 | |
|
10 | 10 | #include "wf_handler.h" |
|
11 | 11 | |
|
12 | //***************** | |
|
13 | // waveform headers | |
|
14 | // SWF | |
|
15 | Header_TM_LFR_SCIENCE_SWF_t headerSWF_F0[7]; | |
|
16 | Header_TM_LFR_SCIENCE_SWF_t headerSWF_F1[7]; | |
|
17 | Header_TM_LFR_SCIENCE_SWF_t headerSWF_F2[7]; | |
|
18 | // CWF | |
|
19 | Header_TM_LFR_SCIENCE_CWF_t headerCWF_F1[ NB_PACKETS_PER_GROUP_OF_CWF ]; | |
|
20 | Header_TM_LFR_SCIENCE_CWF_t headerCWF_F2_BURST[ NB_PACKETS_PER_GROUP_OF_CWF ]; | |
|
21 | Header_TM_LFR_SCIENCE_CWF_t headerCWF_F2_SBM2[ NB_PACKETS_PER_GROUP_OF_CWF ]; | |
|
22 | Header_TM_LFR_SCIENCE_CWF_t headerCWF_F3[ NB_PACKETS_PER_GROUP_OF_CWF ]; | |
|
23 | Header_TM_LFR_SCIENCE_CWF_t headerCWF_F3_light[ NB_PACKETS_PER_GROUP_OF_CWF_LIGHT ]; | |
|
24 | ||
|
25 | 12 | //*************** |
|
26 | 13 | // waveform rings |
|
27 | 14 | // F0 |
@@ -62,7 +49,7 void reset_extractSWF( void ) | |||
|
62 | 49 | swf_f2_ready = false; |
|
63 | 50 | } |
|
64 | 51 | |
|
65 | void waveforms_isr_f3( void ) | |
|
52 | inline void waveforms_isr_f3( void ) | |
|
66 | 53 | { |
|
67 | 54 | rtems_status_code spare_status; |
|
68 | 55 | |
@@ -94,7 +81,7 void waveforms_isr_f3( void ) | |||
|
94 | 81 | } |
|
95 | 82 | } |
|
96 | 83 | |
|
97 | void waveforms_isr_normal( void ) | |
|
84 | inline void waveforms_isr_normal( void ) | |
|
98 | 85 | { |
|
99 | 86 | rtems_status_code status; |
|
100 | 87 | |
@@ -168,7 +155,7 void waveforms_isr_normal( void ) | |||
|
168 | 155 | } |
|
169 | 156 | } |
|
170 | 157 | |
|
171 | void waveforms_isr_burst( void ) | |
|
158 | inline void waveforms_isr_burst( void ) | |
|
172 | 159 | { |
|
173 | 160 | rtems_status_code spare_status; |
|
174 | 161 | |
@@ -197,10 +184,9 void waveforms_isr_burst( void ) | |||
|
197 | 184 | } |
|
198 | 185 | } |
|
199 | 186 | |
|
200 | void waveforms_isr_sbm1( void ) | |
|
187 | inline void waveforms_isr_sbm1( void ) | |
|
201 | 188 | { |
|
202 | 189 | rtems_status_code status; |
|
203 | rtems_status_code spare_status; | |
|
204 | 190 | |
|
205 | 191 | //*** |
|
206 | 192 | // F1 |
@@ -271,15 +257,10 void waveforms_isr_sbm1( void ) | |||
|
271 | 257 | waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; |
|
272 | 258 | waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000] |
|
273 | 259 | } |
|
274 | // start the snapshots transmission | |
|
275 | if (rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_NORMAL ) != RTEMS_SUCCESSFUL) | |
|
276 | { | |
|
277 | spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ); | |
|
278 | } | |
|
279 | 260 | } |
|
280 | 261 | } |
|
281 | 262 | |
|
282 | void waveforms_isr_sbm2( void ) | |
|
263 | inline void waveforms_isr_sbm2( void ) | |
|
283 | 264 | { |
|
284 | 265 | rtems_status_code status; |
|
285 | 266 | |
@@ -440,13 +421,12 rtems_task wfrm_task(rtems_task_argument | |||
|
440 | 421 | rtems_id queue_id; |
|
441 | 422 | rtems_status_code status; |
|
442 | 423 | bool resynchronisationEngaged; |
|
424 | ring_node *ring_node_wf_snap_extracted_ptr; | |
|
425 | ||
|
426 | ring_node_wf_snap_extracted_ptr = (ring_node *) &ring_node_wf_snap_extracted; | |
|
443 | 427 | |
|
444 | 428 | resynchronisationEngaged = false; |
|
445 | 429 | |
|
446 | init_header_snapshot_wf_table( SID_NORM_SWF_F0, headerSWF_F0 ); | |
|
447 | init_header_snapshot_wf_table( SID_NORM_SWF_F1, headerSWF_F1 ); | |
|
448 | init_header_snapshot_wf_table( SID_NORM_SWF_F2, headerSWF_F2 ); | |
|
449 | ||
|
450 | 430 | status = get_message_queue_id_send( &queue_id ); |
|
451 | 431 | if (status != RTEMS_SUCCESSFUL) |
|
452 | 432 | { |
@@ -462,7 +442,7 rtems_task wfrm_task(rtems_task_argument | |||
|
462 | 442 | RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); |
|
463 | 443 | if(resynchronisationEngaged == false) |
|
464 | 444 | { // engage resynchronisation |
|
465 |
snapshot_resynchronization( (unsigned char *) ring_node_to_send_swf_f0-> |
|
|
445 | snapshot_resynchronization( (unsigned char *) ring_node_to_send_swf_f0->coarseTime ); | |
|
466 | 446 | resynchronisationEngaged = true; |
|
467 | 447 | } |
|
468 | 448 | else |
@@ -476,23 +456,32 rtems_task wfrm_task(rtems_task_argument | |||
|
476 | 456 | if (event_out == RTEMS_EVENT_MODE_NORMAL) |
|
477 | 457 | { |
|
478 | 458 | DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_NORMAL\n") |
|
479 |
|
|
|
480 |
|
|
|
481 |
|
|
|
459 | ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0; | |
|
460 | ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1; | |
|
461 | ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2; | |
|
462 | status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) ); | |
|
463 | status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) ); | |
|
464 | status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) ); | |
|
482 | 465 | } |
|
483 | 466 | if (event_out == RTEMS_EVENT_MODE_SBM1) |
|
484 | 467 | { |
|
485 | 468 | DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM1\n") |
|
486 |
|
|
|
487 |
|
|
|
488 |
|
|
|
469 | ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0; | |
|
470 | ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F1; | |
|
471 | ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2; | |
|
472 | status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) ); | |
|
473 | status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) ); | |
|
474 | status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) ); | |
|
489 | 475 | } |
|
490 | 476 | if (event_out == RTEMS_EVENT_MODE_SBM2) |
|
491 | 477 | { |
|
492 | 478 | DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM2\n") |
|
493 |
|
|
|
494 |
|
|
|
495 |
|
|
|
479 | ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0; | |
|
480 | ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1; | |
|
481 | ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F2; | |
|
482 | status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) ); | |
|
483 | status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) ); | |
|
484 | status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) ); | |
|
496 | 485 | } |
|
497 | 486 | } |
|
498 | 487 | } |
@@ -511,9 +500,7 rtems_task cwf3_task(rtems_task_argument | |||
|
511 | 500 | rtems_event_set event_out; |
|
512 | 501 | rtems_id queue_id; |
|
513 | 502 | rtems_status_code status; |
|
514 | ||
|
515 | init_header_continuous_wf_table( SID_NORM_CWF_LONG_F3, headerCWF_F3 ); | |
|
516 | init_header_continuous_cwf3_light_table( headerCWF_F3_light ); | |
|
503 | ring_node ring_node_cwf3_light; | |
|
517 | 504 | |
|
518 | 505 | status = get_message_queue_id_send( &queue_id ); |
|
519 | 506 | if (status != RTEMS_SUCCESSFUL) |
@@ -521,6 +508,17 rtems_task cwf3_task(rtems_task_argument | |||
|
521 | 508 | PRINTF1("in CWF3 *** ERR get_message_queue_id_send %d\n", status) |
|
522 | 509 | } |
|
523 | 510 | |
|
511 | ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3; | |
|
512 | ||
|
513 | // init the ring_node_cwf3_light structure | |
|
514 | ring_node_cwf3_light.buffer_address = (int) wf_cont_f3_light; | |
|
515 | ring_node_cwf3_light.coarseTime = 0x00; | |
|
516 | ring_node_cwf3_light.fineTime = 0x00; | |
|
517 | ring_node_cwf3_light.next = NULL; | |
|
518 | ring_node_cwf3_light.previous = NULL; | |
|
519 | ring_node_cwf3_light.sid = SID_NORM_CWF_F3; | |
|
520 | ring_node_cwf3_light.status = 0x00; | |
|
521 | ||
|
524 | 522 | BOOT_PRINTF("in CWF3 ***\n") |
|
525 | 523 | |
|
526 | 524 | while(1){ |
@@ -533,14 +531,14 rtems_task cwf3_task(rtems_task_argument | |||
|
533 | 531 | if ( (parameter_dump_packet.sy_lfr_n_cwf_long_f3 & 0x01) == 0x01) |
|
534 | 532 | { |
|
535 | 533 | PRINTF("send CWF_LONG_F3\n") |
|
536 |
|
|
|
537 | SID_NORM_CWF_LONG_F3, headerCWF_F3, queue_id ); | |
|
534 | ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3; | |
|
535 | status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf_f2, sizeof( ring_node* ) ); | |
|
538 | 536 | } |
|
539 | 537 | else |
|
540 | 538 | { |
|
541 | 539 | PRINTF("send CWF_F3 (light)\n") |
|
542 |
|
|
|
543 | headerCWF_F3_light, queue_id ); | |
|
540 | ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_F3; | |
|
541 | send_waveform_CWF3_light( ring_node_to_send_cwf_f3, &ring_node_cwf3_light, queue_id ); | |
|
544 | 542 | } |
|
545 | 543 | |
|
546 | 544 | } |
@@ -567,9 +565,6 rtems_task cwf2_task(rtems_task_argument | |||
|
567 | 565 | rtems_id queue_id; |
|
568 | 566 | rtems_status_code status; |
|
569 | 567 | |
|
570 | init_header_continuous_wf_table( SID_BURST_CWF_F2, headerCWF_F2_BURST ); | |
|
571 | init_header_continuous_wf_table( SID_SBM2_CWF_F2, headerCWF_F2_SBM2 ); | |
|
572 | ||
|
573 | 568 | status = get_message_queue_id_send( &queue_id ); |
|
574 | 569 | if (status != RTEMS_SUCCESSFUL) |
|
575 | 570 | { |
@@ -584,11 +579,15 rtems_task cwf2_task(rtems_task_argument | |||
|
584 | 579 | RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); |
|
585 | 580 | if (event_out == RTEMS_EVENT_MODE_BURST) |
|
586 | 581 | { |
|
587 | send_waveform_CWF( ring_node_to_send_cwf_f2, SID_BURST_CWF_F2, headerCWF_F2_BURST, queue_id ); | |
|
582 | // send_waveform_CWF( ring_node_to_send_cwf_f2, SID_BURST_CWF_F2, headerCWF_F2_BURST, queue_id ); | |
|
583 | ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2; | |
|
584 | status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf_f2, sizeof( ring_node* ) ); | |
|
588 | 585 | } |
|
589 | 586 | if (event_out == RTEMS_EVENT_MODE_SBM2) |
|
590 | 587 | { |
|
591 | send_waveform_CWF( ring_node_to_send_cwf_f2, SID_SBM2_CWF_F2, headerCWF_F2_SBM2, queue_id ); | |
|
588 | // send_waveform_CWF( ring_node_to_send_cwf_f2, SID_SBM2_CWF_F2, headerCWF_F2_SBM2, queue_id ); | |
|
589 | ring_node_to_send_cwf_f2->sid = SID_SBM2_CWF_F2; | |
|
590 | status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf_f2, sizeof( ring_node* ) ); | |
|
592 | 591 | // launch snapshot extraction if needed |
|
593 | 592 | if (extractSWF == true) |
|
594 | 593 | { |
@@ -624,7 +623,7 rtems_task cwf1_task(rtems_task_argument | |||
|
624 | 623 | rtems_id queue_id; |
|
625 | 624 | rtems_status_code status; |
|
626 | 625 | |
|
627 | init_header_continuous_wf_table( SID_SBM1_CWF_F1, headerCWF_F1 ); | |
|
626 | // init_header_continuous_wf_table( SID_SBM1_CWF_F1, headerCWF_F1 ); | |
|
628 | 627 | |
|
629 | 628 | status = get_message_queue_id_send( &queue_id ); |
|
630 | 629 | if (status != RTEMS_SUCCESSFUL) |
@@ -638,7 +637,8 rtems_task cwf1_task(rtems_task_argument | |||
|
638 | 637 | // wait for an RTEMS_EVENT |
|
639 | 638 | rtems_event_receive( RTEMS_EVENT_MODE_SBM1, |
|
640 | 639 | RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); |
|
641 |
|
|
|
640 | ring_node_to_send_cwf_f1->sid = SID_SBM1_CWF_F1; | |
|
641 | status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf_f1, sizeof( ring_node* ) ); | |
|
642 | 642 | // launch snapshot extraction if needed |
|
643 | 643 | if (extractSWF == true) |
|
644 | 644 | { |
@@ -695,13 +695,13 rtems_task swbd_task(rtems_task_argument | |||
|
695 | 695 | void WFP_init_rings( void ) |
|
696 | 696 | { |
|
697 | 697 | // F0 RING |
|
698 |
init_ |
|
|
698 | init_ring( waveform_ring_f0, NB_RING_NODES_F0, wf_buffer_f0, WFRM_BUFFER ); | |
|
699 | 699 | // F1 RING |
|
700 |
init_ |
|
|
700 | init_ring( waveform_ring_f1, NB_RING_NODES_F1, wf_buffer_f1, WFRM_BUFFER ); | |
|
701 | 701 | // F2 RING |
|
702 |
init_ |
|
|
702 | init_ring( waveform_ring_f2, NB_RING_NODES_F2, wf_buffer_f2, WFRM_BUFFER ); | |
|
703 | 703 | // F3 RING |
|
704 |
init_ |
|
|
704 | init_ring( waveform_ring_f3, NB_RING_NODES_F3, wf_buffer_f3, WFRM_BUFFER ); | |
|
705 | 705 | |
|
706 | 706 | ring_node_wf_snap_extracted.buffer_address = (int) wf_snap_extracted; |
|
707 | 707 | |
@@ -709,25 +709,42 void WFP_init_rings( void ) | |||
|
709 | 709 | DEBUG_PRINTF1("waveform_ring_f1 @%x\n", (unsigned int) waveform_ring_f1) |
|
710 | 710 | DEBUG_PRINTF1("waveform_ring_f2 @%x\n", (unsigned int) waveform_ring_f2) |
|
711 | 711 | DEBUG_PRINTF1("waveform_ring_f3 @%x\n", (unsigned int) waveform_ring_f3) |
|
712 | DEBUG_PRINTF1("wf_buffer_f0 @%x\n", (unsigned int) wf_buffer_f0) | |
|
713 | DEBUG_PRINTF1("wf_buffer_f1 @%x\n", (unsigned int) wf_buffer_f1) | |
|
714 | DEBUG_PRINTF1("wf_buffer_f2 @%x\n", (unsigned int) wf_buffer_f2) | |
|
715 | DEBUG_PRINTF1("wf_buffer_f3 @%x\n", (unsigned int) wf_buffer_f3) | |
|
716 | ||
|
712 | 717 | } |
|
713 | 718 | |
|
714 |
void init_ |
|
|
719 | void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize ) | |
|
715 | 720 | { |
|
716 | 721 | unsigned char i; |
|
717 | 722 | |
|
718 | waveform_ring[0].next = (ring_node*) &waveform_ring[ 1 ]; | |
|
719 | waveform_ring[0].previous = (ring_node*) &waveform_ring[ nbNodes - 1 ]; | |
|
720 | waveform_ring[0].buffer_address = (int) &wfrm[0]; | |
|
723 | //*************** | |
|
724 | // BUFFER ADDRESS | |
|
725 | for(i=0; i<nbNodes; i++) | |
|
726 | { | |
|
727 | ring[i].coarseTime = 0x00; | |
|
728 | ring[i].fineTime = 0x00; | |
|
729 | ring[i].sid = 0x00; | |
|
730 | ring[i].status = 0x00; | |
|
731 | ring[i].buffer_address = (int) &buffer[ i * bufferSize ]; | |
|
732 | } | |
|
721 | 733 | |
|
722 | waveform_ring[nbNodes-1].next = (ring_node*) &waveform_ring[ 0 ]; | |
|
723 | waveform_ring[nbNodes-1].previous = (ring_node*) &waveform_ring[ nbNodes - 2 ]; | |
|
724 | waveform_ring[nbNodes-1].buffer_address = (int) &wfrm[ (nbNodes-1) * WFRM_BUFFER ]; | |
|
734 | //***** | |
|
735 | // NEXT | |
|
736 | ring[nbNodes-1].next = (ring_node*) &ring[ 0 ]; | |
|
737 | for(i=0; i<nbNodes-1; i++) | |
|
738 | { | |
|
739 | ring[i].next = (ring_node*) &ring[ i + 1 ]; | |
|
740 | } | |
|
725 | 741 | |
|
726 | for(i=1; i<nbNodes-1; i++) | |
|
742 | //********* | |
|
743 | // PREVIOUS | |
|
744 | ring[0].previous = (ring_node*) &ring[ nbNodes - 1 ]; | |
|
745 | for(i=1; i<nbNodes; i++) | |
|
727 | 746 | { |
|
728 |
|
|
|
729 | waveform_ring[i].previous = (ring_node*) &waveform_ring[ i - 1 ]; | |
|
730 | waveform_ring[i].buffer_address = (int) &wfrm[ i * WFRM_BUFFER ]; | |
|
747 | ring[i].previous = (ring_node*) &ring[ i - 1 ]; | |
|
731 | 748 | } |
|
732 | 749 | } |
|
733 | 750 | |
@@ -748,288 +765,7 void WFP_reset_current_ring_nodes( void | |||
|
748 | 765 | ring_node_to_send_cwf_f3 = waveform_ring_f3; |
|
749 | 766 | } |
|
750 | 767 | |
|
751 | int init_header_snapshot_wf_table( unsigned int sid, Header_TM_LFR_SCIENCE_SWF_t *headerSWF) | |
|
752 | { | |
|
753 | unsigned char i; | |
|
754 | int return_value; | |
|
755 | ||
|
756 | return_value = LFR_SUCCESSFUL; | |
|
757 | ||
|
758 | for (i=0; i<7; i++) | |
|
759 | { | |
|
760 | headerSWF[ i ].targetLogicalAddress = CCSDS_DESTINATION_ID; | |
|
761 | headerSWF[ i ].protocolIdentifier = CCSDS_PROTOCOLE_ID; | |
|
762 | headerSWF[ i ].reserved = DEFAULT_RESERVED; | |
|
763 | headerSWF[ i ].userApplication = CCSDS_USER_APP; | |
|
764 | headerSWF[ i ].packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8); | |
|
765 | headerSWF[ i ].packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); | |
|
766 | headerSWF[ i ].packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; | |
|
767 | if (i == 6) | |
|
768 | { | |
|
769 | headerSWF[ i ].packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8); | |
|
770 | headerSWF[ i ].packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 ); | |
|
771 | headerSWF[ i ].blkNr[0] = (unsigned char) (BLK_NR_224 >> 8); | |
|
772 | headerSWF[ i ].blkNr[1] = (unsigned char) (BLK_NR_224 ); | |
|
773 | } | |
|
774 | else | |
|
775 | { | |
|
776 | headerSWF[ i ].packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8); | |
|
777 | headerSWF[ i ].packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 ); | |
|
778 | headerSWF[ i ].blkNr[0] = (unsigned char) (BLK_NR_304 >> 8); | |
|
779 | headerSWF[ i ].blkNr[1] = (unsigned char) (BLK_NR_304 ); | |
|
780 | } | |
|
781 | headerSWF[ i ].packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT; | |
|
782 | headerSWF[ i ].pktCnt = DEFAULT_PKTCNT; // PKT_CNT | |
|
783 | headerSWF[ i ].pktNr = i+1; // PKT_NR | |
|
784 | // DATA FIELD HEADER | |
|
785 | headerSWF[ i ].spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2; | |
|
786 | headerSWF[ i ].serviceType = TM_TYPE_LFR_SCIENCE; // service type | |
|
787 | headerSWF[ i ].serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype | |
|
788 | headerSWF[ i ].destinationID = TM_DESTINATION_ID_GROUND; | |
|
789 | // AUXILIARY DATA HEADER | |
|
790 | headerSWF[ i ].time[0] = 0x00; | |
|
791 | headerSWF[ i ].time[0] = 0x00; | |
|
792 | headerSWF[ i ].time[0] = 0x00; | |
|
793 | headerSWF[ i ].time[0] = 0x00; | |
|
794 | headerSWF[ i ].time[0] = 0x00; | |
|
795 | headerSWF[ i ].time[0] = 0x00; | |
|
796 | headerSWF[ i ].sid = sid; | |
|
797 | headerSWF[ i ].hkBIA = DEFAULT_HKBIA; | |
|
798 | } | |
|
799 | ||
|
800 | return return_value; | |
|
801 | } | |
|
802 | ||
|
803 | int init_header_continuous_wf_table( unsigned int sid, Header_TM_LFR_SCIENCE_CWF_t *headerCWF ) | |
|
804 | { | |
|
805 | unsigned int i; | |
|
806 | int return_value; | |
|
807 | ||
|
808 | return_value = LFR_SUCCESSFUL; | |
|
809 | ||
|
810 | for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) | |
|
811 | { | |
|
812 | headerCWF[ i ].targetLogicalAddress = CCSDS_DESTINATION_ID; | |
|
813 | headerCWF[ i ].protocolIdentifier = CCSDS_PROTOCOLE_ID; | |
|
814 | headerCWF[ i ].reserved = DEFAULT_RESERVED; | |
|
815 | headerCWF[ i ].userApplication = CCSDS_USER_APP; | |
|
816 | if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) ) | |
|
817 | { | |
|
818 | headerCWF[ i ].packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8); | |
|
819 | headerCWF[ i ].packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2); | |
|
820 | } | |
|
821 | else | |
|
822 | { | |
|
823 | headerCWF[ i ].packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8); | |
|
824 | headerCWF[ i ].packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); | |
|
825 | } | |
|
826 | headerCWF[ i ].packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; | |
|
827 | headerCWF[ i ].packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8); | |
|
828 | headerCWF[ i ].packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 ); | |
|
829 | headerCWF[ i ].blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8); | |
|
830 | headerCWF[ i ].blkNr[1] = (unsigned char) (BLK_NR_CWF ); | |
|
831 | headerCWF[ i ].packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT; | |
|
832 | // DATA FIELD HEADER | |
|
833 | headerCWF[ i ].spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2; | |
|
834 | headerCWF[ i ].serviceType = TM_TYPE_LFR_SCIENCE; // service type | |
|
835 | headerCWF[ i ].serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype | |
|
836 | headerCWF[ i ].destinationID = TM_DESTINATION_ID_GROUND; | |
|
837 | // AUXILIARY DATA HEADER | |
|
838 | headerCWF[ i ].sid = sid; | |
|
839 | headerCWF[ i ].hkBIA = DEFAULT_HKBIA; | |
|
840 | headerCWF[ i ].time[0] = 0x00; | |
|
841 | headerCWF[ i ].time[0] = 0x00; | |
|
842 | headerCWF[ i ].time[0] = 0x00; | |
|
843 | headerCWF[ i ].time[0] = 0x00; | |
|
844 | headerCWF[ i ].time[0] = 0x00; | |
|
845 | headerCWF[ i ].time[0] = 0x00; | |
|
846 | } | |
|
847 | ||
|
848 | return return_value; | |
|
849 | } | |
|
850 | ||
|
851 | int init_header_continuous_cwf3_light_table( Header_TM_LFR_SCIENCE_CWF_t *headerCWF ) | |
|
852 | { | |
|
853 | unsigned int i; | |
|
854 | int return_value; | |
|
855 | ||
|
856 | return_value = LFR_SUCCESSFUL; | |
|
857 | ||
|
858 | for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) | |
|
859 | { | |
|
860 | headerCWF[ i ].targetLogicalAddress = CCSDS_DESTINATION_ID; | |
|
861 | headerCWF[ i ].protocolIdentifier = CCSDS_PROTOCOLE_ID; | |
|
862 | headerCWF[ i ].reserved = DEFAULT_RESERVED; | |
|
863 | headerCWF[ i ].userApplication = CCSDS_USER_APP; | |
|
864 | ||
|
865 | headerCWF[ i ].packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8); | |
|
866 | headerCWF[ i ].packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); | |
|
867 | ||
|
868 | headerCWF[ i ].packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; | |
|
869 | headerCWF[ i ].packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> 8); | |
|
870 | headerCWF[ i ].packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 ); | |
|
871 | headerCWF[ i ].blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> 8); | |
|
872 | headerCWF[ i ].blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 ); | |
|
873 | ||
|
874 | headerCWF[ i ].packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT; | |
|
875 | // DATA FIELD HEADER | |
|
876 | headerCWF[ i ].spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2; | |
|
877 | headerCWF[ i ].serviceType = TM_TYPE_LFR_SCIENCE; // service type | |
|
878 | headerCWF[ i ].serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype | |
|
879 | headerCWF[ i ].destinationID = TM_DESTINATION_ID_GROUND; | |
|
880 | // AUXILIARY DATA HEADER | |
|
881 | headerCWF[ i ].sid = SID_NORM_CWF_F3; | |
|
882 | headerCWF[ i ].hkBIA = DEFAULT_HKBIA; | |
|
883 | headerCWF[ i ].time[0] = 0x00; | |
|
884 | headerCWF[ i ].time[0] = 0x00; | |
|
885 | headerCWF[ i ].time[0] = 0x00; | |
|
886 | headerCWF[ i ].time[0] = 0x00; | |
|
887 | headerCWF[ i ].time[0] = 0x00; | |
|
888 | headerCWF[ i ].time[0] = 0x00; | |
|
889 | } | |
|
890 | ||
|
891 | return return_value; | |
|
892 | } | |
|
893 | ||
|
894 | int send_waveform_SWF( ring_node *ring_node_to_send, unsigned int sid, | |
|
895 | Header_TM_LFR_SCIENCE_SWF_t *headerSWF, rtems_id queue_id ) | |
|
896 | { | |
|
897 | /** This function sends SWF CCSDS packets (F2, F1 or F0). | |
|
898 | * | |
|
899 | * @param waveform points to the buffer containing the data that will be send. | |
|
900 | * @param sid is the source identifier of the data that will be sent. | |
|
901 | * @param headerSWF points to a table of headers that have been prepared for the data transmission. | |
|
902 | * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures | |
|
903 | * contain information to setup the transmission of the data packets. | |
|
904 | * | |
|
905 | * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks. | |
|
906 | * | |
|
907 | */ | |
|
908 | ||
|
909 | unsigned int i; | |
|
910 | int ret; | |
|
911 | unsigned int coarseTime; | |
|
912 | unsigned int fineTime; | |
|
913 | rtems_status_code status; | |
|
914 | spw_ioctl_pkt_send spw_ioctl_send_SWF; | |
|
915 | int *dataPtr; | |
|
916 | ||
|
917 | spw_ioctl_send_SWF.hlen = TM_HEADER_LEN + 4 + 12; // + 4 is for the protocole extra header, + 12 is for the auxiliary header | |
|
918 | spw_ioctl_send_SWF.options = 0; | |
|
919 | ||
|
920 | ret = LFR_DEFAULT; | |
|
921 | ||
|
922 | coarseTime = ring_node_to_send->coarseTime; | |
|
923 | fineTime = ring_node_to_send->fineTime; | |
|
924 | dataPtr = (int*) ring_node_to_send->buffer_address; | |
|
925 | ||
|
926 | for (i=0; i<7; i++) // send waveform | |
|
927 | { | |
|
928 | spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ]; | |
|
929 | spw_ioctl_send_SWF.hdr = (char*) &headerSWF[ i ]; | |
|
930 | // BUILD THE DATA | |
|
931 | if (i==6) { | |
|
932 | spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK; | |
|
933 | } | |
|
934 | else { | |
|
935 | spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK; | |
|
936 | } | |
|
937 | // SET PACKET SEQUENCE COUNTER | |
|
938 | increment_seq_counter_source_id( headerSWF[ i ].packetSequenceControl, sid ); | |
|
939 | // SET PACKET TIME | |
|
940 | compute_acquisition_time( coarseTime, fineTime, sid, i, headerSWF[ i ].acquisitionTime ); | |
|
941 | // | |
|
942 | headerSWF[ i ].time[0] = headerSWF[ i ].acquisitionTime[0]; | |
|
943 | headerSWF[ i ].time[1] = headerSWF[ i ].acquisitionTime[1]; | |
|
944 | headerSWF[ i ].time[2] = headerSWF[ i ].acquisitionTime[2]; | |
|
945 | headerSWF[ i ].time[3] = headerSWF[ i ].acquisitionTime[3]; | |
|
946 | headerSWF[ i ].time[4] = headerSWF[ i ].acquisitionTime[4]; | |
|
947 | headerSWF[ i ].time[5] = headerSWF[ i ].acquisitionTime[5]; | |
|
948 | // SEND PACKET | |
|
949 | status = rtems_message_queue_send( queue_id, &spw_ioctl_send_SWF, ACTION_MSG_SPW_IOCTL_SEND_SIZE); | |
|
950 | if (status != RTEMS_SUCCESSFUL) { | |
|
951 | printf("%d-%d, ERR %d\n", sid, i, (int) status); | |
|
952 | ret = LFR_DEFAULT; | |
|
953 | } | |
|
954 | rtems_task_wake_after(TIME_BETWEEN_TWO_SWF_PACKETS); // 300 ms between each packet => 7 * 3 = 21 packets => 6.3 seconds | |
|
955 | } | |
|
956 | ||
|
957 | return ret; | |
|
958 | } | |
|
959 | ||
|
960 | int send_waveform_CWF(ring_node *ring_node_to_send, unsigned int sid, | |
|
961 | Header_TM_LFR_SCIENCE_CWF_t *headerCWF, rtems_id queue_id) | |
|
962 | { | |
|
963 | /** This function sends CWF CCSDS packets (F2, F1 or F0). | |
|
964 | * | |
|
965 | * @param waveform points to the buffer containing the data that will be send. | |
|
966 | * @param sid is the source identifier of the data that will be sent. | |
|
967 | * @param headerCWF points to a table of headers that have been prepared for the data transmission. | |
|
968 | * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures | |
|
969 | * contain information to setup the transmission of the data packets. | |
|
970 | * | |
|
971 | * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks. | |
|
972 | * | |
|
973 | */ | |
|
974 | ||
|
975 | unsigned int i; | |
|
976 | int ret; | |
|
977 | unsigned int coarseTime; | |
|
978 | unsigned int fineTime; | |
|
979 | rtems_status_code status; | |
|
980 | spw_ioctl_pkt_send spw_ioctl_send_CWF; | |
|
981 | int *dataPtr; | |
|
982 | ||
|
983 | spw_ioctl_send_CWF.hlen = TM_HEADER_LEN + 4 + 10; // + 4 is for the protocole extra header, + 10 is for the auxiliary header | |
|
984 | spw_ioctl_send_CWF.options = 0; | |
|
985 | ||
|
986 | ret = LFR_DEFAULT; | |
|
987 | ||
|
988 | coarseTime = ring_node_to_send->coarseTime; | |
|
989 | fineTime = ring_node_to_send->fineTime; | |
|
990 | dataPtr = (int*) ring_node_to_send->buffer_address; | |
|
991 | ||
|
992 | for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform | |
|
993 | { | |
|
994 | spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ]; | |
|
995 | spw_ioctl_send_CWF.hdr = (char*) &headerCWF[ i ]; | |
|
996 | // BUILD THE DATA | |
|
997 | spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK; | |
|
998 | // SET PACKET SEQUENCE COUNTER | |
|
999 | increment_seq_counter_source_id( headerCWF[ i ].packetSequenceControl, sid ); | |
|
1000 | // SET PACKET TIME | |
|
1001 | compute_acquisition_time( coarseTime, fineTime, sid, i, headerCWF[ i ].acquisitionTime); | |
|
1002 | // | |
|
1003 | headerCWF[ i ].time[0] = headerCWF[ i ].acquisitionTime[0]; | |
|
1004 | headerCWF[ i ].time[1] = headerCWF[ i ].acquisitionTime[1]; | |
|
1005 | headerCWF[ i ].time[2] = headerCWF[ i ].acquisitionTime[2]; | |
|
1006 | headerCWF[ i ].time[3] = headerCWF[ i ].acquisitionTime[3]; | |
|
1007 | headerCWF[ i ].time[4] = headerCWF[ i ].acquisitionTime[4]; | |
|
1008 | headerCWF[ i ].time[5] = headerCWF[ i ].acquisitionTime[5]; | |
|
1009 | // SEND PACKET | |
|
1010 | if (sid == SID_NORM_CWF_LONG_F3) | |
|
1011 | { | |
|
1012 | status = rtems_message_queue_send( queue_id, &spw_ioctl_send_CWF, sizeof(spw_ioctl_send_CWF)); | |
|
1013 | if (status != RTEMS_SUCCESSFUL) { | |
|
1014 | printf("%d-%d, ERR %d\n", sid, i, (int) status); | |
|
1015 | ret = LFR_DEFAULT; | |
|
1016 | } | |
|
1017 | rtems_task_wake_after(TIME_BETWEEN_TWO_CWF3_PACKETS); | |
|
1018 | } | |
|
1019 | else | |
|
1020 | { | |
|
1021 | status = rtems_message_queue_send( queue_id, &spw_ioctl_send_CWF, sizeof(spw_ioctl_send_CWF)); | |
|
1022 | if (status != RTEMS_SUCCESSFUL) { | |
|
1023 | printf("%d-%d, ERR %d\n", sid, i, (int) status); | |
|
1024 | ret = LFR_DEFAULT; | |
|
1025 | } | |
|
1026 | } | |
|
1027 | } | |
|
1028 | ||
|
1029 | return ret; | |
|
1030 | } | |
|
1031 | ||
|
1032 | int send_waveform_CWF3_light( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_CWF_t *headerCWF, rtems_id queue_id ) | |
|
768 | int send_waveform_CWF3_light( ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id ) | |
|
1033 | 769 | { |
|
1034 | 770 | /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data. |
|
1035 | 771 | * |
@@ -1045,8 +781,6 int send_waveform_CWF3_light( ring_node | |||
|
1045 | 781 | |
|
1046 | 782 | unsigned int i; |
|
1047 | 783 | int ret; |
|
1048 | unsigned int coarseTime; | |
|
1049 | unsigned int fineTime; | |
|
1050 | 784 | rtems_status_code status; |
|
1051 | 785 | spw_ioctl_pkt_send spw_ioctl_send_CWF; |
|
1052 | 786 | char *sample; |
@@ -1057,10 +791,11 int send_waveform_CWF3_light( ring_node | |||
|
1057 | 791 | |
|
1058 | 792 | ret = LFR_DEFAULT; |
|
1059 | 793 | |
|
1060 | coarseTime = ring_node_to_send->coarseTime; | |
|
1061 | fineTime = ring_node_to_send->fineTime; | |
|
1062 | 794 | dataPtr = (int*) ring_node_to_send->buffer_address; |
|
1063 | 795 | |
|
796 | ring_node_cwf3_light->coarseTime = ring_node_to_send->coarseTime; | |
|
797 | ring_node_cwf3_light->fineTime = ring_node_to_send->fineTime; | |
|
798 | ||
|
1064 | 799 | //********************** |
|
1065 | 800 | // BUILD CWF3_light DATA |
|
1066 | 801 | for ( i=0; i< NB_SAMPLES_PER_SNAPSHOT; i++) |
@@ -1074,47 +809,16 int send_waveform_CWF3_light( ring_node | |||
|
1074 | 809 | wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 5 ] = sample[ 5 ]; |
|
1075 | 810 | } |
|
1076 | 811 | |
|
1077 | //********************* | |
|
1078 | // SEND CWF3_light DATA | |
|
1079 | for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform | |
|
1080 | { | |
|
1081 | spw_ioctl_send_CWF.data = (char*) &wf_cont_f3_light[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ]; | |
|
1082 | spw_ioctl_send_CWF.hdr = (char*) &headerCWF[ i ]; | |
|
1083 | // BUILD THE DATA | |
|
1084 | spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK; | |
|
1085 | // SET PACKET SEQUENCE COUNTER | |
|
1086 | increment_seq_counter_source_id( headerCWF[ i ].packetSequenceControl, SID_NORM_CWF_F3 ); | |
|
1087 | // SET PACKET TIME | |
|
1088 | compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, headerCWF[ i ].acquisitionTime ); | |
|
1089 | // | |
|
1090 | headerCWF[ i ].time[0] = headerCWF[ i ].acquisitionTime[0]; | |
|
1091 | headerCWF[ i ].time[1] = headerCWF[ i ].acquisitionTime[1]; | |
|
1092 | headerCWF[ i ].time[2] = headerCWF[ i ].acquisitionTime[2]; | |
|
1093 | headerCWF[ i ].time[3] = headerCWF[ i ].acquisitionTime[3]; | |
|
1094 | headerCWF[ i ].time[4] = headerCWF[ i ].acquisitionTime[4]; | |
|
1095 | headerCWF[ i ].time[5] = headerCWF[ i ].acquisitionTime[5]; | |
|
1096 | // SEND PACKET | |
|
1097 | status = rtems_message_queue_send( queue_id, &spw_ioctl_send_CWF, sizeof(spw_ioctl_send_CWF)); | |
|
1098 | if (status != RTEMS_SUCCESSFUL) { | |
|
1099 | printf("%d-%d, ERR %d\n", SID_NORM_CWF_F3, i, (int) status); | |
|
1100 | ret = LFR_DEFAULT; | |
|
1101 | } | |
|
1102 | rtems_task_wake_after(TIME_BETWEEN_TWO_CWF3_PACKETS); | |
|
812 | // SEND PACKET | |
|
813 | status = rtems_message_queue_send( queue_id, &ring_node_cwf3_light, sizeof( ring_node* ) ); | |
|
814 | if (status != RTEMS_SUCCESSFUL) { | |
|
815 | printf("%d-%d, ERR %d\n", SID_NORM_CWF_F3, i, (int) status); | |
|
816 | ret = LFR_DEFAULT; | |
|
1103 | 817 | } |
|
1104 | 818 | |
|
1105 | 819 | return ret; |
|
1106 | 820 | } |
|
1107 | 821 | |
|
1108 | int send_ring_node_CWF( ring_node *ring_node_to_send ) | |
|
1109 | { | |
|
1110 | int status; | |
|
1111 | ||
|
1112 | status = LFR_SUCCESSFUL; | |
|
1113 | // status = rtems_message_queue_send( queue_id, ring_node_to_send, 4 ); | |
|
1114 | ||
|
1115 | return status; | |
|
1116 | } | |
|
1117 | ||
|
1118 | 822 | void compute_acquisition_time( unsigned int coarseTime, unsigned int fineTime, |
|
1119 | 823 | unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char * acquisitionTime ) |
|
1120 | 824 | { |
@@ -1173,7 +877,7 void compute_acquisition_time( unsigned | |||
|
1173 | 877 | break; |
|
1174 | 878 | |
|
1175 | 879 | default: |
|
1176 | PRINTF1("in compute_acquisition_time *** ERR unexpected sid %d", sid) | |
|
880 | PRINTF1("in compute_acquisition_time *** ERR unexpected sid %d\n", sid) | |
|
1177 | 881 | deltaT = 0.; |
|
1178 | 882 | break; |
|
1179 | 883 | } |
General Comments 0
You need to be logged in to leave comments.
Login now