##// END OF EJS Templates
TM_LFR_SCIENCE_NORMA_ASM_ packets modified, 32 bits values instead of 16 bits...
paul -
r196:48fc5efcfe9b R3
parent child
Show More
@@ -1,2 +1,2
1 1 a586fe639ac179e95bdc150ebdbab0312f31dc30 LFR_basic-parameters
2 a806a190dcd72f71d336545073400d3cdaaa3119 header/lfr_common_headers
2 72c4d5eb0bb95a1546beb2b22c8b88c31322ae31 header/lfr_common_headers
@@ -1,112 +1,112
1 1 TEMPLATE = app
2 2 # CONFIG += console v8 sim
3 3 # CONFIG options = verbose *** boot_messages *** debug_messages *** cpu_usage_report *** stack_report *** vhdl_dev *** debug_tch
4 4 # lpp_dpu_destid
5 CONFIG += console verbose lpp_dpu_destid
5 CONFIG += console verbose lpp_dpu_destid cpu_usage_report
6 6 CONFIG -= qt
7 7
8 8 include(./sparc.pri)
9 9
10 10 # flight software version
11 11 SWVERSION=-1-0
12 12 DEFINES += SW_VERSION_N1=3 # major
13 13 DEFINES += SW_VERSION_N2=0 # minor
14 14 DEFINES += SW_VERSION_N3=0 # patch
15 15 DEFINES += SW_VERSION_N4=0 # internal
16 16
17 17 # <GCOV>
18 18 #QMAKE_CFLAGS_RELEASE += -fprofile-arcs -ftest-coverage
19 19 #LIBS += -lgcov /opt/GCOV/01A/lib/overload.o -lc
20 20 # </GCOV>
21 21
22 22 # <CHANGE BEFORE FLIGHT>
23 23 contains( CONFIG, lpp_dpu_destid ) {
24 24 DEFINES += LPP_DPU_DESTID
25 25 }
26 26 # </CHANGE BEFORE FLIGHT>
27 27
28 28 contains( CONFIG, debug_tch ) {
29 29 DEFINES += DEBUG_TCH
30 30 }
31 31 DEFINES += MSB_FIRST_TCH
32 32
33 33 contains( CONFIG, vhdl_dev ) {
34 34 DEFINES += VHDL_DEV
35 35 }
36 36
37 37 contains( CONFIG, verbose ) {
38 38 DEFINES += PRINT_MESSAGES_ON_CONSOLE
39 39 }
40 40
41 41 contains( CONFIG, debug_messages ) {
42 42 DEFINES += DEBUG_MESSAGES
43 43 }
44 44
45 45 contains( CONFIG, cpu_usage_report ) {
46 46 DEFINES += PRINT_TASK_STATISTICS
47 47 }
48 48
49 49 contains( CONFIG, stack_report ) {
50 50 DEFINES += PRINT_STACK_REPORT
51 51 }
52 52
53 53 contains( CONFIG, boot_messages ) {
54 54 DEFINES += BOOT_MESSAGES
55 55 }
56 56
57 57 #doxygen.target = doxygen
58 58 #doxygen.commands = doxygen ../doc/Doxyfile
59 59 #QMAKE_EXTRA_TARGETS += doxygen
60 60
61 61 TARGET = fsw
62 62
63 63 INCLUDEPATH += \
64 64 $${PWD}/../src \
65 65 $${PWD}/../header \
66 66 $${PWD}/../header/lfr_common_headers \
67 67 $${PWD}/../header/processing \
68 68 $${PWD}/../LFR_basic-parameters
69 69
70 70 SOURCES += \
71 71 ../src/wf_handler.c \
72 72 ../src/tc_handler.c \
73 73 ../src/fsw_misc.c \
74 74 ../src/fsw_init.c \
75 75 ../src/fsw_globals.c \
76 76 ../src/fsw_spacewire.c \
77 77 ../src/tc_load_dump_parameters.c \
78 78 ../src/tm_lfr_tc_exe.c \
79 79 ../src/tc_acceptance.c \
80 80 ../src/processing/fsw_processing.c \
81 81 ../src/processing/avf0_prc0.c \
82 82 ../src/processing/avf1_prc1.c \
83 83 ../src/processing/avf2_prc2.c \
84 84 ../src/lfr_cpu_usage_report.c \
85 85 ../LFR_basic-parameters/basic_parameters.c
86 86
87 87 HEADERS += \
88 88 ../header/wf_handler.h \
89 89 ../header/tc_handler.h \
90 90 ../header/grlib_regs.h \
91 91 ../header/fsw_misc.h \
92 92 ../header/fsw_init.h \
93 93 ../header/fsw_spacewire.h \
94 94 ../header/tc_load_dump_parameters.h \
95 95 ../header/tm_lfr_tc_exe.h \
96 96 ../header/tc_acceptance.h \
97 97 ../header/processing/fsw_processing.h \
98 98 ../header/processing/avf0_prc0.h \
99 99 ../header/processing/avf1_prc1.h \
100 100 ../header/processing/avf2_prc2.h \
101 101 ../header/fsw_params_wf_handler.h \
102 102 ../header/lfr_cpu_usage_report.h \
103 103 ../header/lfr_common_headers/ccsds_types.h \
104 104 ../header/lfr_common_headers/fsw_params.h \
105 105 ../header/lfr_common_headers/fsw_params_nb_bytes.h \
106 106 ../header/lfr_common_headers/fsw_params_processing.h \
107 107 ../header/lfr_common_headers/TC_types.h \
108 108 ../header/lfr_common_headers/tm_byte_positions.h \
109 109 ../LFR_basic-parameters/basic_parameters.h \
110 110 ../LFR_basic-parameters/basic_parameters_params.h \
111 111 ../header/GscMemoryLPP.hpp
112 112
@@ -1,62 +1,69
1 1 #ifndef GSCMEMORY_HPP_
2 2 #define GSCMEMORY_HPP_
3 3
4 4 #ifndef LEON3
5 5 #define LEON3
6 6 #endif
7 7
8 8 static unsigned int getCacheControlRegister(){
9 9 #ifdef LEON3
10 10 unsigned int cacheControlRegister = 0;
11 11 __asm__ __volatile__("lda [%%g0] 2, %0" : "=r"(cacheControlRegister) : );
12 12 return cacheControlRegister;
13 13 #endif
14 14 }
15 15
16 16 static void setCacheControlRegister(unsigned int cacheControlRegister)
17 17 {
18 18 #ifdef LEON3
19 19 __asm__ __volatile__("sta %0, [%%g0] 2" : : "r"(cacheControlRegister));
20 20 #endif
21 21 }
22 22
23 23
24 24 /**
25 25 * Flush the data cache and the instruction cache.
26 26 *
27 27 * @return
28 28 */
29 29 static inline void flushCache() {
30 30 asm("flush");
31 31 }
32 32
33 static void resetCacheControlRegister() {
34 #ifdef LEON3
35 unsigned int cacheControlRegister;
36 cacheControlRegister = 0x00;
37 setCacheControlRegister(cacheControlRegister);
38 #endif
39 }
33 40
34 41 static void enableInstructionCache() {
35 42 #ifdef LEON3
36 43 unsigned int cacheControlRegister;
37 44 cacheControlRegister = getCacheControlRegister();
38 45 cacheControlRegister = (cacheControlRegister | 0x3);
39 46 setCacheControlRegister(cacheControlRegister);
40 47 #endif
41 48 }
42 49
43 50 static void enableDataCache() {
44 51 #ifdef LEON3
45 52 unsigned int cacheControlRegister;
46 53 cacheControlRegister = getCacheControlRegister();
47 54 cacheControlRegister = (cacheControlRegister | 0xc);
48 55 setCacheControlRegister(cacheControlRegister);
49 56 #endif
50 57 }
51 58
52 59 static void enableInstructionBurstFetch() {
53 60 #ifdef LEON3
54 61 unsigned int cacheControlRegister;
55 62 cacheControlRegister = getCacheControlRegister();
56 63 // set the bit IB to 1
57 64 cacheControlRegister = (cacheControlRegister | 0x10000);
58 65 setCacheControlRegister(cacheControlRegister);
59 66 #endif
60 67 }
61 68
62 69 #endif /* GSCMEMORY_HPP_ */
@@ -1,48 +1,50
1 1 #ifndef FSW_SPACEWIRE_H_INCLUDED
2 2 #define FSW_SPACEWIRE_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <grspw.h>
6 6
7 7 #include <fcntl.h> // for O_RDWR
8 8 #include <unistd.h> // for the read call
9 9 #include <sys/ioctl.h> // for the ioctl call
10 10 #include <errno.h>
11 11
12 12 #include "fsw_params.h"
13 13 #include "tc_handler.h"
14 14
15 15 extern spw_stats spacewire_stats;
16 16 extern spw_stats spacewire_stats_backup;
17 17
18 18 // RTEMS TASK
19 19 rtems_task spiq_task( rtems_task_argument argument );
20 20 rtems_task recv_task( rtems_task_argument unused );
21 21 rtems_task send_task( rtems_task_argument argument );
22 22 rtems_task wtdg_task( rtems_task_argument argument );
23 23
24 24 int spacewire_open_link( void );
25 25 int spacewire_start_link( int fd );
26 26 int spacewire_stop_and_start_link( int fd );
27 27 int spacewire_configure_link(int fd );
28 28 int spacewire_reset_link( void );
29 29 void spacewire_set_NP( unsigned char val, unsigned int regAddr ); // No Port force
30 30 void spacewire_set_RE( unsigned char val, unsigned int regAddr ); // RMAP Enable
31 31 void spacewire_compute_stats_offsets( void );
32 32 void spacewire_update_statistics( void );
33 33
34 34 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header );
35 35 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header );
36 36 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header );
37 37 int spw_send_waveform_CWF( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_CWF_t *header );
38 38 int spw_send_waveform_SWF( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_SWF_t *header );
39 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 );
40 void spw_send_asm_f0( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_ASM_t *header );
41 void spw_send_asm_f1( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_ASM_t *header );
42 void spw_send_asm_f2( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_ASM_t *header );
41 43 void spw_send_k_dump( ring_node *ring_node_to_send );
42 44
43 45 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc );
44 46 rtems_timer_service_routine user_routine( rtems_id timer_id, void *user_data );
45 47
46 48 void (*grspw_timecode_callback) ( void *pDev, void *regs, int minor, unsigned int tc );
47 49
48 50 #endif // FSW_SPACEWIRE_H_INCLUDED
@@ -1,14 +1,14
1 1 # LOAD FSW USING LINK 1
2 SpwPlugin0.StarDundeeSelectLinkNumber( 2 )
2 SpwPlugin0.StarDundeeSelectLinkNumber( 1 )
3 3
4 4 dsu3plugin0.openFile("/opt/DEV_PLE/FSW-qt/bin/fsw")
5 5 #dsu3plugin0.openFile("/opt/LFR/LFR-FSW/2.0.2.3/fsw")
6 6 dsu3plugin0.loadFile()
7 7
8 8 dsu3plugin0.run()
9 9
10 10 # START SENDING TIMECODES AT 1 Hz
11 11 SpwPlugin0.StarDundeeStartTimecodes( 1 )
12 12
13 13 # it is possible to change the time code frequency
14 14 #RMAPPlugin0.changeTimecodeFrequency(2)
@@ -1,25 +1,25
1 1 #include <drvmgr/ambapp_bus.h>
2 2
3 3 // GRSPW0 resources
4 4 struct drvmgr_key grlib_grspw_0n1_res[] = {
5 5 {"txBdCnt", KEY_TYPE_INT, {(unsigned int)50}}, // 7 SWF_F0, 7 SWF_F1, 7 SWF_F2, 7 CWF_F3, 7 CWF_F1 ou 7 CWF_F2
6 6 {"rxBdCnt", KEY_TYPE_INT, {(unsigned int)10}},
7 7 {"txDataSize", KEY_TYPE_INT, {(unsigned int)4096}},
8 {"txHdrSize", KEY_TYPE_INT, {(unsigned int)20+12}}, // 12 is for the auxiliary header, when needed
8 {"txHdrSize", KEY_TYPE_INT, {(unsigned int)34}},
9 9 {"rxPktSize", KEY_TYPE_INT, {(unsigned int)248+4}},
10 10 KEY_EMPTY
11 11 };
12 12
13 13 // If RTEMS_DRVMGR_STARTUP is defined we override the "weak defaults" that is defined by the LEON3 BSP.
14 14
15 15 struct drvmgr_bus_res grlib_drv_resources = {
16 16 .next = NULL,
17 17 .resource = {
18 18 {DRIVER_AMBAPP_GAISLER_GRSPW_ID, 0, &grlib_grspw_0n1_res[0]},
19 19 // {DRIVER_AMBAPP_GAISLER_APBUART_ID, 0, &grlib_drv_res_apbuart0[0]},
20 20 // {DRIVER_AMBAPP_GAISLER_APBUART_ID, 1, &grlib_drv_res_apbuart1[0]},
21 21 RES_EMPTY /* Mark end of device resource array */
22 22 }
23 23 };
24 24
25 25
@@ -1,812 +1,814
1 1 /** This is the RTEMS initialization module.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * This module contains two very different information:
7 7 * - specific instructions to configure the compilation of the RTEMS executive
8 8 * - functions related to the fligth softwre initialization, especially the INIT RTEMS task
9 9 *
10 10 */
11 11
12 12 //*************************
13 13 // GPL reminder to be added
14 14 //*************************
15 15
16 16 #include <rtems.h>
17 17
18 18 /* configuration information */
19 19
20 20 #define CONFIGURE_INIT
21 21
22 22 #include <bsp.h> /* for device driver prototypes */
23 23
24 24 /* configuration information */
25 25
26 26 #define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
27 27 #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
28 28
29 29 #define CONFIGURE_MAXIMUM_TASKS 20
30 30 #define CONFIGURE_RTEMS_INIT_TASKS_TABLE
31 31 #define CONFIGURE_EXTRA_TASK_STACKS (3 * RTEMS_MINIMUM_STACK_SIZE)
32 32 #define CONFIGURE_LIBIO_MAXIMUM_FILE_DESCRIPTORS 32
33 33 #define CONFIGURE_INIT_TASK_PRIORITY 1 // instead of 100
34 34 #define CONFIGURE_INIT_TASK_MODE (RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT)
35 35 #define CONFIGURE_INIT_TASK_ATTRIBUTES (RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT)
36 36 #define CONFIGURE_MAXIMUM_DRIVERS 16
37 37 #define CONFIGURE_MAXIMUM_PERIODS 5
38 38 #define CONFIGURE_MAXIMUM_TIMERS 5 // STAT (1s), send SWF (0.3s), send CWF3 (1s)
39 39 #define CONFIGURE_MAXIMUM_MESSAGE_QUEUES 5
40 40 #ifdef PRINT_STACK_REPORT
41 41 #define CONFIGURE_STACK_CHECKER_ENABLED
42 42 #endif
43 43
44 44 #include <rtems/confdefs.h>
45 45
46 46 /* If --drvmgr was enabled during the configuration of the RTEMS kernel */
47 47 #ifdef RTEMS_DRVMGR_STARTUP
48 48 #ifdef LEON3
49 49 /* Add Timer and UART Driver */
50 50 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
51 51 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
52 52 #endif
53 53 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
54 54 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
55 55 #endif
56 56 #endif
57 57 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
58 58 #include <drvmgr/drvmgr_confdefs.h>
59 59 #endif
60 60
61 61 #include "fsw_init.h"
62 62 #include "fsw_config.c"
63 63
64 64 void initCache()
65 65 {
66 // unsigned int cacheControlRegister;
66 unsigned int cacheControlRegister;
67 67
68 // cacheControlRegister = getCacheControlRegister();
69 // printf("(0) cacheControlRegister = %x\n", cacheControlRegister);
68 cacheControlRegister = getCacheControlRegister();
69 printf("(0) cacheControlRegister = %x\n", cacheControlRegister);
70
71 resetCacheControlRegister();
70 72
71 73 enableInstructionCache();
72 74 enableDataCache();
73 75 enableInstructionBurstFetch();
74 76
75 // cacheControlRegister = getCacheControlRegister();
76 // printf("(1) cacheControlRegister = %x\n", cacheControlRegister);
77 cacheControlRegister = getCacheControlRegister();
78 printf("(1) cacheControlRegister = %x\n", cacheControlRegister);
77 79 }
78 80
79 81 rtems_task Init( rtems_task_argument ignored )
80 82 {
81 /** This is the RTEMS INIT taks, it the first task launched by the system.
83 /** This is the RTEMS INIT taks, it is the first task launched by the system.
82 84 *
83 85 * @param unused is the starting argument of the RTEMS task
84 86 *
85 87 * The INIT task create and run all other RTEMS tasks.
86 88 *
87 89 */
88 90
89 91 //***********
90 92 // INIT CACHE
91 93
92 94 unsigned char *vhdlVersion;
93 95
94 96 reset_lfr();
95 97
96 98 reset_local_time();
97 99
98 100 rtems_cpu_usage_reset();
99 101
100 102 rtems_status_code status;
101 103 rtems_status_code status_spw;
102 104 rtems_isr_entry old_isr_handler;
103 105
104 106 // UART settings
105 107 send_console_outputs_on_apbuart_port();
106 108 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
107 109 enable_apbuart_transmitter();
108 110
109 111 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
110 112
111 113
112 114 PRINTF("\n\n\n\n\n")
113 115
114 116 initCache();
115 117
116 118 PRINTF("*************************\n")
117 119 PRINTF("** LFR Flight Software **\n")
118 120 PRINTF1("** %d.", SW_VERSION_N1)
119 121 PRINTF1("%d." , SW_VERSION_N2)
120 122 PRINTF1("%d." , SW_VERSION_N3)
121 123 PRINTF1("%d **\n", SW_VERSION_N4)
122 124
123 125 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
124 126 PRINTF("** VHDL **\n")
125 127 PRINTF1("** %d.", vhdlVersion[1])
126 128 PRINTF1("%d." , vhdlVersion[2])
127 129 PRINTF1("%d **\n", vhdlVersion[3])
128 130 PRINTF("*************************\n")
129 131 PRINTF("\n\n")
130 132
131 133 init_parameter_dump();
132 134 init_kcoefficients_dump();
133 135 init_local_mode_parameters();
134 136 init_housekeeping_parameters();
135 137 init_k_coefficients_f0();
136 138 init_k_coefficients_f1();
137 139 init_k_coefficients_f2();
138 140
139 141 // waveform picker initialization
140 142 WFP_init_rings(); // initialize the waveform rings
141 143 WFP_reset_current_ring_nodes();
142 144 reset_waveform_picker_regs();
143 145
144 146 // spectral matrices initialization
145 147 SM_init_rings(); // initialize spectral matrices rings
146 148 SM_reset_current_ring_nodes();
147 149 reset_spectral_matrix_regs();
148 150
149 151 // configure calibration
150 152 configureCalibration( false ); // true means interleaved mode, false is for normal mode
151 153
152 154 updateLFRCurrentMode();
153 155
154 156 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
155 157
156 158 create_names(); // create all names
157 159
158 160 status = create_message_queues(); // create message queues
159 161 if (status != RTEMS_SUCCESSFUL)
160 162 {
161 163 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
162 164 }
163 165
164 166 status = create_all_tasks(); // create all tasks
165 167 if (status != RTEMS_SUCCESSFUL)
166 168 {
167 169 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
168 170 }
169 171
170 172 // **************************
171 173 // <SPACEWIRE INITIALIZATION>
172 174 grspw_timecode_callback = &timecode_irq_handler;
173 175
174 176 status_spw = spacewire_open_link(); // (1) open the link
175 177 if ( status_spw != RTEMS_SUCCESSFUL )
176 178 {
177 179 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
178 180 }
179 181
180 182 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
181 183 {
182 184 status_spw = spacewire_configure_link( fdSPW );
183 185 if ( status_spw != RTEMS_SUCCESSFUL )
184 186 {
185 187 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
186 188 }
187 189 }
188 190
189 191 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
190 192 {
191 193 status_spw = spacewire_start_link( fdSPW );
192 194 if ( status_spw != RTEMS_SUCCESSFUL )
193 195 {
194 196 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
195 197 }
196 198 }
197 199 // </SPACEWIRE INITIALIZATION>
198 200 // ***************************
199 201
200 202 status = start_all_tasks(); // start all tasks
201 203 if (status != RTEMS_SUCCESSFUL)
202 204 {
203 205 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
204 206 }
205 207
206 208 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
207 209 status = start_recv_send_tasks();
208 210 if ( status != RTEMS_SUCCESSFUL )
209 211 {
210 212 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
211 213 }
212 214
213 215 // suspend science tasks, they will be restarted later depending on the mode
214 216 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
215 217 if (status != RTEMS_SUCCESSFUL)
216 218 {
217 219 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
218 220 }
219 221
220 222 //******************************
221 223 // <SPECTRAL MATRICES SIMULATOR>
222 224 LEON_Mask_interrupt( IRQ_SM_SIMULATOR );
223 225 configure_timer((gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR, CLKDIV_SM_SIMULATOR,
224 226 IRQ_SPARC_SM_SIMULATOR, spectral_matrices_isr_simu );
225 227 // </SPECTRAL MATRICES SIMULATOR>
226 228 //*******************************
227 229
228 230 // configure IRQ handling for the waveform picker unit
229 231 status = rtems_interrupt_catch( waveforms_isr,
230 232 IRQ_SPARC_WAVEFORM_PICKER,
231 233 &old_isr_handler) ;
232 234 // configure IRQ handling for the spectral matrices unit
233 235 status = rtems_interrupt_catch( spectral_matrices_isr,
234 236 IRQ_SPARC_SPECTRAL_MATRIX,
235 237 &old_isr_handler) ;
236 238
237 239 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
238 240 if ( status_spw != RTEMS_SUCCESSFUL )
239 241 {
240 242 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
241 243 if ( status != RTEMS_SUCCESSFUL ) {
242 244 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
243 245 }
244 246 }
245 247
246 248 BOOT_PRINTF("delete INIT\n")
247 249
248 250 status = rtems_task_delete(RTEMS_SELF);
249 251
250 252 }
251 253
252 254 void init_local_mode_parameters( void )
253 255 {
254 256 /** This function initialize the param_local global variable with default values.
255 257 *
256 258 */
257 259
258 260 unsigned int i;
259 261
260 262 // LOCAL PARAMETERS
261 263
262 264 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
263 265 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
264 266 BOOT_PRINTF1("nb_interrupt_f0_MAX = %d\n", param_local.local_nb_interrupt_f0_MAX)
265 267
266 268 // init sequence counters
267 269
268 270 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
269 271 {
270 272 sequenceCounters_TC_EXE[i] = 0x00;
271 273 }
272 274 sequenceCounters_SCIENCE_NORMAL_BURST = 0x00;
273 275 sequenceCounters_SCIENCE_SBM1_SBM2 = 0x00;
274 276 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
275 277 sequenceCounterParameterDump = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
276 278 }
277 279
278 280 void reset_local_time( void )
279 281 {
280 282 time_management_regs->ctrl = time_management_regs->ctrl | 0x02; // [0010] software reset, coarse time = 0x80000000
281 283 }
282 284
283 285 void create_names( void ) // create all names for tasks and queues
284 286 {
285 287 /** This function creates all RTEMS names used in the software for tasks and queues.
286 288 *
287 289 * @return RTEMS directive status codes:
288 290 * - RTEMS_SUCCESSFUL - successful completion
289 291 *
290 292 */
291 293
292 294 // task names
293 295 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
294 296 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
295 297 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
296 298 Task_name[TASKID_STAT] = rtems_build_name( 'S', 'T', 'A', 'T' );
297 299 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
298 300 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
299 301 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
300 302 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
301 303 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
302 304 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
303 305 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
304 306 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
305 307 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
306 308 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
307 309 Task_name[TASKID_WTDG] = rtems_build_name( 'W', 'T', 'D', 'G' );
308 310 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
309 311 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
310 312 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
311 313 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
312 314
313 315 // rate monotonic period names
314 316 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
315 317
316 318 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
317 319 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
318 320 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
319 321 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
320 322 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
321 323 }
322 324
323 325 int create_all_tasks( void ) // create all tasks which run in the software
324 326 {
325 327 /** This function creates all RTEMS tasks used in the software.
326 328 *
327 329 * @return RTEMS directive status codes:
328 330 * - RTEMS_SUCCESSFUL - task created successfully
329 331 * - RTEMS_INVALID_ADDRESS - id is NULL
330 332 * - RTEMS_INVALID_NAME - invalid task name
331 333 * - RTEMS_INVALID_PRIORITY - invalid task priority
332 334 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
333 335 * - RTEMS_TOO_MANY - too many tasks created
334 336 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
335 337 * - RTEMS_TOO_MANY - too many global objects
336 338 *
337 339 */
338 340
339 341 rtems_status_code status;
340 342
341 343 //**********
342 344 // SPACEWIRE
343 345 // RECV
344 346 status = rtems_task_create(
345 347 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
346 348 RTEMS_DEFAULT_MODES,
347 349 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
348 350 );
349 351 if (status == RTEMS_SUCCESSFUL) // SEND
350 352 {
351 353 status = rtems_task_create(
352 354 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * 2,
353 355 RTEMS_DEFAULT_MODES,
354 356 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
355 357 );
356 358 }
357 359 if (status == RTEMS_SUCCESSFUL) // WTDG
358 360 {
359 361 status = rtems_task_create(
360 362 Task_name[TASKID_WTDG], TASK_PRIORITY_WTDG, RTEMS_MINIMUM_STACK_SIZE,
361 363 RTEMS_DEFAULT_MODES,
362 364 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_WTDG]
363 365 );
364 366 }
365 367 if (status == RTEMS_SUCCESSFUL) // ACTN
366 368 {
367 369 status = rtems_task_create(
368 370 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
369 371 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
370 372 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
371 373 );
372 374 }
373 375 if (status == RTEMS_SUCCESSFUL) // SPIQ
374 376 {
375 377 status = rtems_task_create(
376 378 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
377 379 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
378 380 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
379 381 );
380 382 }
381 383
382 384 //******************
383 385 // SPECTRAL MATRICES
384 386 if (status == RTEMS_SUCCESSFUL) // AVF0
385 387 {
386 388 status = rtems_task_create(
387 389 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
388 390 RTEMS_DEFAULT_MODES,
389 391 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
390 392 );
391 393 }
392 394 if (status == RTEMS_SUCCESSFUL) // PRC0
393 395 {
394 396 status = rtems_task_create(
395 397 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * 2,
396 398 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
397 399 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
398 400 );
399 401 }
400 402 if (status == RTEMS_SUCCESSFUL) // AVF1
401 403 {
402 404 status = rtems_task_create(
403 405 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
404 406 RTEMS_DEFAULT_MODES,
405 407 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
406 408 );
407 409 }
408 410 if (status == RTEMS_SUCCESSFUL) // PRC1
409 411 {
410 412 status = rtems_task_create(
411 413 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * 2,
412 414 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
413 415 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
414 416 );
415 417 }
416 418 if (status == RTEMS_SUCCESSFUL) // AVF2
417 419 {
418 420 status = rtems_task_create(
419 421 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
420 422 RTEMS_DEFAULT_MODES,
421 423 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
422 424 );
423 425 }
424 426 if (status == RTEMS_SUCCESSFUL) // PRC2
425 427 {
426 428 status = rtems_task_create(
427 429 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * 2,
428 430 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
429 431 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
430 432 );
431 433 }
432 434
433 435 //****************
434 436 // WAVEFORM PICKER
435 437 if (status == RTEMS_SUCCESSFUL) // WFRM
436 438 {
437 439 status = rtems_task_create(
438 440 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
439 441 RTEMS_DEFAULT_MODES,
440 442 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
441 443 );
442 444 }
443 445 if (status == RTEMS_SUCCESSFUL) // CWF3
444 446 {
445 447 status = rtems_task_create(
446 448 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
447 449 RTEMS_DEFAULT_MODES,
448 450 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
449 451 );
450 452 }
451 453 if (status == RTEMS_SUCCESSFUL) // CWF2
452 454 {
453 455 status = rtems_task_create(
454 456 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
455 457 RTEMS_DEFAULT_MODES,
456 458 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
457 459 );
458 460 }
459 461 if (status == RTEMS_SUCCESSFUL) // CWF1
460 462 {
461 463 status = rtems_task_create(
462 464 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
463 465 RTEMS_DEFAULT_MODES,
464 466 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
465 467 );
466 468 }
467 469 if (status == RTEMS_SUCCESSFUL) // SWBD
468 470 {
469 471 status = rtems_task_create(
470 472 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
471 473 RTEMS_DEFAULT_MODES,
472 474 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
473 475 );
474 476 }
475 477
476 478 //*****
477 479 // MISC
478 480 if (status == RTEMS_SUCCESSFUL) // STAT
479 481 {
480 482 status = rtems_task_create(
481 483 Task_name[TASKID_STAT], TASK_PRIORITY_STAT, RTEMS_MINIMUM_STACK_SIZE,
482 484 RTEMS_DEFAULT_MODES,
483 485 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_STAT]
484 486 );
485 487 }
486 488 if (status == RTEMS_SUCCESSFUL) // DUMB
487 489 {
488 490 status = rtems_task_create(
489 491 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
490 492 RTEMS_DEFAULT_MODES,
491 493 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
492 494 );
493 495 }
494 496 if (status == RTEMS_SUCCESSFUL) // HOUS
495 497 {
496 498 status = rtems_task_create(
497 499 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
498 500 RTEMS_DEFAULT_MODES,
499 501 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
500 502 );
501 503 }
502 504
503 505 return status;
504 506 }
505 507
506 508 int start_recv_send_tasks( void )
507 509 {
508 510 rtems_status_code status;
509 511
510 512 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
511 513 if (status!=RTEMS_SUCCESSFUL) {
512 514 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
513 515 }
514 516
515 517 if (status == RTEMS_SUCCESSFUL) // SEND
516 518 {
517 519 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
518 520 if (status!=RTEMS_SUCCESSFUL) {
519 521 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
520 522 }
521 523 }
522 524
523 525 return status;
524 526 }
525 527
526 528 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
527 529 {
528 530 /** This function starts all RTEMS tasks used in the software.
529 531 *
530 532 * @return RTEMS directive status codes:
531 533 * - RTEMS_SUCCESSFUL - ask started successfully
532 534 * - RTEMS_INVALID_ADDRESS - invalid task entry point
533 535 * - RTEMS_INVALID_ID - invalid task id
534 536 * - RTEMS_INCORRECT_STATE - task not in the dormant state
535 537 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
536 538 *
537 539 */
538 540 // starts all the tasks fot eh flight software
539 541
540 542 rtems_status_code status;
541 543
542 544 //**********
543 545 // SPACEWIRE
544 546 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
545 547 if (status!=RTEMS_SUCCESSFUL) {
546 548 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
547 549 }
548 550
549 551 if (status == RTEMS_SUCCESSFUL) // WTDG
550 552 {
551 553 status = rtems_task_start( Task_id[TASKID_WTDG], wtdg_task, 1 );
552 554 if (status!=RTEMS_SUCCESSFUL) {
553 555 BOOT_PRINTF("in INIT *** Error starting TASK_WTDG\n")
554 556 }
555 557 }
556 558
557 559 if (status == RTEMS_SUCCESSFUL) // ACTN
558 560 {
559 561 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
560 562 if (status!=RTEMS_SUCCESSFUL) {
561 563 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
562 564 }
563 565 }
564 566
565 567 //******************
566 568 // SPECTRAL MATRICES
567 569 if (status == RTEMS_SUCCESSFUL) // AVF0
568 570 {
569 571 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
570 572 if (status!=RTEMS_SUCCESSFUL) {
571 573 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
572 574 }
573 575 }
574 576 if (status == RTEMS_SUCCESSFUL) // PRC0
575 577 {
576 578 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
577 579 if (status!=RTEMS_SUCCESSFUL) {
578 580 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
579 581 }
580 582 }
581 583 if (status == RTEMS_SUCCESSFUL) // AVF1
582 584 {
583 585 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
584 586 if (status!=RTEMS_SUCCESSFUL) {
585 587 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
586 588 }
587 589 }
588 590 if (status == RTEMS_SUCCESSFUL) // PRC1
589 591 {
590 592 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
591 593 if (status!=RTEMS_SUCCESSFUL) {
592 594 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
593 595 }
594 596 }
595 597 if (status == RTEMS_SUCCESSFUL) // AVF2
596 598 {
597 599 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
598 600 if (status!=RTEMS_SUCCESSFUL) {
599 601 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
600 602 }
601 603 }
602 604 if (status == RTEMS_SUCCESSFUL) // PRC2
603 605 {
604 606 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
605 607 if (status!=RTEMS_SUCCESSFUL) {
606 608 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
607 609 }
608 610 }
609 611
610 612 //****************
611 613 // WAVEFORM PICKER
612 614 if (status == RTEMS_SUCCESSFUL) // WFRM
613 615 {
614 616 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
615 617 if (status!=RTEMS_SUCCESSFUL) {
616 618 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
617 619 }
618 620 }
619 621 if (status == RTEMS_SUCCESSFUL) // CWF3
620 622 {
621 623 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
622 624 if (status!=RTEMS_SUCCESSFUL) {
623 625 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
624 626 }
625 627 }
626 628 if (status == RTEMS_SUCCESSFUL) // CWF2
627 629 {
628 630 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
629 631 if (status!=RTEMS_SUCCESSFUL) {
630 632 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
631 633 }
632 634 }
633 635 if (status == RTEMS_SUCCESSFUL) // CWF1
634 636 {
635 637 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
636 638 if (status!=RTEMS_SUCCESSFUL) {
637 639 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
638 640 }
639 641 }
640 642 if (status == RTEMS_SUCCESSFUL) // SWBD
641 643 {
642 644 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
643 645 if (status!=RTEMS_SUCCESSFUL) {
644 646 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
645 647 }
646 648 }
647 649
648 650 //*****
649 651 // MISC
650 652 if (status == RTEMS_SUCCESSFUL) // HOUS
651 653 {
652 654 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
653 655 if (status!=RTEMS_SUCCESSFUL) {
654 656 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
655 657 }
656 658 }
657 659 if (status == RTEMS_SUCCESSFUL) // DUMB
658 660 {
659 661 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
660 662 if (status!=RTEMS_SUCCESSFUL) {
661 663 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
662 664 }
663 665 }
664 666 if (status == RTEMS_SUCCESSFUL) // STAT
665 667 {
666 668 status = rtems_task_start( Task_id[TASKID_STAT], stat_task, 1 );
667 669 if (status!=RTEMS_SUCCESSFUL) {
668 670 BOOT_PRINTF("in INIT *** Error starting TASK_STAT\n")
669 671 }
670 672 }
671 673
672 674 return status;
673 675 }
674 676
675 677 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
676 678 {
677 679 rtems_status_code status_recv;
678 680 rtems_status_code status_send;
679 681 rtems_status_code status_q_p0;
680 682 rtems_status_code status_q_p1;
681 683 rtems_status_code status_q_p2;
682 684 rtems_status_code ret;
683 685 rtems_id queue_id;
684 686
685 687 //****************************************
686 688 // create the queue for handling valid TCs
687 689 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
688 690 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
689 691 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
690 692 if ( status_recv != RTEMS_SUCCESSFUL ) {
691 693 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
692 694 }
693 695
694 696 //************************************************
695 697 // create the queue for handling TM packet sending
696 698 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
697 699 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
698 700 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
699 701 if ( status_send != RTEMS_SUCCESSFUL ) {
700 702 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
701 703 }
702 704
703 705 //*****************************************************************************
704 706 // create the queue for handling averaged spectral matrices for processing @ f0
705 707 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
706 708 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
707 709 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
708 710 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
709 711 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
710 712 }
711 713
712 714 //*****************************************************************************
713 715 // create the queue for handling averaged spectral matrices for processing @ f1
714 716 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
715 717 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
716 718 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
717 719 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
718 720 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
719 721 }
720 722
721 723 //*****************************************************************************
722 724 // create the queue for handling averaged spectral matrices for processing @ f2
723 725 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
724 726 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
725 727 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
726 728 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
727 729 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
728 730 }
729 731
730 732 if ( status_recv != RTEMS_SUCCESSFUL )
731 733 {
732 734 ret = status_recv;
733 735 }
734 736 else if( status_send != RTEMS_SUCCESSFUL )
735 737 {
736 738 ret = status_send;
737 739 }
738 740 else if( status_q_p0 != RTEMS_SUCCESSFUL )
739 741 {
740 742 ret = status_q_p0;
741 743 }
742 744 else if( status_q_p1 != RTEMS_SUCCESSFUL )
743 745 {
744 746 ret = status_q_p1;
745 747 }
746 748 else
747 749 {
748 750 ret = status_q_p2;
749 751 }
750 752
751 753 return ret;
752 754 }
753 755
754 756 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
755 757 {
756 758 rtems_status_code status;
757 759 rtems_name queue_name;
758 760
759 761 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
760 762
761 763 status = rtems_message_queue_ident( queue_name, 0, queue_id );
762 764
763 765 return status;
764 766 }
765 767
766 768 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
767 769 {
768 770 rtems_status_code status;
769 771 rtems_name queue_name;
770 772
771 773 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
772 774
773 775 status = rtems_message_queue_ident( queue_name, 0, queue_id );
774 776
775 777 return status;
776 778 }
777 779
778 780 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
779 781 {
780 782 rtems_status_code status;
781 783 rtems_name queue_name;
782 784
783 785 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
784 786
785 787 status = rtems_message_queue_ident( queue_name, 0, queue_id );
786 788
787 789 return status;
788 790 }
789 791
790 792 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
791 793 {
792 794 rtems_status_code status;
793 795 rtems_name queue_name;
794 796
795 797 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
796 798
797 799 status = rtems_message_queue_ident( queue_name, 0, queue_id );
798 800
799 801 return status;
800 802 }
801 803
802 804 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
803 805 {
804 806 rtems_status_code status;
805 807 rtems_name queue_name;
806 808
807 809 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
808 810
809 811 status = rtems_message_queue_ident( queue_name, 0, queue_id );
810 812
811 813 return status;
812 814 }
@@ -1,1156 +1,1295
1 1 /** Functions related to the SpaceWire interface.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle SpaceWire transmissions:
7 7 * - configuration of the SpaceWire link
8 8 * - SpaceWire related interruption requests processing
9 9 * - transmission of TeleMetry packets by a dedicated RTEMS task
10 10 * - reception of TeleCommands by a dedicated RTEMS task
11 11 *
12 12 */
13 13
14 14 #include "fsw_spacewire.h"
15 15
16 16 rtems_name semq_name;
17 17 rtems_id semq_id;
18 18
19 19 //*****************
20 20 // waveform headers
21 21 Header_TM_LFR_SCIENCE_CWF_t headerCWF;
22 22 Header_TM_LFR_SCIENCE_SWF_t headerSWF;
23 23 Header_TM_LFR_SCIENCE_ASM_t headerASM;
24 24
25 25 //***********
26 26 // RTEMS TASK
27 27 rtems_task spiq_task(rtems_task_argument unused)
28 28 {
29 29 /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver.
30 30 *
31 31 * @param unused is the starting argument of the RTEMS task
32 32 *
33 33 */
34 34
35 35 rtems_event_set event_out;
36 36 rtems_status_code status;
37 37 int linkStatus;
38 38
39 39 BOOT_PRINTF("in SPIQ *** \n")
40 40
41 41 while(true){
42 42 rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT
43 43 PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n")
44 44
45 45 // [0] SUSPEND RECV AND SEND TASKS
46 46 status = rtems_task_suspend( Task_id[ TASKID_RECV ] );
47 47 if ( status != RTEMS_SUCCESSFUL ) {
48 48 PRINTF("in SPIQ *** ERR suspending RECV Task\n")
49 49 }
50 50 status = rtems_task_suspend( Task_id[ TASKID_SEND ] );
51 51 if ( status != RTEMS_SUCCESSFUL ) {
52 52 PRINTF("in SPIQ *** ERR suspending SEND Task\n")
53 53 }
54 54
55 55 // [1] CHECK THE LINK
56 56 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1)
57 57 if ( linkStatus != 5) {
58 58 PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus)
59 59 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
60 60 }
61 61
62 62 // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT
63 63 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2)
64 64 if ( linkStatus != 5 ) // [2.a] not in run state, reset the link
65 65 {
66 66 spacewire_compute_stats_offsets();
67 67 status = spacewire_reset_link( );
68 68 }
69 69 else // [2.b] in run state, start the link
70 70 {
71 71 status = spacewire_stop_and_start_link( fdSPW ); // start the link
72 72 if ( status != RTEMS_SUCCESSFUL)
73 73 {
74 74 PRINTF1("in SPIQ *** ERR spacewire_start_link %d\n", status)
75 75 }
76 76 }
77 77
78 78 // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS
79 79 if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully
80 80 {
81 81 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
82 82 if ( status != RTEMS_SUCCESSFUL ) {
83 83 PRINTF("in SPIQ *** ERR resuming SEND Task\n")
84 84 }
85 85 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
86 86 if ( status != RTEMS_SUCCESSFUL ) {
87 87 PRINTF("in SPIQ *** ERR resuming RECV Task\n")
88 88 }
89 89 }
90 90 else // [3.b] the link is not in run state, go in STANDBY mode
91 91 {
92 92 status = stop_current_mode();
93 93 if ( status != RTEMS_SUCCESSFUL ) {
94 94 PRINTF1("in SPIQ *** ERR stop_current_mode *** code %d\n", status)
95 95 }
96 96 status = enter_mode( LFR_MODE_STANDBY, 0 );
97 97 if ( status != RTEMS_SUCCESSFUL ) {
98 98 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
99 99 }
100 100 // wake the WTDG task up to wait for the link recovery
101 101 status = rtems_event_send ( Task_id[TASKID_WTDG], RTEMS_EVENT_0 );
102 102 status = rtems_task_suspend( RTEMS_SELF );
103 103 }
104 104 }
105 105 }
106 106
107 107 rtems_task recv_task( rtems_task_argument unused )
108 108 {
109 109 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
110 110 *
111 111 * @param unused is the starting argument of the RTEMS task
112 112 *
113 113 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
114 114 * 1. It reads the incoming data.
115 115 * 2. Launches the acceptance procedure.
116 116 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
117 117 *
118 118 */
119 119
120 120 int len;
121 121 ccsdsTelecommandPacket_t currentTC;
122 122 unsigned char computed_CRC[ 2 ];
123 123 unsigned char currentTC_LEN_RCV[ 2 ];
124 124 unsigned char destinationID;
125 125 unsigned int estimatedPacketLength;
126 126 unsigned int parserCode;
127 127 rtems_status_code status;
128 128 rtems_id queue_recv_id;
129 129 rtems_id queue_send_id;
130 130
131 131 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
132 132
133 133 status = get_message_queue_id_recv( &queue_recv_id );
134 134 if (status != RTEMS_SUCCESSFUL)
135 135 {
136 136 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
137 137 }
138 138
139 139 status = get_message_queue_id_send( &queue_send_id );
140 140 if (status != RTEMS_SUCCESSFUL)
141 141 {
142 142 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
143 143 }
144 144
145 145 BOOT_PRINTF("in RECV *** \n")
146 146
147 147 while(1)
148 148 {
149 149 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
150 150 if (len == -1){ // error during the read call
151 151 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
152 152 }
153 153 else {
154 154 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
155 155 PRINTF("in RECV *** packet lenght too short\n")
156 156 }
157 157 else {
158 158 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // => -3 is for Prot ID, Reserved and User App bytes
159 159 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> 8);
160 160 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
161 161 // CHECK THE TC
162 162 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
163 163 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
164 164 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
165 165 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
166 166 || (parserCode == WRONG_SRC_ID) )
167 167 { // send TM_LFR_TC_EXE_CORRUPTED
168 168 PRINTF1("TC corrupted received, with code: %d\n", parserCode)
169 169 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
170 170 &&
171 171 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
172 172 )
173 173 {
174 174 if ( parserCode == WRONG_SRC_ID )
175 175 {
176 176 destinationID = SID_TC_GROUND;
177 177 }
178 178 else
179 179 {
180 180 destinationID = currentTC.sourceID;
181 181 }
182 182 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
183 183 computed_CRC, currentTC_LEN_RCV,
184 184 destinationID );
185 185 }
186 186 }
187 187 else
188 188 { // send valid TC to the action launcher
189 189 status = rtems_message_queue_send( queue_recv_id, &currentTC,
190 190 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + 3);
191 191 }
192 192 }
193 193 }
194 194 }
195 195 }
196 196
197 197 rtems_task send_task( rtems_task_argument argument)
198 198 {
199 199 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
200 200 *
201 201 * @param unused is the starting argument of the RTEMS task
202 202 *
203 203 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
204 204 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
205 205 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
206 206 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
207 207 * data it contains.
208 208 *
209 209 */
210 210
211 211 rtems_status_code status; // RTEMS status code
212 212 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
213 213 ring_node *incomingRingNodePtr;
214 214 int ring_node_address;
215 215 char *charPtr;
216 216 spw_ioctl_pkt_send *spw_ioctl_send;
217 217 size_t size; // size of the incoming TC packet
218 218 u_int32_t count;
219 219 rtems_id queue_id;
220 220 unsigned int sid;
221 221
222 222 incomingRingNodePtr = NULL;
223 223 ring_node_address = 0;
224 224 charPtr = (char *) &ring_node_address;
225 225 sid = 0;
226 226
227 227 init_header_cwf( &headerCWF );
228 228 init_header_swf( &headerSWF );
229 229 init_header_asm( &headerASM );
230 230
231 231 status = get_message_queue_id_send( &queue_id );
232 232 if (status != RTEMS_SUCCESSFUL)
233 233 {
234 234 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
235 235 }
236 236
237 237 BOOT_PRINTF("in SEND *** \n")
238 238
239 239 while(1)
240 240 {
241 241 status = rtems_message_queue_receive( queue_id, incomingData, &size,
242 242 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
243 243
244 244 if (status!=RTEMS_SUCCESSFUL)
245 245 {
246 246 PRINTF1("in SEND *** (1) ERR = %d\n", status)
247 247 }
248 248 else
249 249 {
250 250 if ( size == sizeof(ring_node*) )
251 251 {
252 252 charPtr[0] = incomingData[0];
253 253 charPtr[1] = incomingData[1];
254 254 charPtr[2] = incomingData[2];
255 255 charPtr[3] = incomingData[3];
256 256 incomingRingNodePtr = (ring_node*) ring_node_address;
257 257 sid = incomingRingNodePtr->sid;
258 258 if ( (sid==SID_NORM_CWF_LONG_F3)
259 259 || (sid==SID_BURST_CWF_F2 )
260 260 || (sid==SID_SBM1_CWF_F1 )
261 261 || (sid==SID_SBM2_CWF_F2 ))
262 262 {
263 263 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
264 264 }
265 265 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
266 266 {
267 267 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
268 268 }
269 269 else if ( (sid==SID_NORM_CWF_F3) )
270 270 {
271 271 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
272 272 }
273 else if ( (sid==SID_NORM_ASM_F0) || (sid==SID_NORM_ASM_F1) || (sid==SID_NORM_ASM_F2) )
273 else if (sid==SID_NORM_ASM_F0)
274 {
275 spw_send_asm_f0( incomingRingNodePtr, &headerASM );
276 }
277 else if (sid==SID_NORM_ASM_F1)
274 278 {
275 spw_send_asm( incomingRingNodePtr, &headerASM );
279 spw_send_asm_f1( incomingRingNodePtr, &headerASM );
280 }
281 else if (sid==SID_NORM_ASM_F2)
282 {
283 spw_send_asm_f2( incomingRingNodePtr, &headerASM );
276 284 }
277 285 else if ( sid==TM_CODE_K_DUMP )
278 286 {
279 287 spw_send_k_dump( incomingRingNodePtr );
280 288 }
281 289 else
282 290 {
283 291 printf("unexpected sid = %d\n", sid);
284 292 }
285 293 }
286 294 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
287 295 {
288 296 status = write( fdSPW, incomingData, size );
289 297 if (status == -1){
290 298 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
291 299 }
292 300 }
293 301 else // the incoming message is a spw_ioctl_pkt_send structure
294 302 {
295 303 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
296 304 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
297 305 if (status == -1){
298 306 printf("size = %d, %x, %x, %x, %x, %x\n",
299 307 size,
300 308 incomingData[0],
301 309 incomingData[1],
302 310 incomingData[2],
303 311 incomingData[3],
304 312 incomingData[4]);
305 313 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
306 314 }
307 315 }
308 316 }
309 317
310 318 status = rtems_message_queue_get_number_pending( queue_id, &count );
311 319 if (status != RTEMS_SUCCESSFUL)
312 320 {
313 321 PRINTF1("in SEND *** (3) ERR = %d\n", status)
314 322 }
315 323 else
316 324 {
317 325 if (count > maxCount)
318 326 {
319 327 maxCount = count;
320 328 }
321 329 }
322 330 }
323 331 }
324 332
325 333 rtems_task wtdg_task( rtems_task_argument argument )
326 334 {
327 335 rtems_event_set event_out;
328 336 rtems_status_code status;
329 337 int linkStatus;
330 338
331 339 BOOT_PRINTF("in WTDG ***\n")
332 340
333 341 while(1)
334 342 {
335 343 // wait for an RTEMS_EVENT
336 344 rtems_event_receive( RTEMS_EVENT_0,
337 345 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
338 346 PRINTF("in WTDG *** wait for the link\n")
339 347 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
340 348 while( linkStatus != 5) // wait for the link
341 349 {
342 350 status = rtems_task_wake_after( 10 ); // monitor the link each 100ms
343 351 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
344 352 }
345 353
346 354 status = spacewire_stop_and_start_link( fdSPW );
347 355
348 356 if (status != RTEMS_SUCCESSFUL)
349 357 {
350 358 PRINTF1("in WTDG *** ERR link not started %d\n", status)
351 359 }
352 360 else
353 361 {
354 362 PRINTF("in WTDG *** OK link started\n")
355 363 }
356 364
357 365 // restart the SPIQ task
358 366 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
359 367 if ( status != RTEMS_SUCCESSFUL ) {
360 368 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
361 369 }
362 370
363 371 // restart RECV and SEND
364 372 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
365 373 if ( status != RTEMS_SUCCESSFUL ) {
366 374 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
367 375 }
368 376 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
369 377 if ( status != RTEMS_SUCCESSFUL ) {
370 378 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
371 379 }
372 380 }
373 381 }
374 382
375 383 //****************
376 384 // OTHER FUNCTIONS
377 385 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
378 386 {
379 387 /** This function opens the SpaceWire link.
380 388 *
381 389 * @return a valid file descriptor in case of success, -1 in case of a failure
382 390 *
383 391 */
384 392 rtems_status_code status;
385 393
386 394 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
387 395 if ( fdSPW < 0 ) {
388 396 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
389 397 }
390 398 else
391 399 {
392 400 status = RTEMS_SUCCESSFUL;
393 401 }
394 402
395 403 return status;
396 404 }
397 405
398 406 int spacewire_start_link( int fd )
399 407 {
400 408 rtems_status_code status;
401 409
402 410 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
403 411 // -1 default hardcoded driver timeout
404 412
405 413 return status;
406 414 }
407 415
408 416 int spacewire_stop_and_start_link( int fd )
409 417 {
410 418 rtems_status_code status;
411 419
412 420 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
413 421 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
414 422 // -1 default hardcoded driver timeout
415 423
416 424 return status;
417 425 }
418 426
419 427 int spacewire_configure_link( int fd )
420 428 {
421 429 /** This function configures the SpaceWire link.
422 430 *
423 431 * @return GR-RTEMS-DRIVER directive status codes:
424 432 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
425 433 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
426 434 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
427 435 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
428 436 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
429 437 * - 5 EIO - Error when writing to grswp hardware registers.
430 438 * - 2 ENOENT - No such file or directory
431 439 */
432 440
433 441 rtems_status_code status;
434 442
435 443 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
436 444 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
437 445
438 446 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
439 447 if (status!=RTEMS_SUCCESSFUL) {
440 448 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
441 449 }
442 450 //
443 451 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
444 452 if (status!=RTEMS_SUCCESSFUL) {
445 453 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
446 454 }
447 455 //
448 456 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
449 457 if (status!=RTEMS_SUCCESSFUL) {
450 458 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
451 459 }
452 460 //
453 461 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
454 462 if (status!=RTEMS_SUCCESSFUL) {
455 463 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
456 464 }
457 465 //
458 466 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
459 467 if (status!=RTEMS_SUCCESSFUL) {
460 468 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
461 469 }
462 470 //
463 471 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
464 472 if (status!=RTEMS_SUCCESSFUL) {
465 473 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
466 474 }
467 475 //
468 476 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, 0x0909); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
469 477 if (status!=RTEMS_SUCCESSFUL) {
470 478 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
471 479 }
472 480
473 481 return status;
474 482 }
475 483
476 484 int spacewire_reset_link( void )
477 485 {
478 486 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
479 487 *
480 488 * @return RTEMS directive status code:
481 489 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
482 490 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
483 491 *
484 492 */
485 493
486 494 rtems_status_code status_spw;
487 495 rtems_status_code status;
488 496 int i;
489 497
490 498 for ( i=0; i<SY_LFR_DPU_CONNECT_ATTEMPT; i++ )
491 499 {
492 500 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
493 501
494 502 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
495 503
496 504 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
497 505
498 506 status_spw = spacewire_stop_and_start_link( fdSPW );
499 507 if ( status_spw != RTEMS_SUCCESSFUL )
500 508 {
501 509 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw)
502 510 }
503 511
504 512 if ( status_spw == RTEMS_SUCCESSFUL)
505 513 {
506 514 break;
507 515 }
508 516 }
509 517
510 518 return status_spw;
511 519 }
512 520
513 521 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
514 522 {
515 523 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
516 524 *
517 525 * @param val is the value, 0 or 1, used to set the value of the NP bit.
518 526 * @param regAddr is the address of the GRSPW control register.
519 527 *
520 528 * NP is the bit 20 of the GRSPW control register.
521 529 *
522 530 */
523 531
524 532 unsigned int *spwptr = (unsigned int*) regAddr;
525 533
526 534 if (val == 1) {
527 535 *spwptr = *spwptr | 0x00100000; // [NP] set the No port force bit
528 536 }
529 537 if (val== 0) {
530 538 *spwptr = *spwptr & 0xffdfffff;
531 539 }
532 540 }
533 541
534 542 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
535 543 {
536 544 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
537 545 *
538 546 * @param val is the value, 0 or 1, used to set the value of the RE bit.
539 547 * @param regAddr is the address of the GRSPW control register.
540 548 *
541 549 * RE is the bit 16 of the GRSPW control register.
542 550 *
543 551 */
544 552
545 553 unsigned int *spwptr = (unsigned int*) regAddr;
546 554
547 555 if (val == 1)
548 556 {
549 557 *spwptr = *spwptr | 0x00010000; // [RE] set the RMAP Enable bit
550 558 }
551 559 if (val== 0)
552 560 {
553 561 *spwptr = *spwptr & 0xfffdffff;
554 562 }
555 563 }
556 564
557 565 void spacewire_compute_stats_offsets( void )
558 566 {
559 567 /** This function computes the SpaceWire statistics offsets in case of a SpaceWire related interruption raising.
560 568 *
561 569 * The offsets keep a record of the statistics in case of a reset of the statistics. They are added to the current statistics
562 570 * to keep the counters consistent even after a reset of the SpaceWire driver (the counter are set to zero by the driver when it
563 571 * during the open systel call).
564 572 *
565 573 */
566 574
567 575 spw_stats spacewire_stats_grspw;
568 576 rtems_status_code status;
569 577
570 578 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
571 579
572 580 spacewire_stats_backup.packets_received = spacewire_stats_grspw.packets_received
573 581 + spacewire_stats.packets_received;
574 582 spacewire_stats_backup.packets_sent = spacewire_stats_grspw.packets_sent
575 583 + spacewire_stats.packets_sent;
576 584 spacewire_stats_backup.parity_err = spacewire_stats_grspw.parity_err
577 585 + spacewire_stats.parity_err;
578 586 spacewire_stats_backup.disconnect_err = spacewire_stats_grspw.disconnect_err
579 587 + spacewire_stats.disconnect_err;
580 588 spacewire_stats_backup.escape_err = spacewire_stats_grspw.escape_err
581 589 + spacewire_stats.escape_err;
582 590 spacewire_stats_backup.credit_err = spacewire_stats_grspw.credit_err
583 591 + spacewire_stats.credit_err;
584 592 spacewire_stats_backup.write_sync_err = spacewire_stats_grspw.write_sync_err
585 593 + spacewire_stats.write_sync_err;
586 594 spacewire_stats_backup.rx_rmap_header_crc_err = spacewire_stats_grspw.rx_rmap_header_crc_err
587 595 + spacewire_stats.rx_rmap_header_crc_err;
588 596 spacewire_stats_backup.rx_rmap_data_crc_err = spacewire_stats_grspw.rx_rmap_data_crc_err
589 597 + spacewire_stats.rx_rmap_data_crc_err;
590 598 spacewire_stats_backup.early_ep = spacewire_stats_grspw.early_ep
591 599 + spacewire_stats.early_ep;
592 600 spacewire_stats_backup.invalid_address = spacewire_stats_grspw.invalid_address
593 601 + spacewire_stats.invalid_address;
594 602 spacewire_stats_backup.rx_eep_err = spacewire_stats_grspw.rx_eep_err
595 603 + spacewire_stats.rx_eep_err;
596 604 spacewire_stats_backup.rx_truncated = spacewire_stats_grspw.rx_truncated
597 605 + spacewire_stats.rx_truncated;
598 606 }
599 607
600 608 void spacewire_update_statistics( void )
601 609 {
602 610 rtems_status_code status;
603 611 spw_stats spacewire_stats_grspw;
604 612
605 613 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
606 614
607 615 spacewire_stats.packets_received = spacewire_stats_backup.packets_received
608 616 + spacewire_stats_grspw.packets_received;
609 617 spacewire_stats.packets_sent = spacewire_stats_backup.packets_sent
610 618 + spacewire_stats_grspw.packets_sent;
611 619 spacewire_stats.parity_err = spacewire_stats_backup.parity_err
612 620 + spacewire_stats_grspw.parity_err;
613 621 spacewire_stats.disconnect_err = spacewire_stats_backup.disconnect_err
614 622 + spacewire_stats_grspw.disconnect_err;
615 623 spacewire_stats.escape_err = spacewire_stats_backup.escape_err
616 624 + spacewire_stats_grspw.escape_err;
617 625 spacewire_stats.credit_err = spacewire_stats_backup.credit_err
618 626 + spacewire_stats_grspw.credit_err;
619 627 spacewire_stats.write_sync_err = spacewire_stats_backup.write_sync_err
620 628 + spacewire_stats_grspw.write_sync_err;
621 629 spacewire_stats.rx_rmap_header_crc_err = spacewire_stats_backup.rx_rmap_header_crc_err
622 630 + spacewire_stats_grspw.rx_rmap_header_crc_err;
623 631 spacewire_stats.rx_rmap_data_crc_err = spacewire_stats_backup.rx_rmap_data_crc_err
624 632 + spacewire_stats_grspw.rx_rmap_data_crc_err;
625 633 spacewire_stats.early_ep = spacewire_stats_backup.early_ep
626 634 + spacewire_stats_grspw.early_ep;
627 635 spacewire_stats.invalid_address = spacewire_stats_backup.invalid_address
628 636 + spacewire_stats_grspw.invalid_address;
629 637 spacewire_stats.rx_eep_err = spacewire_stats_backup.rx_eep_err
630 638 + spacewire_stats_grspw.rx_eep_err;
631 639 spacewire_stats.rx_truncated = spacewire_stats_backup.rx_truncated
632 640 + spacewire_stats_grspw.rx_truncated;
633 641 //spacewire_stats.tx_link_err;
634 642
635 643 //****************************
636 644 // DPU_SPACEWIRE_IF_STATISTICS
637 645 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (spacewire_stats.packets_received >> 8);
638 646 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (spacewire_stats.packets_received);
639 647 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (spacewire_stats.packets_sent >> 8);
640 648 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (spacewire_stats.packets_sent);
641 649 //housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt;
642 650 //housekeeping_packet.hk_lfr_dpu_spw_last_timc;
643 651
644 652 //******************************************
645 653 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
646 654 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) spacewire_stats.parity_err;
647 655 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) spacewire_stats.disconnect_err;
648 656 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) spacewire_stats.escape_err;
649 657 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) spacewire_stats.credit_err;
650 658 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) spacewire_stats.write_sync_err;
651 659
652 660 //*********************************************
653 661 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
654 662 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) spacewire_stats.early_ep;
655 663 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) spacewire_stats.invalid_address;
656 664 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) spacewire_stats.rx_eep_err;
657 665 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) spacewire_stats.rx_truncated;
658 666 }
659 667
660 668 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
661 669 {
662 670 // a valid timecode has been received, write it in the HK report
663 671 unsigned int * grspwPtr;
664 672
665 673 grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER);
666 674
667 675 housekeeping_packet.hk_lfr_dpu_spw_last_timc = (unsigned char) (grspwPtr[0] & 0xff); // [11 1111]
668 676
669 677 // update the number of valid timecodes that have been received
670 678 if (housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt == 255)
671 679 {
672 680 housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt = 0;
673 681 }
674 682 else
675 683 {
676 684 housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt = housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt + 1;
677 685 }
678 686 }
679 687
680 688 rtems_timer_service_routine user_routine( rtems_id timer_id, void *user_data )
681 689 {
682 690 int linkStatus;
683 691 rtems_status_code status;
684 692
685 693 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
686 694
687 695 if ( linkStatus == 5) {
688 696 PRINTF("in spacewire_reset_link *** link is running\n")
689 697 status = RTEMS_SUCCESSFUL;
690 698 }
691 699 }
692 700
693 701 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
694 702 {
695 703 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
696 704 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
697 705 header->reserved = DEFAULT_RESERVED;
698 706 header->userApplication = CCSDS_USER_APP;
699 707 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
700 708 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
701 709 header->packetLength[0] = 0x00;
702 710 header->packetLength[1] = 0x00;
703 711 // DATA FIELD HEADER
704 712 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
705 713 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
706 714 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
707 715 header->destinationID = TM_DESTINATION_ID_GROUND;
708 716 header->time[0] = 0x00;
709 717 header->time[0] = 0x00;
710 718 header->time[0] = 0x00;
711 719 header->time[0] = 0x00;
712 720 header->time[0] = 0x00;
713 721 header->time[0] = 0x00;
714 722 // AUXILIARY DATA HEADER
715 723 header->sid = 0x00;
716 724 header->hkBIA = DEFAULT_HKBIA;
717 725 header->blkNr[0] = 0x00;
718 726 header->blkNr[1] = 0x00;
719 727 }
720 728
721 729 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
722 730 {
723 731 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
724 732 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
725 733 header->reserved = DEFAULT_RESERVED;
726 734 header->userApplication = CCSDS_USER_APP;
727 735 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
728 736 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
729 737 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
730 738 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
731 739 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
732 740 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
733 741 // DATA FIELD HEADER
734 742 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
735 743 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
736 744 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
737 745 header->destinationID = TM_DESTINATION_ID_GROUND;
738 746 header->time[0] = 0x00;
739 747 header->time[0] = 0x00;
740 748 header->time[0] = 0x00;
741 749 header->time[0] = 0x00;
742 750 header->time[0] = 0x00;
743 751 header->time[0] = 0x00;
744 752 // AUXILIARY DATA HEADER
745 753 header->sid = 0x00;
746 754 header->hkBIA = DEFAULT_HKBIA;
747 755 header->pktCnt = DEFAULT_PKTCNT; // PKT_CNT
748 756 header->pktNr = 0x00;
749 757 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
750 758 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
751 759 }
752 760
753 761 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
754 762 {
755 763 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
756 764 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
757 765 header->reserved = DEFAULT_RESERVED;
758 766 header->userApplication = CCSDS_USER_APP;
759 767 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
760 768 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
761 769 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
762 770 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
763 771 header->packetLength[0] = 0x00;
764 772 header->packetLength[1] = 0x00;
765 773 // DATA FIELD HEADER
766 774 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
767 775 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
768 776 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
769 777 header->destinationID = TM_DESTINATION_ID_GROUND;
770 778 header->time[0] = 0x00;
771 779 header->time[0] = 0x00;
772 780 header->time[0] = 0x00;
773 781 header->time[0] = 0x00;
774 782 header->time[0] = 0x00;
775 783 header->time[0] = 0x00;
776 784 // AUXILIARY DATA HEADER
777 785 header->sid = 0x00;
778 786 header->biaStatusInfo = 0x00;
779 787 header->pa_lfr_pkt_cnt_asm = 0x00;
780 788 header->pa_lfr_pkt_nr_asm = 0x00;
781 789 header->pa_lfr_asm_blk_nr[0] = 0x00;
782 790 header->pa_lfr_asm_blk_nr[1] = 0x00;
783 791 }
784 792
785 793 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
786 794 Header_TM_LFR_SCIENCE_CWF_t *header )
787 795 {
788 796 /** This function sends CWF CCSDS packets (F2, F1 or F0).
789 797 *
790 798 * @param waveform points to the buffer containing the data that will be send.
791 799 * @param sid is the source identifier of the data that will be sent.
792 800 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
793 801 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
794 802 * contain information to setup the transmission of the data packets.
795 803 *
796 804 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
797 805 *
798 806 */
799 807
800 808 unsigned int i;
801 809 int ret;
802 810 unsigned int coarseTime;
803 811 unsigned int fineTime;
804 812 rtems_status_code status;
805 813 spw_ioctl_pkt_send spw_ioctl_send_CWF;
806 814 int *dataPtr;
807 815 unsigned char sid;
808 816
809 spw_ioctl_send_CWF.hlen = TM_HEADER_LEN + 4 + 10; // + 4 is for the protocole extra header, + 10 is for the auxiliary header
817 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
810 818 spw_ioctl_send_CWF.options = 0;
811 819
812 820 ret = LFR_DEFAULT;
813 821 sid = (unsigned char) ring_node_to_send->sid;
814 822
815 823 coarseTime = ring_node_to_send->coarseTime;
816 824 fineTime = ring_node_to_send->fineTime;
817 825 dataPtr = (int*) ring_node_to_send->buffer_address;
818 826
819 827 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
820 828 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
821 829 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
822 830 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
823 831 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
824 832
825 833 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
826 834 {
827 835 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
828 836 spw_ioctl_send_CWF.hdr = (char*) header;
829 837 // BUILD THE DATA
830 838 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
831 839
832 840 // SET PACKET SEQUENCE CONTROL
833 841 increment_seq_counter_source_id( header->packetSequenceControl, sid );
834 842
835 843 // SET SID
836 844 header->sid = sid;
837 845
838 846 // SET PACKET TIME
839 847 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
840 848 //
841 849 header->time[0] = header->acquisitionTime[0];
842 850 header->time[1] = header->acquisitionTime[1];
843 851 header->time[2] = header->acquisitionTime[2];
844 852 header->time[3] = header->acquisitionTime[3];
845 853 header->time[4] = header->acquisitionTime[4];
846 854 header->time[5] = header->acquisitionTime[5];
847 855
848 856 // SET PACKET ID
849 857 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
850 858 {
851 859 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8);
852 860 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
853 861 }
854 862 else
855 863 {
856 864 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
857 865 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
858 866 }
859 867
860 868 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
861 869 if (status != RTEMS_SUCCESSFUL) {
862 870 printf("%d-%d, ERR %d\n", sid, i, (int) status);
863 871 ret = LFR_DEFAULT;
864 872 }
865 873 }
866 874
867 875 return ret;
868 876 }
869 877
870 878 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
871 879 Header_TM_LFR_SCIENCE_SWF_t *header )
872 880 {
873 881 /** This function sends SWF CCSDS packets (F2, F1 or F0).
874 882 *
875 883 * @param waveform points to the buffer containing the data that will be send.
876 884 * @param sid is the source identifier of the data that will be sent.
877 885 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
878 886 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
879 887 * contain information to setup the transmission of the data packets.
880 888 *
881 889 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
882 890 *
883 891 */
884 892
885 893 unsigned int i;
886 894 int ret;
887 895 unsigned int coarseTime;
888 896 unsigned int fineTime;
889 897 rtems_status_code status;
890 898 spw_ioctl_pkt_send spw_ioctl_send_SWF;
891 899 int *dataPtr;
892 900 unsigned char sid;
893 901
894 spw_ioctl_send_SWF.hlen = TM_HEADER_LEN + 4 + 12; // + 4 is for the protocole extra header, + 12 is for the auxiliary header
902 spw_ioctl_send_SWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_SWF;
895 903 spw_ioctl_send_SWF.options = 0;
896 904
897 905 ret = LFR_DEFAULT;
898 906
899 907 coarseTime = ring_node_to_send->coarseTime;
900 908 fineTime = ring_node_to_send->fineTime;
901 909 dataPtr = (int*) ring_node_to_send->buffer_address;
902 910 sid = ring_node_to_send->sid;
903 911
904 912 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
905 913
906 914 for (i=0; i<7; i++) // send waveform
907 915 {
908 916 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
909 917 spw_ioctl_send_SWF.hdr = (char*) header;
910 918
911 919 // SET PACKET SEQUENCE CONTROL
912 920 increment_seq_counter_source_id( header->packetSequenceControl, sid );
913 921
914 922 // SET PACKET LENGTH AND BLKNR
915 923 if (i == 6)
916 924 {
917 925 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
918 926 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8);
919 927 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
920 928 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> 8);
921 929 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
922 930 }
923 931 else
924 932 {
925 933 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
926 934 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8);
927 935 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
928 936 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> 8);
929 937 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
930 938 }
931 939
932 940 // SET PACKET TIME
933 941 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
934 942 //
935 943 header->time[0] = header->acquisitionTime[0];
936 944 header->time[1] = header->acquisitionTime[1];
937 945 header->time[2] = header->acquisitionTime[2];
938 946 header->time[3] = header->acquisitionTime[3];
939 947 header->time[4] = header->acquisitionTime[4];
940 948 header->time[5] = header->acquisitionTime[5];
941 949
942 950 // SET SID
943 951 header->sid = sid;
944 952
945 953 // SET PKTNR
946 954 header->pktNr = i+1; // PKT_NR
947 955
948 956 // SEND PACKET
949 957 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
950 958 if (status != RTEMS_SUCCESSFUL) {
951 959 printf("%d-%d, ERR %d\n", sid, i, (int) status);
952 960 ret = LFR_DEFAULT;
953 961 }
954 962 }
955 963
956 964 return ret;
957 965 }
958 966
959 967 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
960 968 Header_TM_LFR_SCIENCE_CWF_t *header )
961 969 {
962 970 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
963 971 *
964 972 * @param waveform points to the buffer containing the data that will be send.
965 973 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
966 974 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
967 975 * contain information to setup the transmission of the data packets.
968 976 *
969 977 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
970 978 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
971 979 *
972 980 */
973 981
974 982 unsigned int i;
975 983 int ret;
976 984 unsigned int coarseTime;
977 985 unsigned int fineTime;
978 986 rtems_status_code status;
979 987 spw_ioctl_pkt_send spw_ioctl_send_CWF;
980 988 char *dataPtr;
981 989 unsigned char sid;
982 990
983 spw_ioctl_send_CWF.hlen = TM_HEADER_LEN + 4 + 10; // + 4 is for the protocole extra header, + 10 is for the auxiliary header
991 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
984 992 spw_ioctl_send_CWF.options = 0;
985 993
986 994 ret = LFR_DEFAULT;
987 995 sid = ring_node_to_send->sid;
988 996
989 997 coarseTime = ring_node_to_send->coarseTime;
990 998 fineTime = ring_node_to_send->fineTime;
991 999 dataPtr = (char*) ring_node_to_send->buffer_address;
992 1000
993 1001 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> 8);
994 1002 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
995 1003 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
996 1004 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> 8);
997 1005 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
998 1006
999 1007 //*********************
1000 1008 // SEND CWF3_light DATA
1001 1009 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
1002 1010 {
1003 1011 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
1004 1012 spw_ioctl_send_CWF.hdr = (char*) header;
1005 1013 // BUILD THE DATA
1006 1014 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
1007 1015
1008 1016 // SET PACKET SEQUENCE COUNTER
1009 1017 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1010 1018
1011 1019 // SET SID
1012 1020 header->sid = sid;
1013 1021
1014 1022 // SET PACKET TIME
1015 1023 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1016 1024 //
1017 1025 header->time[0] = header->acquisitionTime[0];
1018 1026 header->time[1] = header->acquisitionTime[1];
1019 1027 header->time[2] = header->acquisitionTime[2];
1020 1028 header->time[3] = header->acquisitionTime[3];
1021 1029 header->time[4] = header->acquisitionTime[4];
1022 1030 header->time[5] = header->acquisitionTime[5];
1023 1031
1024 1032 // SET PACKET ID
1025 1033 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1026 1034 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1027 1035
1028 1036 // SEND PACKET
1029 1037 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1030 1038 if (status != RTEMS_SUCCESSFUL) {
1031 1039 printf("%d-%d, ERR %d\n", sid, i, (int) status);
1032 1040 ret = LFR_DEFAULT;
1033 1041 }
1034 1042 }
1035 1043
1036 1044 return ret;
1037 1045 }
1038 1046
1039 void spw_send_asm( ring_node *ring_node_to_send,
1047 void spw_send_asm_f0( ring_node *ring_node_to_send,
1040 1048 Header_TM_LFR_SCIENCE_ASM_t *header )
1041 1049 {
1042 1050 unsigned int i;
1043 1051 unsigned int length = 0;
1044 1052 rtems_status_code status;
1045 1053 unsigned int sid;
1046 char *spectral_matrix;
1054 float *spectral_matrix;
1047 1055 int coarseTime;
1048 1056 int fineTime;
1049 1057 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1050 1058
1051 1059 sid = ring_node_to_send->sid;
1052 spectral_matrix = (char*) ring_node_to_send->buffer_address;
1060 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1053 1061 coarseTime = ring_node_to_send->coarseTime;
1054 1062 fineTime = ring_node_to_send->fineTime;
1055 1063
1056 1064 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1057 1065
1058 for (i=0; i<2; i++)
1066 for (i=0; i<3; i++)
1059 1067 {
1060 // (1) BUILD THE DATA
1061 switch(sid)
1068 if ((i==0) || (i==1))
1062 1069 {
1063 case SID_NORM_ASM_F0:
1064 spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F0_IN_BYTES / 2; // 2 packets will be sent
1065 spw_ioctl_send_ASM.data = &spectral_matrix[
1066 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0) ) * NB_VALUES_PER_SM ) * 2
1067 ];
1068 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0;
1069 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1070 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0) >> 8 ); // BLK_NR MSB
1071 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0); // BLK_NR LSB
1072 break;
1073 case SID_NORM_ASM_F1:
1074 spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F1_IN_BYTES / 2; // 2 packets will be sent
1075 spw_ioctl_send_ASM.data = &spectral_matrix[
1076 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1) ) * NB_VALUES_PER_SM ) * 2
1070 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_1;
1071 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1072 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1077 1073 ];
1078 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1;
1074 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_1;
1079 1075 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1080 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1) >> 8 ); // BLK_NR MSB
1081 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1); // BLK_NR LSB
1082 break;
1083 case SID_NORM_ASM_F2:
1084 spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F2_IN_BYTES / 2; // 2 packets will be sent
1085 spw_ioctl_send_ASM.data = &spectral_matrix[
1086 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM ) * 2
1076 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> 8 ); // BLK_NR MSB
1077 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_1); // BLK_NR LSB
1078 }
1079 else
1080 {
1081 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_2;
1082 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1083 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1087 1084 ];
1088 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1089 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1090 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB
1091 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1092 break;
1093 default:
1094 PRINTF1("ERR *** in spw_send_asm *** unexpected sid %d\n", sid)
1095 break;
1085 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_2;
1086 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1087 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> 8 ); // BLK_NR MSB
1088 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_2); // BLK_NR LSB
1096 1089 }
1097 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM + CCSDS_PROTOCOLE_EXTRA_BYTES;
1090
1091 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1098 1092 spw_ioctl_send_ASM.hdr = (char *) header;
1099 1093 spw_ioctl_send_ASM.options = 0;
1100 1094
1101 1095 // (2) BUILD THE HEADER
1102 1096 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1103 1097 header->packetLength[0] = (unsigned char) (length>>8);
1104 1098 header->packetLength[1] = (unsigned char) (length);
1105 1099 header->sid = (unsigned char) sid; // SID
1106 header->pa_lfr_pkt_cnt_asm = 2;
1100 header->pa_lfr_pkt_cnt_asm = 3;
1101 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1102
1103 // (3) SET PACKET TIME
1104 header->time[0] = (unsigned char) (coarseTime>>24);
1105 header->time[1] = (unsigned char) (coarseTime>>16);
1106 header->time[2] = (unsigned char) (coarseTime>>8);
1107 header->time[3] = (unsigned char) (coarseTime);
1108 header->time[4] = (unsigned char) (fineTime>>8);
1109 header->time[5] = (unsigned char) (fineTime);
1110 //
1111 header->acquisitionTime[0] = header->time[0];
1112 header->acquisitionTime[1] = header->time[1];
1113 header->acquisitionTime[2] = header->time[2];
1114 header->acquisitionTime[3] = header->time[3];
1115 header->acquisitionTime[4] = header->time[4];
1116 header->acquisitionTime[5] = header->time[5];
1117
1118 // (4) SEND PACKET
1119 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1120 if (status != RTEMS_SUCCESSFUL) {
1121 printf("in ASM_send *** ERR %d\n", (int) status);
1122 }
1123 }
1124 }
1125
1126 void spw_send_asm_f1( ring_node *ring_node_to_send,
1127 Header_TM_LFR_SCIENCE_ASM_t *header )
1128 {
1129 unsigned int i;
1130 unsigned int length = 0;
1131 rtems_status_code status;
1132 unsigned int sid;
1133 float *spectral_matrix;
1134 int coarseTime;
1135 int fineTime;
1136 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1137
1138 sid = ring_node_to_send->sid;
1139 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1140 coarseTime = ring_node_to_send->coarseTime;
1141 fineTime = ring_node_to_send->fineTime;
1142
1143 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1144
1145 for (i=0; i<3; i++)
1146 {
1147 if ((i==0) || (i==1))
1148 {
1149 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_1;
1150 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1151 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1152 ];
1153 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_1;
1154 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1155 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> 8 ); // BLK_NR MSB
1156 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_1); // BLK_NR LSB
1157 }
1158 else
1159 {
1160 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_2;
1161 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1162 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1163 ];
1164 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_2;
1165 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1166 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> 8 ); // BLK_NR MSB
1167 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_2); // BLK_NR LSB
1168 }
1169
1170 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1171 spw_ioctl_send_ASM.hdr = (char *) header;
1172 spw_ioctl_send_ASM.options = 0;
1173
1174 // (2) BUILD THE HEADER
1175 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1176 header->packetLength[0] = (unsigned char) (length>>8);
1177 header->packetLength[1] = (unsigned char) (length);
1178 header->sid = (unsigned char) sid; // SID
1179 header->pa_lfr_pkt_cnt_asm = 3;
1180 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1181
1182 // (3) SET PACKET TIME
1183 header->time[0] = (unsigned char) (coarseTime>>24);
1184 header->time[1] = (unsigned char) (coarseTime>>16);
1185 header->time[2] = (unsigned char) (coarseTime>>8);
1186 header->time[3] = (unsigned char) (coarseTime);
1187 header->time[4] = (unsigned char) (fineTime>>8);
1188 header->time[5] = (unsigned char) (fineTime);
1189 //
1190 header->acquisitionTime[0] = header->time[0];
1191 header->acquisitionTime[1] = header->time[1];
1192 header->acquisitionTime[2] = header->time[2];
1193 header->acquisitionTime[3] = header->time[3];
1194 header->acquisitionTime[4] = header->time[4];
1195 header->acquisitionTime[5] = header->time[5];
1196
1197 // (4) SEND PACKET
1198 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1199 if (status != RTEMS_SUCCESSFUL) {
1200 printf("in ASM_send *** ERR %d\n", (int) status);
1201 }
1202 }
1203 }
1204
1205 void spw_send_asm_f2( ring_node *ring_node_to_send,
1206 Header_TM_LFR_SCIENCE_ASM_t *header )
1207 {
1208 unsigned int i;
1209 unsigned int length = 0;
1210 rtems_status_code status;
1211 unsigned int sid;
1212 float *spectral_matrix;
1213 int coarseTime;
1214 int fineTime;
1215 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1216
1217 sid = ring_node_to_send->sid;
1218 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1219 coarseTime = ring_node_to_send->coarseTime;
1220 fineTime = ring_node_to_send->fineTime;
1221
1222 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1223
1224 for (i=0; i<3; i++)
1225 {
1226
1227 spw_ioctl_send_ASM.dlen = DLEN_ASM_F2_PKT;
1228 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1229 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM )
1230 ];
1231 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1232 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1233 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB
1234 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1235
1236 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1237 spw_ioctl_send_ASM.hdr = (char *) header;
1238 spw_ioctl_send_ASM.options = 0;
1239
1240 // (2) BUILD THE HEADER
1241 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1242 header->packetLength[0] = (unsigned char) (length>>8);
1243 header->packetLength[1] = (unsigned char) (length);
1244 header->sid = (unsigned char) sid; // SID
1245 header->pa_lfr_pkt_cnt_asm = 3;
1107 1246 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1108 1247
1109 1248 // (3) SET PACKET TIME
1110 1249 header->time[0] = (unsigned char) (coarseTime>>24);
1111 1250 header->time[1] = (unsigned char) (coarseTime>>16);
1112 1251 header->time[2] = (unsigned char) (coarseTime>>8);
1113 1252 header->time[3] = (unsigned char) (coarseTime);
1114 1253 header->time[4] = (unsigned char) (fineTime>>8);
1115 1254 header->time[5] = (unsigned char) (fineTime);
1116 1255 //
1117 1256 header->acquisitionTime[0] = header->time[0];
1118 1257 header->acquisitionTime[1] = header->time[1];
1119 1258 header->acquisitionTime[2] = header->time[2];
1120 1259 header->acquisitionTime[3] = header->time[3];
1121 1260 header->acquisitionTime[4] = header->time[4];
1122 1261 header->acquisitionTime[5] = header->time[5];
1123 1262
1124 1263 // (4) SEND PACKET
1125 1264 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1126 1265 if (status != RTEMS_SUCCESSFUL) {
1127 1266 printf("in ASM_send *** ERR %d\n", (int) status);
1128 1267 }
1129 1268 }
1130 1269 }
1131 1270
1132 1271 void spw_send_k_dump( ring_node *ring_node_to_send )
1133 1272 {
1134 1273 rtems_status_code status;
1135 1274 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump;
1136 1275 unsigned int packetLength;
1137 1276 unsigned int size;
1138 1277
1139 1278 printf("spw_send_k_dump\n");
1140 1279
1141 1280 kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address;
1142 1281
1143 1282 packetLength = kcoefficients_dump->packetLength[0] * 256 + kcoefficients_dump->packetLength[1];
1144 1283
1145 1284 size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
1146 1285
1147 1286 printf("packetLength %d, size %d\n", packetLength, size );
1148 1287
1149 1288 status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size );
1150 1289
1151 1290 if (status == -1){
1152 1291 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
1153 1292 }
1154 1293
1155 1294 ring_node_to_send->status = 0x00;
1156 1295 }
@@ -1,401 +1,399
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf0_prc0.h"
11 11 #include "fsw_processing.h"
12 12
13 13 nb_sm_before_bp_asm_f0 nb_sm_before_f0;
14 14
15 15 //***
16 16 // F0
17 17 ring_node_asm asm_ring_norm_f0 [ NB_RING_NODES_ASM_NORM_F0 ];
18 18 ring_node_asm asm_ring_burst_sbm_f0 [ NB_RING_NODES_ASM_BURST_SBM_F0 ];
19 19
20 20 ring_node ring_to_send_asm_f0 [ NB_RING_NODES_ASM_F0 ];
21 21 int buffer_asm_f0 [ NB_RING_NODES_ASM_F0 * TOTAL_SIZE_SM ];
22 22
23 23 float asm_f0_patched_norm [ TOTAL_SIZE_SM ];
24 24 float asm_f0_patched_burst_sbm [ TOTAL_SIZE_SM ];
25 25 float asm_f0_reorganized [ TOTAL_SIZE_SM ];
26 26
27 27 char asm_f0_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ];
28 28 float compressed_sm_norm_f0[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F0];
29 29 float compressed_sm_sbm_f0 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F0 ];
30 30
31 31 float k_coeff_intercalib_f0_norm[ NB_BINS_COMPRESSED_SM_F0 * NB_K_COEFF_PER_BIN ]; // 11 * 32 = 352
32 32 float k_coeff_intercalib_f0_sbm[ NB_BINS_COMPRESSED_SM_SBM_F0 * NB_K_COEFF_PER_BIN ]; // 22 * 32 = 704
33 33
34 34 //************
35 35 // RTEMS TASKS
36 36
37 37 rtems_task avf0_task( rtems_task_argument lfrRequestedMode )
38 38 {
39 39 int i;
40 40
41 41 rtems_event_set event_out;
42 42 rtems_status_code status;
43 43 rtems_id queue_id_prc0;
44 44 asm_msg msgForMATR;
45 45 ring_node *nodeForAveraging;
46 46 ring_node *ring_node_tab[8];
47 47 ring_node_asm *current_ring_node_asm_burst_sbm_f0;
48 48 ring_node_asm *current_ring_node_asm_norm_f0;
49 49
50 50 unsigned int nb_norm_bp1;
51 51 unsigned int nb_norm_bp2;
52 52 unsigned int nb_norm_asm;
53 53 unsigned int nb_sbm_bp1;
54 54 unsigned int nb_sbm_bp2;
55 55
56 56 nb_norm_bp1 = 0;
57 57 nb_norm_bp2 = 0;
58 58 nb_norm_asm = 0;
59 59 nb_sbm_bp1 = 0;
60 60 nb_sbm_bp2 = 0;
61 61
62 62 reset_nb_sm_f0( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
63 63 ASM_generic_init_ring( asm_ring_norm_f0, NB_RING_NODES_ASM_NORM_F0 );
64 64 ASM_generic_init_ring( asm_ring_burst_sbm_f0, NB_RING_NODES_ASM_BURST_SBM_F0 );
65 65 current_ring_node_asm_norm_f0 = asm_ring_norm_f0;
66 66 current_ring_node_asm_burst_sbm_f0 = asm_ring_burst_sbm_f0;
67 67
68 68 BOOT_PRINTF1("in AVFO *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
69 69
70 70 status = get_message_queue_id_prc0( &queue_id_prc0 );
71 71 if (status != RTEMS_SUCCESSFUL)
72 72 {
73 73 PRINTF1("in MATR *** ERR get_message_queue_id_prc0 %d\n", status)
74 74 }
75 75
76 76 while(1){
77 77 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
78 78
79 79 //****************************************
80 80 // initialize the mesage for the MATR task
81 81 msgForMATR.norm = current_ring_node_asm_norm_f0;
82 82 msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f0;
83 83 msgForMATR.event = 0x00; // this composite event will be sent to the PRC0 task
84 84 //
85 85 //****************************************
86 86
87 87 nodeForAveraging = getRingNodeForAveraging( 0 );
88 88
89 89 ring_node_tab[NB_SM_BEFORE_AVF0-1] = nodeForAveraging;
90 90 for ( i = 2; i < (NB_SM_BEFORE_AVF0+1); i++ )
91 91 {
92 92 nodeForAveraging = nodeForAveraging->previous;
93 93 ring_node_tab[NB_SM_BEFORE_AVF0-i] = nodeForAveraging;
94 94 }
95 95
96 96 // compute the average and store it in the averaged_sm_f1 buffer
97 97 SM_average( current_ring_node_asm_norm_f0->matrix,
98 98 current_ring_node_asm_burst_sbm_f0->matrix,
99 99 ring_node_tab,
100 100 nb_norm_bp1, nb_sbm_bp1,
101 101 &msgForMATR );
102 102
103 103 // update nb_average
104 104 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0;
105 105 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0;
106 106 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0;
107 107 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0;
108 108 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0;
109 109
110 110 if (nb_sbm_bp1 == nb_sm_before_f0.burst_sbm_bp1)
111 111 {
112 112 nb_sbm_bp1 = 0;
113 113 // set another ring for the ASM storage
114 114 current_ring_node_asm_burst_sbm_f0 = current_ring_node_asm_burst_sbm_f0->next;
115 115 if ( lfrCurrentMode == LFR_MODE_BURST )
116 116 {
117 117 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP1_F0;
118 118 }
119 119 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
120 120 {
121 121 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP1_F0;
122 122 }
123 123 }
124 124
125 125 if (nb_sbm_bp2 == nb_sm_before_f0.burst_sbm_bp2)
126 126 {
127 127 nb_sbm_bp2 = 0;
128 128 if ( lfrCurrentMode == LFR_MODE_BURST )
129 129 {
130 130 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP2_F0;
131 131 }
132 132 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
133 133 {
134 134 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP2_F0;
135 135 }
136 136 }
137 137
138 138 if (nb_norm_bp1 == nb_sm_before_f0.norm_bp1)
139 139 {
140 140 nb_norm_bp1 = 0;
141 141 // set another ring for the ASM storage
142 142 current_ring_node_asm_norm_f0 = current_ring_node_asm_norm_f0->next;
143 143 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
144 144 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
145 145 {
146 146 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F0;
147 147 }
148 148 }
149 149
150 150 if (nb_norm_bp2 == nb_sm_before_f0.norm_bp2)
151 151 {
152 152 nb_norm_bp2 = 0;
153 153 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
154 154 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
155 155 {
156 156 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F0;
157 157 }
158 158 }
159 159
160 160 if (nb_norm_asm == nb_sm_before_f0.norm_asm)
161 161 {
162 162 nb_norm_asm = 0;
163 163 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
164 164 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
165 165 {
166 166 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F0;
167 167 }
168 168 }
169 169
170 170 //*************************
171 171 // send the message to MATR
172 172 if (msgForMATR.event != 0x00)
173 173 {
174 174 status = rtems_message_queue_send( queue_id_prc0, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC0);
175 175 }
176 176
177 177 if (status != RTEMS_SUCCESSFUL) {
178 178 printf("in AVF0 *** Error sending message to MATR, code %d\n", status);
179 179 }
180 180 }
181 181 }
182 182
183 183 rtems_task prc0_task( rtems_task_argument lfrRequestedMode )
184 184 {
185 185 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
186 186 size_t size; // size of the incoming TC packet
187 187 asm_msg *incomingMsg;
188 188 //
189 189 unsigned char sid;
190 190 rtems_status_code status;
191 191 rtems_id queue_id;
192 192 rtems_id queue_id_q_p0;
193 193 bp_packet_with_spare packet_norm_bp1;
194 194 bp_packet packet_norm_bp2;
195 195 bp_packet packet_sbm_bp1;
196 196 bp_packet packet_sbm_bp2;
197 197 ring_node *current_ring_node_to_send_asm_f0;
198 198
199 199 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
200 200 init_ring( ring_to_send_asm_f0, NB_RING_NODES_ASM_F0, (volatile int*) buffer_asm_f0, TOTAL_SIZE_SM );
201 201 current_ring_node_to_send_asm_f0 = ring_to_send_asm_f0;
202 202
203 203 //*************
204 204 // NORM headers
205 205 BP_init_header_with_spare( &packet_norm_bp1,
206 206 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F0,
207 207 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0, NB_BINS_COMPRESSED_SM_F0 );
208 208 BP_init_header( &packet_norm_bp2,
209 209 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F0,
210 210 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0, NB_BINS_COMPRESSED_SM_F0);
211 211
212 212 //****************************
213 213 // BURST SBM1 and SBM2 headers
214 214 if ( lfrRequestedMode == LFR_MODE_BURST )
215 215 {
216 216 BP_init_header( &packet_sbm_bp1,
217 217 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F0,
218 218 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
219 219 BP_init_header( &packet_sbm_bp2,
220 220 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F0,
221 221 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
222 222 }
223 223 else if ( lfrRequestedMode == LFR_MODE_SBM1 )
224 224 {
225 225 BP_init_header( &packet_sbm_bp1,
226 226 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP1_F0,
227 227 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
228 228 BP_init_header( &packet_sbm_bp2,
229 229 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP2_F0,
230 230 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
231 231 }
232 232 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
233 233 {
234 234 BP_init_header( &packet_sbm_bp1,
235 235 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F0,
236 236 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
237 237 BP_init_header( &packet_sbm_bp2,
238 238 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F0,
239 239 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
240 240 }
241 241 else
242 242 {
243 243 PRINTF1("in PRC0 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
244 244 }
245 245
246 246 status = get_message_queue_id_send( &queue_id );
247 247 if (status != RTEMS_SUCCESSFUL)
248 248 {
249 249 PRINTF1("in PRC0 *** ERR get_message_queue_id_send %d\n", status)
250 250 }
251 251 status = get_message_queue_id_prc0( &queue_id_q_p0);
252 252 if (status != RTEMS_SUCCESSFUL)
253 253 {
254 254 PRINTF1("in PRC0 *** ERR get_message_queue_id_prc0 %d\n", status)
255 255 }
256 256
257 257 BOOT_PRINTF1("in PRC0 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
258 258
259 259 while(1){
260 260 status = rtems_message_queue_receive( queue_id_q_p0, incomingData, &size, //************************************
261 261 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
262 262
263 263 incomingMsg = (asm_msg*) incomingData;
264 264
265 265 ASM_patch( incomingMsg->norm->matrix, asm_f0_patched_norm );
266 266 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f0_patched_burst_sbm );
267 267
268 268 //****************
269 269 //****************
270 270 // BURST SBM1 SBM2
271 271 //****************
272 272 //****************
273 273 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F0 ) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F0 ) )
274 274 {
275 275 sid = getSID( incomingMsg->event );
276 276 // 1) compress the matrix for Basic Parameters calculation
277 277 ASM_compress_reorganize_and_divide( asm_f0_patched_burst_sbm, compressed_sm_sbm_f0,
278 278 nb_sm_before_f0.burst_sbm_bp1,
279 279 NB_BINS_COMPRESSED_SM_SBM_F0, NB_BINS_TO_AVERAGE_ASM_SBM_F0,
280 280 ASM_F0_INDICE_START);
281 281 // 2) compute the BP1 set
282 282 BP1_set( compressed_sm_sbm_f0, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp1.data );
283 283 // 3) send the BP1 set
284 284 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
285 285 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
286 286 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
287 287 BP_send( (char *) &packet_sbm_bp1, queue_id,
288 288 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0 + PACKET_LENGTH_DELTA,
289 289 sid);
290 290 // 4) compute the BP2 set if needed
291 291 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F0) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F0) )
292 292 {
293 293 // 1) compute the BP2 set
294 294 BP2_set( compressed_sm_sbm_f0, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp2.data );
295 295 // 2) send the BP2 set
296 296 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
297 297 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
298 298 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
299 299 BP_send( (char *) &packet_sbm_bp2, queue_id,
300 300 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0 + PACKET_LENGTH_DELTA,
301 301 sid);
302 302 }
303 303 }
304 304
305 305 //*****
306 306 //*****
307 307 // NORM
308 308 //*****
309 309 //*****
310 310 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F0)
311 311 {
312 312 // 1) compress the matrix for Basic Parameters calculation
313 313 ASM_compress_reorganize_and_divide( asm_f0_patched_norm, compressed_sm_norm_f0,
314 314 nb_sm_before_f0.norm_bp1,
315 315 NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0,
316 316 ASM_F0_INDICE_START );
317 317 // 2) compute the BP1 set
318 318 BP1_set( compressed_sm_norm_f0, k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp1.data );
319 319 // 3) send the BP1 set
320 320 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
321 321 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
322 322 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
323 323 BP_send( (char *) &packet_norm_bp1, queue_id,
324 324 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0 + PACKET_LENGTH_DELTA,
325 325 SID_NORM_BP1_F0 );
326 326 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F0)
327 327 {
328 328 // 1) compute the BP2 set using the same ASM as the one used for BP1
329 329 BP2_set( compressed_sm_norm_f0, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp2.data );
330 330 // 2) send the BP2 set
331 331 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
332 332 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
333 333 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
334 334 BP_send( (char *) &packet_norm_bp2, queue_id,
335 335 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0 + PACKET_LENGTH_DELTA,
336 336 SID_NORM_BP2_F0);
337 337 }
338 338 }
339 339
340 340 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F0)
341 341 {
342 342 // 1) reorganize the ASM and divide
343 343 ASM_reorganize_and_divide( asm_f0_patched_norm,
344 asm_f0_reorganized,
344 (float*) current_ring_node_to_send_asm_f0->buffer_address,
345 345 nb_sm_before_f0.norm_bp1 );
346 // 2) convert the float array in a char array
347 ASM_convert( asm_f0_reorganized, (char*) current_ring_node_to_send_asm_f0->buffer_address );
348 346 current_ring_node_to_send_asm_f0->coarseTime = incomingMsg->coarseTimeNORM;
349 347 current_ring_node_to_send_asm_f0->fineTime = incomingMsg->fineTimeNORM;
350 348 current_ring_node_to_send_asm_f0->sid = SID_NORM_ASM_F0;
351 349
352 350 // 3) send the spectral matrix packets
353 351 status = rtems_message_queue_send( queue_id, &current_ring_node_to_send_asm_f0, sizeof( ring_node* ) );
354 352 // change asm ring node
355 353 current_ring_node_to_send_asm_f0 = current_ring_node_to_send_asm_f0->next;
356 354 }
357 355 }
358 356 }
359 357
360 358 //**********
361 359 // FUNCTIONS
362 360
363 361 void reset_nb_sm_f0( unsigned char lfrMode )
364 362 {
365 363 nb_sm_before_f0.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 96;
366 364 nb_sm_before_f0.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 96;
367 365 nb_sm_before_f0.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1]) * 96;
368 366 nb_sm_before_f0.sbm1_bp1 = parameter_dump_packet.sy_lfr_s1_bp_p0 * 24; // 0.25 s per digit
369 367 nb_sm_before_f0.sbm1_bp2 = parameter_dump_packet.sy_lfr_s1_bp_p1 * 96;
370 368 nb_sm_before_f0.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 96;
371 369 nb_sm_before_f0.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 96;
372 370 nb_sm_before_f0.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 96;
373 371 nb_sm_before_f0.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 96;
374 372
375 373 if (lfrMode == LFR_MODE_SBM1)
376 374 {
377 375 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm1_bp1;
378 376 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm1_bp2;
379 377 }
380 378 else if (lfrMode == LFR_MODE_SBM2)
381 379 {
382 380 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm2_bp1;
383 381 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm2_bp2;
384 382 }
385 383 else if (lfrMode == LFR_MODE_BURST)
386 384 {
387 385 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
388 386 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
389 387 }
390 388 else
391 389 {
392 390 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
393 391 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
394 392 }
395 393 }
396 394
397 395 void init_k_coefficients_f0( void )
398 396 {
399 397 init_k_coefficients( k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0 );
400 398 init_k_coefficients( k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_SBM_F0);
401 399 }
@@ -1,389 +1,387
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf1_prc1.h"
11 11
12 12 nb_sm_before_bp_asm_f1 nb_sm_before_f1;
13 13
14 14 extern ring_node sm_ring_f1[ ];
15 15
16 16 //***
17 17 // F1
18 18 ring_node_asm asm_ring_norm_f1 [ NB_RING_NODES_ASM_NORM_F1 ];
19 19 ring_node_asm asm_ring_burst_sbm_f1 [ NB_RING_NODES_ASM_BURST_SBM_F1 ];
20 20
21 21 ring_node ring_to_send_asm_f1 [ NB_RING_NODES_ASM_F1 ];
22 22 int buffer_asm_f1 [ NB_RING_NODES_ASM_F1 * TOTAL_SIZE_SM ];
23 23
24 24 float asm_f1_patched_norm [ TOTAL_SIZE_SM ];
25 25 float asm_f1_patched_burst_sbm [ TOTAL_SIZE_SM ];
26 26 float asm_f1_reorganized [ TOTAL_SIZE_SM ];
27 27
28 28 char asm_f1_char [ TOTAL_SIZE_SM * 2 ];
29 29 float compressed_sm_norm_f1[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F1];
30 30 float compressed_sm_sbm_f1 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 ];
31 31
32 32 float k_coeff_intercalib_f1_norm[ NB_BINS_COMPRESSED_SM_F1 * NB_K_COEFF_PER_BIN ]; // 13 * 32 = 416
33 33 float k_coeff_intercalib_f1_sbm[ NB_BINS_COMPRESSED_SM_SBM_F1 * NB_K_COEFF_PER_BIN ]; // 26 * 32 = 832
34 34
35 35 //************
36 36 // RTEMS TASKS
37 37
38 38 rtems_task avf1_task( rtems_task_argument lfrRequestedMode )
39 39 {
40 40 int i;
41 41
42 42 rtems_event_set event_out;
43 43 rtems_status_code status;
44 44 rtems_id queue_id_prc1;
45 45 asm_msg msgForMATR;
46 46 ring_node *nodeForAveraging;
47 47 ring_node *ring_node_tab[NB_SM_BEFORE_AVF0];
48 48 ring_node_asm *current_ring_node_asm_burst_sbm_f1;
49 49 ring_node_asm *current_ring_node_asm_norm_f1;
50 50
51 51 unsigned int nb_norm_bp1;
52 52 unsigned int nb_norm_bp2;
53 53 unsigned int nb_norm_asm;
54 54 unsigned int nb_sbm_bp1;
55 55 unsigned int nb_sbm_bp2;
56 56
57 57 nb_norm_bp1 = 0;
58 58 nb_norm_bp2 = 0;
59 59 nb_norm_asm = 0;
60 60 nb_sbm_bp1 = 0;
61 61 nb_sbm_bp2 = 0;
62 62
63 63 reset_nb_sm_f1( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
64 64 ASM_generic_init_ring( asm_ring_norm_f1, NB_RING_NODES_ASM_NORM_F1 );
65 65 ASM_generic_init_ring( asm_ring_burst_sbm_f1, NB_RING_NODES_ASM_BURST_SBM_F1 );
66 66 current_ring_node_asm_norm_f1 = asm_ring_norm_f1;
67 67 current_ring_node_asm_burst_sbm_f1 = asm_ring_burst_sbm_f1;
68 68
69 69 BOOT_PRINTF1("in AVF1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
70 70
71 71 status = get_message_queue_id_prc1( &queue_id_prc1 );
72 72 if (status != RTEMS_SUCCESSFUL)
73 73 {
74 74 PRINTF1("in AVF1 *** ERR get_message_queue_id_prc1 %d\n", status)
75 75 }
76 76
77 77 while(1){
78 78 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
79 79
80 80 //****************************************
81 81 // initialize the mesage for the MATR task
82 82 msgForMATR.norm = current_ring_node_asm_norm_f1;
83 83 msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f1;
84 84 msgForMATR.event = 0x00; // this composite event will be sent to the PRC1 task
85 85 //
86 86 //****************************************
87 87
88 88 nodeForAveraging = getRingNodeForAveraging( 1 );
89 89
90 90 ring_node_tab[NB_SM_BEFORE_AVF1-1] = nodeForAveraging;
91 91 for ( i = 2; i < (NB_SM_BEFORE_AVF1+1); i++ )
92 92 {
93 93 nodeForAveraging = nodeForAveraging->previous;
94 94 ring_node_tab[NB_SM_BEFORE_AVF1-i] = nodeForAveraging;
95 95 }
96 96
97 97 // compute the average and store it in the averaged_sm_f1 buffer
98 98 SM_average( current_ring_node_asm_norm_f1->matrix,
99 99 current_ring_node_asm_burst_sbm_f1->matrix,
100 100 ring_node_tab,
101 101 nb_norm_bp1, nb_sbm_bp1,
102 102 &msgForMATR );
103 103
104 104 // update nb_average
105 105 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF1;
106 106 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF1;
107 107 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF1;
108 108 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF1;
109 109 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF1;
110 110
111 111 if (nb_sbm_bp1 == nb_sm_before_f1.burst_sbm_bp1)
112 112 {
113 113 nb_sbm_bp1 = 0;
114 114 // set another ring for the ASM storage
115 115 current_ring_node_asm_burst_sbm_f1 = current_ring_node_asm_burst_sbm_f1->next;
116 116 if ( lfrCurrentMode == LFR_MODE_BURST )
117 117 {
118 118 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP1_F1;
119 119 }
120 120 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
121 121 {
122 122 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP1_F1;
123 123 }
124 124 }
125 125
126 126 if (nb_sbm_bp2 == nb_sm_before_f1.burst_sbm_bp2)
127 127 {
128 128 nb_sbm_bp2 = 0;
129 129 if ( lfrCurrentMode == LFR_MODE_BURST )
130 130 {
131 131 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP2_F1;
132 132 }
133 133 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
134 134 {
135 135 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP2_F1;
136 136 }
137 137 }
138 138
139 139 if (nb_norm_bp1 == nb_sm_before_f1.norm_bp1)
140 140 {
141 141 nb_norm_bp1 = 0;
142 142 // set another ring for the ASM storage
143 143 current_ring_node_asm_norm_f1 = current_ring_node_asm_norm_f1->next;
144 144 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
145 145 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
146 146 {
147 147 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F1;
148 148 }
149 149 }
150 150
151 151 if (nb_norm_bp2 == nb_sm_before_f1.norm_bp2)
152 152 {
153 153 nb_norm_bp2 = 0;
154 154 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
155 155 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
156 156 {
157 157 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F1;
158 158 }
159 159 }
160 160
161 161 if (nb_norm_asm == nb_sm_before_f1.norm_asm)
162 162 {
163 163 nb_norm_asm = 0;
164 164 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
165 165 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
166 166 {
167 167 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F1;
168 168 }
169 169 }
170 170
171 171 //*************************
172 172 // send the message to MATR
173 173 if (msgForMATR.event != 0x00)
174 174 {
175 175 status = rtems_message_queue_send( queue_id_prc1, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC1);
176 176 }
177 177
178 178 if (status != RTEMS_SUCCESSFUL) {
179 179 printf("in AVF1 *** Error sending message to PRC1, code %d\n", status);
180 180 }
181 181 }
182 182 }
183 183
184 184 rtems_task prc1_task( rtems_task_argument lfrRequestedMode )
185 185 {
186 186 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
187 187 size_t size; // size of the incoming TC packet
188 188 asm_msg *incomingMsg;
189 189 //
190 190 unsigned char sid;
191 191 rtems_status_code status;
192 192 rtems_id queue_id_send;
193 193 rtems_id queue_id_q_p1;
194 194 bp_packet_with_spare packet_norm_bp1;
195 195 bp_packet packet_norm_bp2;
196 196 bp_packet packet_sbm_bp1;
197 197 bp_packet packet_sbm_bp2;
198 198 ring_node *current_ring_node_to_send_asm_f1;
199 199
200 200 unsigned long long int localTime;
201 201
202 202 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
203 203 init_ring( ring_to_send_asm_f1, NB_RING_NODES_ASM_F1, (volatile int*) buffer_asm_f1, TOTAL_SIZE_SM );
204 204 current_ring_node_to_send_asm_f1 = ring_to_send_asm_f1;
205 205
206 206 //*************
207 207 // NORM headers
208 208 BP_init_header_with_spare( &packet_norm_bp1,
209 209 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F1,
210 210 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1, NB_BINS_COMPRESSED_SM_F1 );
211 211 BP_init_header( &packet_norm_bp2,
212 212 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F1,
213 213 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1, NB_BINS_COMPRESSED_SM_F1);
214 214
215 215 //***********************
216 216 // BURST and SBM2 headers
217 217 if ( lfrRequestedMode == LFR_MODE_BURST )
218 218 {
219 219 BP_init_header( &packet_sbm_bp1,
220 220 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F1,
221 221 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
222 222 BP_init_header( &packet_sbm_bp2,
223 223 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F1,
224 224 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
225 225 }
226 226 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
227 227 {
228 228 BP_init_header( &packet_sbm_bp1,
229 229 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F1,
230 230 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
231 231 BP_init_header( &packet_sbm_bp2,
232 232 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F1,
233 233 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
234 234 }
235 235 else
236 236 {
237 237 PRINTF1("in PRC1 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
238 238 }
239 239
240 240 status = get_message_queue_id_send( &queue_id_send );
241 241 if (status != RTEMS_SUCCESSFUL)
242 242 {
243 243 PRINTF1("in PRC1 *** ERR get_message_queue_id_send %d\n", status)
244 244 }
245 245 status = get_message_queue_id_prc1( &queue_id_q_p1);
246 246 if (status != RTEMS_SUCCESSFUL)
247 247 {
248 248 PRINTF1("in PRC1 *** ERR get_message_queue_id_prc1 %d\n", status)
249 249 }
250 250
251 251 BOOT_PRINTF1("in PRC1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
252 252
253 253 while(1){
254 254 status = rtems_message_queue_receive( queue_id_q_p1, incomingData, &size, //************************************
255 255 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
256 256
257 257 incomingMsg = (asm_msg*) incomingData;
258 258
259 259 ASM_patch( incomingMsg->norm->matrix, asm_f1_patched_norm );
260 260 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f1_patched_burst_sbm );
261 261
262 262 localTime = getTimeAsUnsignedLongLongInt( );
263 263 //***********
264 264 //***********
265 265 // BURST SBM2
266 266 //***********
267 267 //***********
268 268 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F1) )
269 269 {
270 270 sid = getSID( incomingMsg->event );
271 271 // 1) compress the matrix for Basic Parameters calculation
272 272 ASM_compress_reorganize_and_divide( asm_f1_patched_burst_sbm, compressed_sm_sbm_f1,
273 273 nb_sm_before_f1.burst_sbm_bp1,
274 274 NB_BINS_COMPRESSED_SM_SBM_F1, NB_BINS_TO_AVERAGE_ASM_SBM_F1,
275 275 ASM_F1_INDICE_START);
276 276 // 2) compute the BP1 set
277 277 BP1_set( compressed_sm_sbm_f1, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp1.data );
278 278 // 3) send the BP1 set
279 279 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
280 280 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
281 281 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
282 282 BP_send( (char *) &packet_sbm_bp1, queue_id_send,
283 283 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 + PACKET_LENGTH_DELTA,
284 284 sid );
285 285 // 4) compute the BP2 set if needed
286 286 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F1) )
287 287 {
288 288 // 1) compute the BP2 set
289 289 BP2_set( compressed_sm_sbm_f1, NB_BINS_COMPRESSED_SM_SBM_F1, packet_norm_bp2.data );
290 290 // 2) send the BP2 set
291 291 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
292 292 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
293 293 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
294 294 BP_send( (char *) &packet_sbm_bp2, queue_id_send,
295 295 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1 + PACKET_LENGTH_DELTA,
296 296 sid );
297 297 }
298 298 }
299 299
300 300 //*****
301 301 //*****
302 302 // NORM
303 303 //*****
304 304 //*****
305 305 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F1)
306 306 {
307 307 // 1) compress the matrix for Basic Parameters calculation
308 308 ASM_compress_reorganize_and_divide( asm_f1_patched_norm, compressed_sm_norm_f1,
309 309 nb_sm_before_f1.norm_bp1,
310 310 NB_BINS_COMPRESSED_SM_F1, NB_BINS_TO_AVERAGE_ASM_F1,
311 311 ASM_F1_INDICE_START );
312 312 // 2) compute the BP1 set
313 313 BP1_set( compressed_sm_norm_f1, k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp1.data );
314 314 // 3) send the BP1 set
315 315 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
316 316 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
317 317 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
318 318 BP_send( (char *) &packet_norm_bp1, queue_id_send,
319 319 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 + PACKET_LENGTH_DELTA,
320 320 SID_NORM_BP1_F1 );
321 321 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F1)
322 322 {
323 323 // 1) compute the BP2 set
324 324 BP2_set( compressed_sm_norm_f1, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp2.data );
325 325 // 2) send the BP2 set
326 326 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
327 327 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
328 328 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
329 329 BP_send( (char *) &packet_norm_bp2, queue_id_send,
330 330 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 + PACKET_LENGTH_DELTA,
331 331 SID_NORM_BP2_F1 );
332 332 }
333 333 }
334 334
335 335 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F1)
336 336 {
337 337 // 1) reorganize the ASM and divide
338 338 ASM_reorganize_and_divide( asm_f1_patched_norm,
339 asm_f1_reorganized,
339 (float*) current_ring_node_to_send_asm_f1->buffer_address,
340 340 nb_sm_before_f1.norm_bp1 );
341 // 2) convert the float array in a char array
342 ASM_convert( asm_f1_reorganized, (char*) current_ring_node_to_send_asm_f1->buffer_address );
343 341 current_ring_node_to_send_asm_f1->coarseTime = incomingMsg->coarseTimeNORM;
344 342 current_ring_node_to_send_asm_f1->fineTime = incomingMsg->fineTimeNORM;
345 343 current_ring_node_to_send_asm_f1->sid = SID_NORM_ASM_F1;
346 344 // 3) send the spectral matrix packets
347 345 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f1, sizeof( ring_node* ) );
348 346 // change asm ring node
349 347 current_ring_node_to_send_asm_f1 = current_ring_node_to_send_asm_f1->next;
350 348 }
351 349
352 350 }
353 351 }
354 352
355 353 //**********
356 354 // FUNCTIONS
357 355
358 356 void reset_nb_sm_f1( unsigned char lfrMode )
359 357 {
360 358 nb_sm_before_f1.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 16;
361 359 nb_sm_before_f1.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 16;
362 360 nb_sm_before_f1.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1]) * 16;
363 361 nb_sm_before_f1.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 16;
364 362 nb_sm_before_f1.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 16;
365 363 nb_sm_before_f1.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 16;
366 364 nb_sm_before_f1.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 16;
367 365
368 366 if (lfrMode == LFR_MODE_SBM2)
369 367 {
370 368 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.sbm2_bp1;
371 369 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.sbm2_bp2;
372 370 }
373 371 else if (lfrMode == LFR_MODE_BURST)
374 372 {
375 373 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
376 374 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
377 375 }
378 376 else
379 377 {
380 378 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
381 379 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
382 380 }
383 381 }
384 382
385 383 void init_k_coefficients_f1( void )
386 384 {
387 385 init_k_coefficients( k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1 );
388 386 init_k_coefficients( k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1);
389 387 }
@@ -1,291 +1,289
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf2_prc2.h"
11 11
12 12 nb_sm_before_bp_asm_f2 nb_sm_before_f2;
13 13
14 14 extern ring_node sm_ring_f2[ ];
15 15
16 16 //***
17 17 // F2
18 18 ring_node_asm asm_ring_norm_f2 [ NB_RING_NODES_ASM_NORM_F2 ];
19 19
20 20 ring_node ring_to_send_asm_f2 [ NB_RING_NODES_ASM_F2 ];
21 21 int buffer_asm_f2 [ NB_RING_NODES_ASM_F2 * TOTAL_SIZE_SM ];
22 22
23 23 float asm_f2_patched_norm [ TOTAL_SIZE_SM ];
24 24 float asm_f2_reorganized [ TOTAL_SIZE_SM ];
25 25
26 26 char asm_f2_char [ TOTAL_SIZE_SM * 2 ];
27 27 float compressed_sm_norm_f2[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F2];
28 28
29 29 float k_coeff_intercalib_f2[ NB_BINS_COMPRESSED_SM_F2 * NB_K_COEFF_PER_BIN ]; // 12 * 32 = 384
30 30
31 31 //************
32 32 // RTEMS TASKS
33 33
34 34 //***
35 35 // F2
36 36 rtems_task avf2_task( rtems_task_argument argument )
37 37 {
38 38 rtems_event_set event_out;
39 39 rtems_status_code status;
40 40 rtems_id queue_id_prc2;
41 41 asm_msg msgForMATR;
42 42 ring_node *nodeForAveraging;
43 43 ring_node_asm *current_ring_node_asm_norm_f2;
44 44
45 45 unsigned int nb_norm_bp1;
46 46 unsigned int nb_norm_bp2;
47 47 unsigned int nb_norm_asm;
48 48
49 49 nb_norm_bp1 = 0;
50 50 nb_norm_bp2 = 0;
51 51 nb_norm_asm = 0;
52 52
53 53 reset_nb_sm_f2( ); // reset the sm counters that drive the BP and ASM computations / transmissions
54 54 ASM_generic_init_ring( asm_ring_norm_f2, NB_RING_NODES_ASM_NORM_F2 );
55 55 current_ring_node_asm_norm_f2 = asm_ring_norm_f2;
56 56
57 57 BOOT_PRINTF("in AVF2 ***\n")
58 58
59 59 status = get_message_queue_id_prc2( &queue_id_prc2 );
60 60 if (status != RTEMS_SUCCESSFUL)
61 61 {
62 62 PRINTF1("in AVF2 *** ERR get_message_queue_id_prc2 %d\n", status)
63 63 }
64 64
65 65 while(1){
66 66 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
67 67
68 68 //****************************************
69 69 // initialize the mesage for the MATR task
70 70 msgForMATR.norm = current_ring_node_asm_norm_f2;
71 71 msgForMATR.burst_sbm = NULL;
72 72 msgForMATR.event = 0x00; // this composite event will be sent to the PRC2 task
73 73 //
74 74 //****************************************
75 75
76 76 nodeForAveraging = getRingNodeForAveraging( 2 );
77 77
78 78 // printf(" **0** %x . %x", sm_ring_f2[0].coarseTime, sm_ring_f2[0].fineTime);
79 79 // printf(" **1** %x . %x", sm_ring_f2[1].coarseTime, sm_ring_f2[1].fineTime);
80 80 // printf(" **2** %x . %x", sm_ring_f2[2].coarseTime, sm_ring_f2[2].fineTime);
81 81 // printf(" **3** %x . %x", sm_ring_f2[3].coarseTime, sm_ring_f2[3].fineTime);
82 82 // printf(" **4** %x . %x", sm_ring_f2[4].coarseTime, sm_ring_f2[4].fineTime);
83 83 // printf(" **5** %x . %x", sm_ring_f2[5].coarseTime, sm_ring_f2[5].fineTime);
84 84 // printf(" **6** %x . %x", sm_ring_f2[6].coarseTime, sm_ring_f2[6].fineTime);
85 85 // printf(" **7** %x . %x", sm_ring_f2[7].coarseTime, sm_ring_f2[7].fineTime);
86 86 // printf(" **8** %x . %x", sm_ring_f2[8].coarseTime, sm_ring_f2[8].fineTime);
87 87 // printf(" **9** %x . %x", sm_ring_f2[9].coarseTime, sm_ring_f2[9].fineTime);
88 88 // printf(" **10** %x . %x\n", sm_ring_f2[10].coarseTime, sm_ring_f2[10].fineTime);
89 89
90 90 // compute the average and store it in the averaged_sm_f2 buffer
91 91 SM_average_f2( current_ring_node_asm_norm_f2->matrix,
92 92 nodeForAveraging,
93 93 nb_norm_bp1,
94 94 &msgForMATR );
95 95
96 96 // update nb_average
97 97 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF2;
98 98 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF2;
99 99 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF2;
100 100
101 101 if (nb_norm_bp1 == nb_sm_before_f2.norm_bp1)
102 102 {
103 103 nb_norm_bp1 = 0;
104 104 // set another ring for the ASM storage
105 105 current_ring_node_asm_norm_f2 = current_ring_node_asm_norm_f2->next;
106 106 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
107 107 || (lfrCurrentMode == LFR_MODE_SBM2) )
108 108 {
109 109 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F2;
110 110 }
111 111 }
112 112
113 113 if (nb_norm_bp2 == nb_sm_before_f2.norm_bp2)
114 114 {
115 115 nb_norm_bp2 = 0;
116 116 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
117 117 || (lfrCurrentMode == LFR_MODE_SBM2) )
118 118 {
119 119 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F2;
120 120 }
121 121 }
122 122
123 123 if (nb_norm_asm == nb_sm_before_f2.norm_asm)
124 124 {
125 125 nb_norm_asm = 0;
126 126 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
127 127 || (lfrCurrentMode == LFR_MODE_SBM2) )
128 128 {
129 129 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F2;
130 130 }
131 131 }
132 132
133 133 //*************************
134 134 // send the message to MATR
135 135 if (msgForMATR.event != 0x00)
136 136 {
137 137 status = rtems_message_queue_send( queue_id_prc2, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC2);
138 138 }
139 139
140 140 if (status != RTEMS_SUCCESSFUL) {
141 141 printf("in AVF2 *** Error sending message to MATR, code %d\n", status);
142 142 }
143 143 }
144 144 }
145 145
146 146 rtems_task prc2_task( rtems_task_argument argument )
147 147 {
148 148 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
149 149 size_t size; // size of the incoming TC packet
150 150 asm_msg *incomingMsg;
151 151 //
152 152 rtems_status_code status;
153 153 rtems_id queue_id_send;
154 154 rtems_id queue_id_q_p2;
155 155 bp_packet packet_norm_bp1;
156 156 bp_packet packet_norm_bp2;
157 157 ring_node *current_ring_node_to_send_asm_f2;
158 158
159 159 unsigned long long int localTime;
160 160
161 161 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
162 162 init_ring( ring_to_send_asm_f2, NB_RING_NODES_ASM_F2, (volatile int*) buffer_asm_f2, TOTAL_SIZE_SM );
163 163 current_ring_node_to_send_asm_f2 = ring_to_send_asm_f2;
164 164
165 165 //*************
166 166 // NORM headers
167 167 BP_init_header( &packet_norm_bp1,
168 168 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F2,
169 169 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2, NB_BINS_COMPRESSED_SM_F2 );
170 170 BP_init_header( &packet_norm_bp2,
171 171 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F2,
172 172 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2, NB_BINS_COMPRESSED_SM_F2 );
173 173
174 174 status = get_message_queue_id_send( &queue_id_send );
175 175 if (status != RTEMS_SUCCESSFUL)
176 176 {
177 177 PRINTF1("in PRC2 *** ERR get_message_queue_id_send %d\n", status)
178 178 }
179 179 status = get_message_queue_id_prc2( &queue_id_q_p2);
180 180 if (status != RTEMS_SUCCESSFUL)
181 181 {
182 182 PRINTF1("in PRC2 *** ERR get_message_queue_id_prc2 %d\n", status)
183 183 }
184 184
185 185 BOOT_PRINTF("in PRC2 ***\n")
186 186
187 187 while(1){
188 188 status = rtems_message_queue_receive( queue_id_q_p2, incomingData, &size, //************************************
189 189 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF2
190 190
191 191 incomingMsg = (asm_msg*) incomingData;
192 192
193 193 ASM_patch( incomingMsg->norm->matrix, asm_f2_patched_norm );
194 194
195 195 localTime = getTimeAsUnsignedLongLongInt( );
196 196
197 197 //*****
198 198 //*****
199 199 // NORM
200 200 //*****
201 201 //*****
202 202 // 1) compress the matrix for Basic Parameters calculation
203 203 ASM_compress_reorganize_and_divide( asm_f2_patched_norm, compressed_sm_norm_f2,
204 204 nb_sm_before_f2.norm_bp1,
205 205 NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2,
206 206 ASM_F2_INDICE_START );
207 207 // BP1_F2
208 208 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F2)
209 209 {
210 210 // 1) compute the BP1 set
211 211 BP1_set( compressed_sm_norm_f2, k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp1.data );
212 212 // 2) send the BP1 set
213 213 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
214 214 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
215 215 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
216 216 BP_send( (char *) &packet_norm_bp1, queue_id_send,
217 217 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2 + PACKET_LENGTH_DELTA,
218 218 SID_NORM_BP1_F2 );
219 219 }
220 220 // BP2_F2
221 221 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F2)
222 222 {
223 223 // 1) compute the BP2 set
224 224 BP2_set( compressed_sm_norm_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp2.data );
225 225 // 2) send the BP2 set
226 226 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
227 227 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
228 228 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
229 229 BP_send( (char *) &packet_norm_bp2, queue_id_send,
230 230 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2 + PACKET_LENGTH_DELTA,
231 231 SID_NORM_BP2_F2 );
232 232 }
233 233
234 234 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F2)
235 235 {
236 236 // 1) reorganize the ASM and divide
237 237 ASM_reorganize_and_divide( asm_f2_patched_norm,
238 asm_f2_reorganized,
238 (float*) current_ring_node_to_send_asm_f2->buffer_address,
239 239 nb_sm_before_f2.norm_bp1 );
240 // 2) convert the float array in a char array
241 ASM_convert( asm_f2_reorganized, (char*) current_ring_node_to_send_asm_f2->buffer_address );
242 240 current_ring_node_to_send_asm_f2->coarseTime = incomingMsg->coarseTimeNORM;
243 241 current_ring_node_to_send_asm_f2->fineTime = incomingMsg->fineTimeNORM;
244 242 current_ring_node_to_send_asm_f2->sid = SID_NORM_ASM_F2;
245 243 // 3) send the spectral matrix packets
246 244 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f2, sizeof( ring_node* ) );
247 245 // change asm ring node
248 246 current_ring_node_to_send_asm_f2 = current_ring_node_to_send_asm_f2->next;
249 247 }
250 248
251 249 }
252 250 }
253 251
254 252 //**********
255 253 // FUNCTIONS
256 254
257 255 void reset_nb_sm_f2( void )
258 256 {
259 257 nb_sm_before_f2.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0;
260 258 nb_sm_before_f2.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1;
261 259 nb_sm_before_f2.norm_asm = parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1];
262 260 }
263 261
264 262 void SM_average_f2( float *averaged_spec_mat_f2,
265 263 ring_node *ring_node,
266 264 unsigned int nbAverageNormF2,
267 265 asm_msg *msgForMATR )
268 266 {
269 267 float sum;
270 268 unsigned int i;
271 269
272 270 for(i=0; i<TOTAL_SIZE_SM; i++)
273 271 {
274 272 sum = ( (int *) (ring_node->buffer_address) ) [ i ];
275 273 if ( (nbAverageNormF2 == 0) )
276 274 {
277 275 averaged_spec_mat_f2[ i ] = sum;
278 276 msgForMATR->coarseTimeNORM = ring_node->coarseTime;
279 277 msgForMATR->fineTimeNORM = ring_node->fineTime;
280 278 }
281 279 else
282 280 {
283 281 averaged_spec_mat_f2[ i ] = ( averaged_spec_mat_f2[ i ] + sum );
284 282 }
285 283 }
286 284 }
287 285
288 286 void init_k_coefficients_f2( void )
289 287 {
290 288 init_k_coefficients( k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2);
291 289 }
General Comments 0
You need to be logged in to leave comments. Login now