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

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

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