##// END OF EJS Templates
HG_REPOSITORIES » LPP » INSTRUMENTATION » USERS » JEANDET » LFR_FSW
RSS

Fork of HG_REPOSITORIES/LPP/INSTRUMENTATION/SOLO_LFR/DEV_PLE

Clone from https://hephaistos.lpp.polytechnique.fr/rhodecode/HG_REPOSITORIES/LPP/INSTRUMENTATION/SOLO_LFR/DEV_PLE

TC_LFR_LOAD_KCOEFFICIENTS...
paul -
r194:72cdb2a15242 R3
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,2 +1,2
1 1 a586fe639ac179e95bdc150ebdbab0312f31dc30 LFR_basic-parameters
2 ddd0a6fe16cc1861ad679bf646663e070189e037 header/lfr_common_headers
2 5467523e44cd6a627a81b156673a891f4d6b0017 header/lfr_common_headers
Show file before | Show file after | Hide comments
@@ -1,47 +1,48
1 1 #ifndef FSW_SPACEWIRE_H_INCLUDED
2 2 #define FSW_SPACEWIRE_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <grspw.h>
6 6
7 7 #include <fcntl.h> // for O_RDWR
8 8 #include <unistd.h> // for the read call
9 9 #include <sys/ioctl.h> // for the ioctl call
10 10 #include <errno.h>
11 11
12 12 #include "fsw_params.h"
13 13 #include "tc_handler.h"
14 14
15 15 extern spw_stats spacewire_stats;
16 16 extern spw_stats spacewire_stats_backup;
17 17
18 18 // RTEMS TASK
19 19 rtems_task spiq_task( rtems_task_argument argument );
20 20 rtems_task recv_task( rtems_task_argument unused );
21 21 rtems_task send_task( rtems_task_argument argument );
22 22 rtems_task wtdg_task( rtems_task_argument argument );
23 23
24 24 int spacewire_open_link( void );
25 25 int spacewire_start_link( int fd );
26 26 int spacewire_stop_and_start_link( int fd );
27 27 int spacewire_configure_link(int fd );
28 28 int spacewire_reset_link( void );
29 29 void spacewire_set_NP( unsigned char val, unsigned int regAddr ); // No Port force
30 30 void spacewire_set_RE( unsigned char val, unsigned int regAddr ); // RMAP Enable
31 31 void spacewire_compute_stats_offsets( void );
32 32 void spacewire_update_statistics( void );
33 33
34 34 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header );
35 35 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header );
36 36 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header );
37 37 int spw_send_waveform_CWF( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_CWF_t *header );
38 38 int spw_send_waveform_SWF( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_SWF_t *header );
39 39 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_CWF_t *header );
40 40 void spw_send_asm( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_ASM_t *header );
41 void spw_send_k_dump( ring_node *ring_node_to_send );
41 42
42 43 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc );
43 44 rtems_timer_service_routine user_routine( rtems_id timer_id, void *user_data );
44 45
45 46 void (*grspw_timecode_callback) ( void *pDev, void *regs, int minor, unsigned int tc );
46 47
47 48 #endif // FSW_SPACEWIRE_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,54 +1,65
1 1 #ifndef TC_LOAD_DUMP_PARAMETERS_H
2 2 #define TC_LOAD_DUMP_PARAMETERS_H
3 3
4 4 #include <rtems.h>
5 5 #include <stdio.h>
6 6
7 7 #include "fsw_params.h"
8 8 #include "wf_handler.h"
9 9 #include "tm_lfr_tc_exe.h"
10 10 #include "fsw_misc.h"
11 #include "basic_parameters_params.h"
11 12
12 13 #define FLOAT_EQUAL_ZERO 0.001
13 14
14 15 extern unsigned short sequenceCounterParameterDump;
16 extern float k_coeff_intercalib_f0_norm[ ];
17 extern float k_coeff_intercalib_f0_sbm[ ];
18 extern float k_coeff_intercalib_f1_norm[ ];
19 extern float k_coeff_intercalib_f1_sbm[ ];
20 extern float k_coeff_intercalib_f2[ ];
15 21
16 22 int action_load_common_par( ccsdsTelecommandPacket_t *TC );
17 23 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
18 24 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
19 25 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
20 26 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
21 27 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
22 28 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
23 29 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
24 30 int action_dump_par(rtems_id queue_id );
25 31
26 32 // NORMAL
27 33 int check_common_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
28 34 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC );
29 35 int set_sy_lfr_n_swf_p( ccsdsTelecommandPacket_t *TC );
30 36 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC );
31 37 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC );
32 38 int set_sy_lfr_n_bp_p1( ccsdsTelecommandPacket_t *TC );
33 39 int set_sy_lfr_n_cwf_long_f3( ccsdsTelecommandPacket_t *TC );
34 40
35 41 // BURST
36 42 int set_sy_lfr_b_bp_p0( ccsdsTelecommandPacket_t *TC );
37 43 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC );
38 44
39 45 // SBM1
40 46 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC );
41 47 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC );
42 48
43 49 // SBM2
44 50 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC );
45 51 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC );
46 52
47 53 // TC_LFR_UPDATE_INFO
48 54 unsigned int check_update_info_hk_lfr_mode( unsigned char mode );
49 55 unsigned int check_update_info_hk_tds_mode( unsigned char mode );
50 56 unsigned int check_update_info_hk_thr_mode( unsigned char mode );
51 57
58 // KCOEFFICIENTS
59 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC );
60
52 61 void init_parameter_dump( void );
62 void init_kcoefficients_dump( void );
63 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr );
53 64
54 65 #endif // TC_LOAD_DUMP_PARAMETERS_H
Show file before | Show file after | Hide comments
@@ -1,14 +1,14
1 1 # LOAD FSW USING LINK 1
2 2 SpwPlugin0.StarDundeeSelectLinkNumber( 1 )
3 3
4 #dsu3plugin0.openFile("/opt/DEV_PLE/FSW-qt/bin/fsw")
5 dsu3plugin0.openFile("/opt/LFR/LFR-FSW/2.0.2.3/fsw")
4 dsu3plugin0.openFile("/opt/DEV_PLE/FSW-qt/bin/fsw")
5 #dsu3plugin0.openFile("/opt/LFR/LFR-FSW/2.0.2.3/fsw")
6 6 dsu3plugin0.loadFile()
7 7
8 8 dsu3plugin0.run()
9 9
10 10 # START SENDING TIMECODES AT 1 Hz
11 11 SpwPlugin0.StarDundeeStartTimecodes( 1 )
12 12
13 13 # it is possible to change the time code frequency
14 14 #RMAPPlugin0.changeTimecodeFrequency(2)
Show file before | Show file after | Hide comments
@@ -1,29 +1,30
1 1 #!/usr/bin/lppmon -e
2 2
3 3 import time
4 4
5 5 proxy.loadSysDriver("SpwPlugin","SpwPlugin0")
6 6 SpwPlugin0.selectBridge("STAR-Dundee Spw USB Brick")
7 7
8 8 proxy.loadSysDriverToParent("dsu3plugin","SpwPlugin0")
9 9 proxy.loadSysDriverToParent("LFRControlPlugin","SpwPlugin0")
10 10
11 11 availableBrickCount = SpwPlugin0.StarDundeeGetAvailableBrickCount()
12 12 print "availableBrickCount = ", availableBrickCount
13 13
14 14 SpwPlugin0.StarDundeeSelectBrick(1)
15 15 SpwPlugin0.StarDundeeSetBrickAsARouter(1)
16 16 SpwPlugin0.connectBridge()
17 17
18 18 #SpwPlugin0.TCPServerSetIP("127.0.0.1")
19 19 SpwPlugin0.TCPServerConnect()
20 20
21 21 #LFRControlPlugin0.SetSpwServerIP(129,104,27,164)
22 22 LFRControlPlugin0.TCPServerConnect()
23 23
24 24 dsu3plugin0.openFile("/opt/DEV_PLE/FSW-qt/bin/fsw")
25 #dsu3plugin0.openFile("/opt/LFR/LFR-FSW/2.0.2.3/fsw")
25 26 dsu3plugin0.loadFile()
26 27 dsu3plugin0.run()
27 28
28 29 LFRControlPlugin0.TMEchoBridgeOpenPort()
29 30
Show file before | Show file after | Hide comments
@@ -1,813 +1,814
1 1 /** This is the RTEMS initialization module.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * This module contains two very different information:
7 7 * - specific instructions to configure the compilation of the RTEMS executive
8 8 * - functions related to the fligth softwre initialization, especially the INIT RTEMS task
9 9 *
10 10 */
11 11
12 12 //*************************
13 13 // GPL reminder to be added
14 14 //*************************
15 15
16 16 #include <rtems.h>
17 17
18 18 /* configuration information */
19 19
20 20 #define CONFIGURE_INIT
21 21
22 22 #include <bsp.h> /* for device driver prototypes */
23 23
24 24 /* configuration information */
25 25
26 26 #define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
27 27 #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
28 28
29 29 #define CONFIGURE_MAXIMUM_TASKS 20
30 30 #define CONFIGURE_RTEMS_INIT_TASKS_TABLE
31 31 #define CONFIGURE_EXTRA_TASK_STACKS (3 * RTEMS_MINIMUM_STACK_SIZE)
32 32 #define CONFIGURE_LIBIO_MAXIMUM_FILE_DESCRIPTORS 32
33 33 #define CONFIGURE_INIT_TASK_PRIORITY 1 // instead of 100
34 34 #define CONFIGURE_INIT_TASK_MODE (RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT)
35 35 #define CONFIGURE_INIT_TASK_ATTRIBUTES (RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT)
36 36 #define CONFIGURE_MAXIMUM_DRIVERS 16
37 37 #define CONFIGURE_MAXIMUM_PERIODS 5
38 38 #define CONFIGURE_MAXIMUM_TIMERS 5 // STAT (1s), send SWF (0.3s), send CWF3 (1s)
39 39 #define CONFIGURE_MAXIMUM_MESSAGE_QUEUES 5
40 40 #ifdef PRINT_STACK_REPORT
41 41 #define CONFIGURE_STACK_CHECKER_ENABLED
42 42 #endif
43 43
44 44 #include <rtems/confdefs.h>
45 45
46 46 /* If --drvmgr was enabled during the configuration of the RTEMS kernel */
47 47 #ifdef RTEMS_DRVMGR_STARTUP
48 48 #ifdef LEON3
49 49 /* Add Timer and UART Driver */
50 50 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
51 51 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
52 52 #endif
53 53 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
54 54 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
55 55 #endif
56 56 #endif
57 57 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
58 58 #include <drvmgr/drvmgr_confdefs.h>
59 59 #endif
60 60
61 61 #include "fsw_init.h"
62 62 #include "fsw_config.c"
63 63
64 64 void initCache()
65 65 {
66 66 // unsigned int cacheControlRegister;
67 67
68 68 // cacheControlRegister = getCacheControlRegister();
69 69 // printf("(0) cacheControlRegister = %x\n", cacheControlRegister);
70 70
71 71 enableInstructionCache();
72 72 enableDataCache();
73 73 enableInstructionBurstFetch();
74 74
75 75 // cacheControlRegister = getCacheControlRegister();
76 76 // printf("(1) cacheControlRegister = %x\n", cacheControlRegister);
77 77 }
78 78
79 79 rtems_task Init( rtems_task_argument ignored )
80 80 {
81 81 /** This is the RTEMS INIT taks, it the first task launched by the system.
82 82 *
83 83 * @param unused is the starting argument of the RTEMS task
84 84 *
85 85 * The INIT task create and run all other RTEMS tasks.
86 86 *
87 87 */
88 88
89 89 //***********
90 90 // INIT CACHE
91 91
92 92 unsigned char *vhdlVersion;
93 93
94 94 reset_lfr();
95 95
96 96 reset_local_time();
97 97
98 98 rtems_cpu_usage_reset();
99 99
100 100 rtems_status_code status;
101 101 rtems_status_code status_spw;
102 102 rtems_isr_entry old_isr_handler;
103 103
104 104 // UART settings
105 105 send_console_outputs_on_apbuart_port();
106 106 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
107 107 enable_apbuart_transmitter();
108 108
109 109 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
110 110
111 111
112 112 PRINTF("\n\n\n\n\n")
113 113
114 114 initCache();
115 115
116 116 PRINTF("*************************\n")
117 117 PRINTF("** LFR Flight Software **\n")
118 118 PRINTF1("** %d.", SW_VERSION_N1)
119 119 PRINTF1("%d." , SW_VERSION_N2)
120 120 PRINTF1("%d." , SW_VERSION_N3)
121 121 PRINTF1("%d **\n", SW_VERSION_N4)
122 122
123 123 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
124 124 PRINTF("** VHDL **\n")
125 125 PRINTF1("** %d.", vhdlVersion[1])
126 126 PRINTF1("%d." , vhdlVersion[2])
127 127 PRINTF1("%d **\n", vhdlVersion[3])
128 128 PRINTF("*************************\n")
129 129 PRINTF("\n\n")
130 130
131 131 init_parameter_dump();
132 init_kcoefficients_dump();
132 133 init_local_mode_parameters();
133 134 init_housekeeping_parameters();
134 135 init_k_coefficients_f0();
135 136 init_k_coefficients_f1();
136 137 init_k_coefficients_f2();
137 138
138 139 // waveform picker initialization
139 140 WFP_init_rings(); // initialize the waveform rings
140 141 WFP_reset_current_ring_nodes();
141 142 reset_waveform_picker_regs();
142 143
143 144 // spectral matrices initialization
144 145 SM_init_rings(); // initialize spectral matrices rings
145 146 SM_reset_current_ring_nodes();
146 147 reset_spectral_matrix_regs();
147 148
148 149 // configure calibration
149 150 configureCalibration( false ); // true means interleaved mode, false is for normal mode
150 151
151 152 updateLFRCurrentMode();
152 153
153 154 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
154 155
155 156 create_names(); // create all names
156 157
157 158 status = create_message_queues(); // create message queues
158 159 if (status != RTEMS_SUCCESSFUL)
159 160 {
160 161 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
161 162 }
162 163
163 164 status = create_all_tasks(); // create all tasks
164 165 if (status != RTEMS_SUCCESSFUL)
165 166 {
166 167 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
167 168 }
168 169
169 170 // **************************
170 171 // <SPACEWIRE INITIALIZATION>
171 172 grspw_timecode_callback = &timecode_irq_handler;
172 173
173 174 status_spw = spacewire_open_link(); // (1) open the link
174 175 if ( status_spw != RTEMS_SUCCESSFUL )
175 176 {
176 177 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
177 178 }
178 179
179 180 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
180 181 {
181 182 status_spw = spacewire_configure_link( fdSPW );
182 183 if ( status_spw != RTEMS_SUCCESSFUL )
183 184 {
184 185 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
185 186 }
186 187 }
187 188
188 189 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
189 190 {
190 191 status_spw = spacewire_start_link( fdSPW );
191 192 if ( status_spw != RTEMS_SUCCESSFUL )
192 193 {
193 194 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
194 195 }
195 196 }
196 197 // </SPACEWIRE INITIALIZATION>
197 198 // ***************************
198 199
199 200 status = start_all_tasks(); // start all tasks
200 201 if (status != RTEMS_SUCCESSFUL)
201 202 {
202 203 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
203 204 }
204 205
205 206 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
206 207 status = start_recv_send_tasks();
207 208 if ( status != RTEMS_SUCCESSFUL )
208 209 {
209 210 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
210 211 }
211 212
212 213 // suspend science tasks, they will be restarted later depending on the mode
213 214 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
214 215 if (status != RTEMS_SUCCESSFUL)
215 216 {
216 217 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
217 218 }
218 219
219 220 //******************************
220 221 // <SPECTRAL MATRICES SIMULATOR>
221 222 LEON_Mask_interrupt( IRQ_SM_SIMULATOR );
222 223 configure_timer((gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR, CLKDIV_SM_SIMULATOR,
223 224 IRQ_SPARC_SM_SIMULATOR, spectral_matrices_isr_simu );
224 225 // </SPECTRAL MATRICES SIMULATOR>
225 226 //*******************************
226 227
227 228 // configure IRQ handling for the waveform picker unit
228 229 status = rtems_interrupt_catch( waveforms_isr,
229 230 IRQ_SPARC_WAVEFORM_PICKER,
230 231 &old_isr_handler) ;
231 232 // configure IRQ handling for the spectral matrices unit
232 233 status = rtems_interrupt_catch( spectral_matrices_isr,
233 234 IRQ_SPARC_SPECTRAL_MATRIX,
234 235 &old_isr_handler) ;
235 236
236 237 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
237 238 if ( status_spw != RTEMS_SUCCESSFUL )
238 239 {
239 240 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
240 241 if ( status != RTEMS_SUCCESSFUL ) {
241 242 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
242 243 }
243 244 }
244 245
245 246 BOOT_PRINTF("delete INIT\n")
246 247
247 248 // test_TCH();
248 249
249 250 status = rtems_task_delete(RTEMS_SELF);
250 251
251 252 }
252 253
253 254 void init_local_mode_parameters( void )
254 255 {
255 256 /** This function initialize the param_local global variable with default values.
256 257 *
257 258 */
258 259
259 260 unsigned int i;
260 261
261 262 // LOCAL PARAMETERS
262 263
263 264 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
264 265 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
265 266 BOOT_PRINTF1("nb_interrupt_f0_MAX = %d\n", param_local.local_nb_interrupt_f0_MAX)
266 267
267 268 // init sequence counters
268 269
269 270 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
270 271 {
271 272 sequenceCounters_TC_EXE[i] = 0x00;
272 273 }
273 274 sequenceCounters_SCIENCE_NORMAL_BURST = 0x00;
274 275 sequenceCounters_SCIENCE_SBM1_SBM2 = 0x00;
275 276 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
276 277 sequenceCounterParameterDump = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
277 278 }
278 279
279 280 void reset_local_time( void )
280 281 {
281 282 time_management_regs->ctrl = time_management_regs->ctrl | 0x02; // [0010] software reset, coarse time = 0x80000000
282 283 }
283 284
284 285 void create_names( void ) // create all names for tasks and queues
285 286 {
286 287 /** This function creates all RTEMS names used in the software for tasks and queues.
287 288 *
288 289 * @return RTEMS directive status codes:
289 290 * - RTEMS_SUCCESSFUL - successful completion
290 291 *
291 292 */
292 293
293 294 // task names
294 295 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
295 296 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
296 297 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
297 298 Task_name[TASKID_STAT] = rtems_build_name( 'S', 'T', 'A', 'T' );
298 299 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
299 300 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
300 301 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
301 302 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
302 303 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
303 304 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
304 305 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
305 306 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
306 307 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
307 308 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
308 309 Task_name[TASKID_WTDG] = rtems_build_name( 'W', 'T', 'D', 'G' );
309 310 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
310 311 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
311 312 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
312 313 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
313 314
314 315 // rate monotonic period names
315 316 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
316 317
317 318 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
318 319 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
319 320 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
320 321 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
321 322 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
322 323 }
323 324
324 325 int create_all_tasks( void ) // create all tasks which run in the software
325 326 {
326 327 /** This function creates all RTEMS tasks used in the software.
327 328 *
328 329 * @return RTEMS directive status codes:
329 330 * - RTEMS_SUCCESSFUL - task created successfully
330 331 * - RTEMS_INVALID_ADDRESS - id is NULL
331 332 * - RTEMS_INVALID_NAME - invalid task name
332 333 * - RTEMS_INVALID_PRIORITY - invalid task priority
333 334 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
334 335 * - RTEMS_TOO_MANY - too many tasks created
335 336 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
336 337 * - RTEMS_TOO_MANY - too many global objects
337 338 *
338 339 */
339 340
340 341 rtems_status_code status;
341 342
342 343 //**********
343 344 // SPACEWIRE
344 345 // RECV
345 346 status = rtems_task_create(
346 347 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
347 348 RTEMS_DEFAULT_MODES,
348 349 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
349 350 );
350 351 if (status == RTEMS_SUCCESSFUL) // SEND
351 352 {
352 353 status = rtems_task_create(
353 354 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * 2,
354 355 RTEMS_DEFAULT_MODES,
355 356 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
356 357 );
357 358 }
358 359 if (status == RTEMS_SUCCESSFUL) // WTDG
359 360 {
360 361 status = rtems_task_create(
361 362 Task_name[TASKID_WTDG], TASK_PRIORITY_WTDG, RTEMS_MINIMUM_STACK_SIZE,
362 363 RTEMS_DEFAULT_MODES,
363 364 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_WTDG]
364 365 );
365 366 }
366 367 if (status == RTEMS_SUCCESSFUL) // ACTN
367 368 {
368 369 status = rtems_task_create(
369 370 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
370 371 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
371 372 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
372 373 );
373 374 }
374 375 if (status == RTEMS_SUCCESSFUL) // SPIQ
375 376 {
376 377 status = rtems_task_create(
377 378 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
378 379 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
379 380 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
380 381 );
381 382 }
382 383
383 384 //******************
384 385 // SPECTRAL MATRICES
385 386 if (status == RTEMS_SUCCESSFUL) // AVF0
386 387 {
387 388 status = rtems_task_create(
388 389 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
389 390 RTEMS_DEFAULT_MODES,
390 391 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
391 392 );
392 393 }
393 394 if (status == RTEMS_SUCCESSFUL) // PRC0
394 395 {
395 396 status = rtems_task_create(
396 397 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * 2,
397 398 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
398 399 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
399 400 );
400 401 }
401 402 if (status == RTEMS_SUCCESSFUL) // AVF1
402 403 {
403 404 status = rtems_task_create(
404 405 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
405 406 RTEMS_DEFAULT_MODES,
406 407 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
407 408 );
408 409 }
409 410 if (status == RTEMS_SUCCESSFUL) // PRC1
410 411 {
411 412 status = rtems_task_create(
412 413 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * 2,
413 414 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
414 415 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
415 416 );
416 417 }
417 418 if (status == RTEMS_SUCCESSFUL) // AVF2
418 419 {
419 420 status = rtems_task_create(
420 421 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
421 422 RTEMS_DEFAULT_MODES,
422 423 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
423 424 );
424 425 }
425 426 if (status == RTEMS_SUCCESSFUL) // PRC2
426 427 {
427 428 status = rtems_task_create(
428 429 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * 2,
429 430 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
430 431 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
431 432 );
432 433 }
433 434
434 435 //****************
435 436 // WAVEFORM PICKER
436 437 if (status == RTEMS_SUCCESSFUL) // WFRM
437 438 {
438 439 status = rtems_task_create(
439 440 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
440 441 RTEMS_DEFAULT_MODES,
441 442 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
442 443 );
443 444 }
444 445 if (status == RTEMS_SUCCESSFUL) // CWF3
445 446 {
446 447 status = rtems_task_create(
447 448 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
448 449 RTEMS_DEFAULT_MODES,
449 450 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
450 451 );
451 452 }
452 453 if (status == RTEMS_SUCCESSFUL) // CWF2
453 454 {
454 455 status = rtems_task_create(
455 456 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
456 457 RTEMS_DEFAULT_MODES,
457 458 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
458 459 );
459 460 }
460 461 if (status == RTEMS_SUCCESSFUL) // CWF1
461 462 {
462 463 status = rtems_task_create(
463 464 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
464 465 RTEMS_DEFAULT_MODES,
465 466 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
466 467 );
467 468 }
468 469 if (status == RTEMS_SUCCESSFUL) // SWBD
469 470 {
470 471 status = rtems_task_create(
471 472 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
472 473 RTEMS_DEFAULT_MODES,
473 474 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
474 475 );
475 476 }
476 477
477 478 //*****
478 479 // MISC
479 480 if (status == RTEMS_SUCCESSFUL) // STAT
480 481 {
481 482 status = rtems_task_create(
482 483 Task_name[TASKID_STAT], TASK_PRIORITY_STAT, RTEMS_MINIMUM_STACK_SIZE,
483 484 RTEMS_DEFAULT_MODES,
484 485 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_STAT]
485 486 );
486 487 }
487 488 if (status == RTEMS_SUCCESSFUL) // DUMB
488 489 {
489 490 status = rtems_task_create(
490 491 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
491 492 RTEMS_DEFAULT_MODES,
492 493 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
493 494 );
494 495 }
495 496 if (status == RTEMS_SUCCESSFUL) // HOUS
496 497 {
497 498 status = rtems_task_create(
498 499 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
499 500 RTEMS_DEFAULT_MODES,
500 501 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
501 502 );
502 503 }
503 504
504 505 return status;
505 506 }
506 507
507 508 int start_recv_send_tasks( void )
508 509 {
509 510 rtems_status_code status;
510 511
511 512 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
512 513 if (status!=RTEMS_SUCCESSFUL) {
513 514 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
514 515 }
515 516
516 517 if (status == RTEMS_SUCCESSFUL) // SEND
517 518 {
518 519 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
519 520 if (status!=RTEMS_SUCCESSFUL) {
520 521 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
521 522 }
522 523 }
523 524
524 525 return status;
525 526 }
526 527
527 528 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
528 529 {
529 530 /** This function starts all RTEMS tasks used in the software.
530 531 *
531 532 * @return RTEMS directive status codes:
532 533 * - RTEMS_SUCCESSFUL - ask started successfully
533 534 * - RTEMS_INVALID_ADDRESS - invalid task entry point
534 535 * - RTEMS_INVALID_ID - invalid task id
535 536 * - RTEMS_INCORRECT_STATE - task not in the dormant state
536 537 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
537 538 *
538 539 */
539 540 // starts all the tasks fot eh flight software
540 541
541 542 rtems_status_code status;
542 543
543 544 //**********
544 545 // SPACEWIRE
545 546 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
546 547 if (status!=RTEMS_SUCCESSFUL) {
547 548 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
548 549 }
549 550
550 551 if (status == RTEMS_SUCCESSFUL) // WTDG
551 552 {
552 553 status = rtems_task_start( Task_id[TASKID_WTDG], wtdg_task, 1 );
553 554 if (status!=RTEMS_SUCCESSFUL) {
554 555 BOOT_PRINTF("in INIT *** Error starting TASK_WTDG\n")
555 556 }
556 557 }
557 558
558 559 if (status == RTEMS_SUCCESSFUL) // ACTN
559 560 {
560 561 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
561 562 if (status!=RTEMS_SUCCESSFUL) {
562 563 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
563 564 }
564 565 }
565 566
566 567 //******************
567 568 // SPECTRAL MATRICES
568 569 if (status == RTEMS_SUCCESSFUL) // AVF0
569 570 {
570 571 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
571 572 if (status!=RTEMS_SUCCESSFUL) {
572 573 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
573 574 }
574 575 }
575 576 if (status == RTEMS_SUCCESSFUL) // PRC0
576 577 {
577 578 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
578 579 if (status!=RTEMS_SUCCESSFUL) {
579 580 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
580 581 }
581 582 }
582 583 if (status == RTEMS_SUCCESSFUL) // AVF1
583 584 {
584 585 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
585 586 if (status!=RTEMS_SUCCESSFUL) {
586 587 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
587 588 }
588 589 }
589 590 if (status == RTEMS_SUCCESSFUL) // PRC1
590 591 {
591 592 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
592 593 if (status!=RTEMS_SUCCESSFUL) {
593 594 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
594 595 }
595 596 }
596 597 if (status == RTEMS_SUCCESSFUL) // AVF2
597 598 {
598 599 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
599 600 if (status!=RTEMS_SUCCESSFUL) {
600 601 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
601 602 }
602 603 }
603 604 if (status == RTEMS_SUCCESSFUL) // PRC2
604 605 {
605 606 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
606 607 if (status!=RTEMS_SUCCESSFUL) {
607 608 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
608 609 }
609 610 }
610 611
611 612 //****************
612 613 // WAVEFORM PICKER
613 614 if (status == RTEMS_SUCCESSFUL) // WFRM
614 615 {
615 616 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
616 617 if (status!=RTEMS_SUCCESSFUL) {
617 618 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
618 619 }
619 620 }
620 621 if (status == RTEMS_SUCCESSFUL) // CWF3
621 622 {
622 623 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
623 624 if (status!=RTEMS_SUCCESSFUL) {
624 625 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
625 626 }
626 627 }
627 628 if (status == RTEMS_SUCCESSFUL) // CWF2
628 629 {
629 630 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
630 631 if (status!=RTEMS_SUCCESSFUL) {
631 632 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
632 633 }
633 634 }
634 635 if (status == RTEMS_SUCCESSFUL) // CWF1
635 636 {
636 637 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
637 638 if (status!=RTEMS_SUCCESSFUL) {
638 639 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
639 640 }
640 641 }
641 642 if (status == RTEMS_SUCCESSFUL) // SWBD
642 643 {
643 644 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
644 645 if (status!=RTEMS_SUCCESSFUL) {
645 646 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
646 647 }
647 648 }
648 649
649 650 //*****
650 651 // MISC
651 652 if (status == RTEMS_SUCCESSFUL) // HOUS
652 653 {
653 654 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
654 655 if (status!=RTEMS_SUCCESSFUL) {
655 656 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
656 657 }
657 658 }
658 659 if (status == RTEMS_SUCCESSFUL) // DUMB
659 660 {
660 661 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
661 662 if (status!=RTEMS_SUCCESSFUL) {
662 663 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
663 664 }
664 665 }
665 666 if (status == RTEMS_SUCCESSFUL) // STAT
666 667 {
667 668 status = rtems_task_start( Task_id[TASKID_STAT], stat_task, 1 );
668 669 if (status!=RTEMS_SUCCESSFUL) {
669 670 BOOT_PRINTF("in INIT *** Error starting TASK_STAT\n")
670 671 }
671 672 }
672 673
673 674 return status;
674 675 }
675 676
676 677 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
677 678 {
678 679 rtems_status_code status_recv;
679 680 rtems_status_code status_send;
680 681 rtems_status_code status_q_p0;
681 682 rtems_status_code status_q_p1;
682 683 rtems_status_code status_q_p2;
683 684 rtems_status_code ret;
684 685 rtems_id queue_id;
685 686
686 687 //****************************************
687 688 // create the queue for handling valid TCs
688 689 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
689 690 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
690 691 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
691 692 if ( status_recv != RTEMS_SUCCESSFUL ) {
692 693 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
693 694 }
694 695
695 696 //************************************************
696 697 // create the queue for handling TM packet sending
697 698 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
698 699 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
699 700 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
700 701 if ( status_send != RTEMS_SUCCESSFUL ) {
701 702 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
702 703 }
703 704
704 705 //*****************************************************************************
705 706 // create the queue for handling averaged spectral matrices for processing @ f0
706 707 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
707 708 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
708 709 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
709 710 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
710 711 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
711 712 }
712 713
713 714 //*****************************************************************************
714 715 // create the queue for handling averaged spectral matrices for processing @ f1
715 716 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
716 717 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
717 718 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
718 719 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
719 720 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
720 721 }
721 722
722 723 //*****************************************************************************
723 724 // create the queue for handling averaged spectral matrices for processing @ f2
724 725 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
725 726 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
726 727 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
727 728 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
728 729 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
729 730 }
730 731
731 732 if ( status_recv != RTEMS_SUCCESSFUL )
732 733 {
733 734 ret = status_recv;
734 735 }
735 736 else if( status_send != RTEMS_SUCCESSFUL )
736 737 {
737 738 ret = status_send;
738 739 }
739 740 else if( status_q_p0 != RTEMS_SUCCESSFUL )
740 741 {
741 742 ret = status_q_p0;
742 743 }
743 744 else if( status_q_p1 != RTEMS_SUCCESSFUL )
744 745 {
745 746 ret = status_q_p1;
746 747 }
747 748 else
748 749 {
749 750 ret = status_q_p2;
750 751 }
751 752
752 753 return ret;
753 754 }
754 755
755 756 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
756 757 {
757 758 rtems_status_code status;
758 759 rtems_name queue_name;
759 760
760 761 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
761 762
762 763 status = rtems_message_queue_ident( queue_name, 0, queue_id );
763 764
764 765 return status;
765 766 }
766 767
767 768 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
768 769 {
769 770 rtems_status_code status;
770 771 rtems_name queue_name;
771 772
772 773 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
773 774
774 775 status = rtems_message_queue_ident( queue_name, 0, queue_id );
775 776
776 777 return status;
777 778 }
778 779
779 780 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
780 781 {
781 782 rtems_status_code status;
782 783 rtems_name queue_name;
783 784
784 785 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
785 786
786 787 status = rtems_message_queue_ident( queue_name, 0, queue_id );
787 788
788 789 return status;
789 790 }
790 791
791 792 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
792 793 {
793 794 rtems_status_code status;
794 795 rtems_name queue_name;
795 796
796 797 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
797 798
798 799 status = rtems_message_queue_ident( queue_name, 0, queue_id );
799 800
800 801 return status;
801 802 }
802 803
803 804 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
804 805 {
805 806 rtems_status_code status;
806 807 rtems_name queue_name;
807 808
808 809 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
809 810
810 811 status = rtems_message_queue_ident( queue_name, 0, queue_id );
811 812
812 813 return status;
813 814 }
Show file before | Show file after | Hide comments
@@ -1,511 +1,511
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 housekeeping_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
165 165 housekeeping_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
166 166 housekeeping_packet.reserved = DEFAULT_RESERVED;
167 167 housekeeping_packet.userApplication = CCSDS_USER_APP;
168 168 housekeeping_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
169 169 housekeeping_packet.packetID[1] = (unsigned char) (APID_TM_HK);
170 170 housekeeping_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
171 171 housekeeping_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
172 172 housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
173 173 housekeeping_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
174 174 housekeeping_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
175 175 housekeeping_packet.serviceType = TM_TYPE_HK;
176 176 housekeeping_packet.serviceSubType = TM_SUBTYPE_HK;
177 177 housekeeping_packet.destinationID = TM_DESTINATION_ID_GROUND;
178 178 housekeeping_packet.sid = SID_HK;
179 179
180 180 status = rtems_rate_monotonic_cancel(HK_id);
181 181 if( status != RTEMS_SUCCESSFUL ) {
182 182 PRINTF1( "ERR *** in HOUS *** rtems_rate_monotonic_cancel(HK_id) ***code: %d\n", status )
183 183 }
184 184 else {
185 185 DEBUG_PRINTF("OK *** in HOUS *** rtems_rate_monotonic_cancel(HK_id)\n")
186 186 }
187 187
188 188 // startup phase
189 189 status = rtems_rate_monotonic_period( HK_id, SY_LFR_TIME_SYN_TIMEOUT_in_ticks );
190 190 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
191 191 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
192 192 while(period_status.state != RATE_MONOTONIC_EXPIRED ) // after SY_LFR_TIME_SYN_TIMEOUT ms, starts HK anyway
193 193 {
194 194 if ((time_management_regs->coarse_time & 0x80000000) == 0x00000000) // check time synchronization
195 195 {
196 196 break; // break if LFR is synchronized
197 197 }
198 198 else
199 199 {
200 200 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
201 201 // sched_yield();
202 202 status = rtems_task_wake_after( 10 ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 100 ms = 10 * 10 ms
203 203 }
204 204 }
205 205 status = rtems_rate_monotonic_cancel(HK_id);
206 206 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
207 207
208 208 while(1){ // launch the rate monotonic task
209 209 status = rtems_rate_monotonic_period( HK_id, HK_PERIOD );
210 210 if ( status != RTEMS_SUCCESSFUL ) {
211 211 PRINTF1( "in HOUS *** ERR period: %d\n", status);
212 212 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_6 );
213 213 }
214 214 else {
215 215 housekeeping_packet.packetSequenceControl[0] = (unsigned char) (sequenceCounterHK >> 8);
216 216 housekeeping_packet.packetSequenceControl[1] = (unsigned char) (sequenceCounterHK );
217 217 increment_seq_counter( &sequenceCounterHK );
218 218
219 219 housekeeping_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
220 220 housekeeping_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
221 221 housekeeping_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
222 222 housekeeping_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
223 223 housekeeping_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
224 224 housekeeping_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
225 225
226 226 spacewire_update_statistics();
227 227
228 228 get_temperatures( housekeeping_packet.hk_lfr_temp_scm );
229 229 get_v_e1_e2_f3( housekeeping_packet.hk_lfr_sc_v_f3 );
230 230 get_cpu_load( (unsigned char *) &housekeeping_packet.hk_lfr_cpu_load );
231 231
232 232 // SEND PACKET
233 233 status = rtems_message_queue_send( queue_id, &housekeeping_packet,
234 234 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
235 235 if (status != RTEMS_SUCCESSFUL) {
236 236 PRINTF1("in HOUS *** ERR send: %d\n", status)
237 237 }
238 238 }
239 239 }
240 240
241 241 PRINTF("in HOUS *** deleting task\n")
242 242
243 243 status = rtems_task_delete( RTEMS_SELF ); // should not return
244 244 printf( "rtems_task_delete returned with status of %d.\n", status );
245 245 return;
246 246 }
247 247
248 248 rtems_task dumb_task( rtems_task_argument unused )
249 249 {
250 250 /** This RTEMS taks is used to print messages without affecting the general behaviour of the software.
251 251 *
252 252 * @param unused is the starting argument of the RTEMS task
253 253 *
254 254 * The DUMB taks waits for RTEMS events and print messages depending on the incoming events.
255 255 *
256 256 */
257 257
258 258 unsigned int i;
259 259 unsigned int intEventOut;
260 260 unsigned int coarse_time = 0;
261 261 unsigned int fine_time = 0;
262 262 rtems_event_set event_out;
263 263
264 264 char *DumbMessages[12] = {"in DUMB *** default", // RTEMS_EVENT_0
265 265 "in DUMB *** timecode_irq_handler", // RTEMS_EVENT_1
266 266 "in DUMB *** f3 buffer changed", // RTEMS_EVENT_2
267 267 "in DUMB *** in SMIQ *** Error sending event to AVF0", // RTEMS_EVENT_3
268 268 "in DUMB *** spectral_matrices_isr *** Error sending event to SMIQ", // RTEMS_EVENT_4
269 269 "in DUMB *** waveforms_simulator_isr", // RTEMS_EVENT_5
270 270 "VHDL SM *** two buffers f0 ready", // RTEMS_EVENT_6
271 271 "ready for dump", // RTEMS_EVENT_7
272 272 "VHDL ERR *** spectral matrix", // RTEMS_EVENT_8
273 273 "tick", // RTEMS_EVENT_9
274 274 "VHDL ERR *** waveform picker", // RTEMS_EVENT_10
275 275 "VHDL ERR *** unexpected ready matrix values" // RTEMS_EVENT_11
276 276 };
277 277
278 278 BOOT_PRINTF("in DUMB *** \n")
279 279
280 280 while(1){
281 281 rtems_event_receive(RTEMS_EVENT_0 | RTEMS_EVENT_1 | RTEMS_EVENT_2 | RTEMS_EVENT_3
282 282 | RTEMS_EVENT_4 | RTEMS_EVENT_5 | RTEMS_EVENT_6 | RTEMS_EVENT_7
283 283 | RTEMS_EVENT_8 | RTEMS_EVENT_9,
284 284 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT
285 285 intEventOut = (unsigned int) event_out;
286 286 for ( i=0; i<32; i++)
287 287 {
288 288 if ( ((intEventOut >> i) & 0x0001) != 0)
289 289 {
290 290 coarse_time = time_management_regs->coarse_time;
291 291 fine_time = time_management_regs->fine_time;
292 292 printf("in DUMB *** coarse: %x, fine: %x, %s\n", coarse_time, fine_time, DumbMessages[i]);
293 293 if (i==8)
294 294 {
295 295 }
296 296 if (i==10)
297 297 {
298 298 }
299 299 }
300 300 }
301 301 }
302 302 }
303 303
304 304 //*****************************
305 305 // init housekeeping parameters
306 306
307 307 void init_housekeeping_parameters( void )
308 308 {
309 309 /** This function initialize the housekeeping_packet global variable with default values.
310 310 *
311 311 */
312 312
313 313 unsigned int i = 0;
314 314 unsigned char *parameters;
315 315
316 316 parameters = (unsigned char*) &housekeeping_packet.lfr_status_word;
317 317 for(i = 0; i< SIZE_HK_PARAMETERS; i++)
318 318 {
319 319 parameters[i] = 0x00;
320 320 }
321 321 // init status word
322 322 housekeeping_packet.lfr_status_word[0] = DEFAULT_STATUS_WORD_BYTE0;
323 323 housekeeping_packet.lfr_status_word[1] = DEFAULT_STATUS_WORD_BYTE1;
324 324 // init software version
325 325 housekeeping_packet.lfr_sw_version[0] = SW_VERSION_N1;
326 326 housekeeping_packet.lfr_sw_version[1] = SW_VERSION_N2;
327 327 housekeeping_packet.lfr_sw_version[2] = SW_VERSION_N3;
328 328 housekeeping_packet.lfr_sw_version[3] = SW_VERSION_N4;
329 329 // init fpga version
330 330 parameters = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
331 331 housekeeping_packet.lfr_fpga_version[0] = parameters[1]; // n1
332 332 housekeeping_packet.lfr_fpga_version[1] = parameters[2]; // n2
333 333 housekeeping_packet.lfr_fpga_version[2] = parameters[3]; // n3
334 334 }
335 335
336 336 void increment_seq_counter( unsigned short *packetSequenceControl )
337 337 {
338 /** This function increment the sequence counter psased in argument.
338 /** This function increment the sequence counter passes in argument.
339 339 *
340 340 * The increment does not affect the grouping flag. In case of an overflow, the counter is reset to 0.
341 341 *
342 342 */
343 343
344 344 unsigned short segmentation_grouping_flag;
345 345 unsigned short sequence_cnt;
346 346
347 347 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8; // keep bits 7 downto 6
348 348 sequence_cnt = (*packetSequenceControl) & 0x3fff; // [0011 1111 1111 1111]
349 349
350 350 if ( sequence_cnt < SEQ_CNT_MAX)
351 351 {
352 352 sequence_cnt = sequence_cnt + 1;
353 353 }
354 354 else
355 355 {
356 356 sequence_cnt = 0;
357 357 }
358 358
359 359 *packetSequenceControl = segmentation_grouping_flag | sequence_cnt ;
360 360 }
361 361
362 362 void getTime( unsigned char *time)
363 363 {
364 364 /** This function write the current local time in the time buffer passed in argument.
365 365 *
366 366 */
367 367
368 368 time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
369 369 time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
370 370 time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
371 371 time[3] = (unsigned char) (time_management_regs->coarse_time);
372 372 time[4] = (unsigned char) (time_management_regs->fine_time>>8);
373 373 time[5] = (unsigned char) (time_management_regs->fine_time);
374 374 }
375 375
376 376 unsigned long long int getTimeAsUnsignedLongLongInt( )
377 377 {
378 378 /** This function write the current local time in the time buffer passed in argument.
379 379 *
380 380 */
381 381 unsigned long long int time;
382 382
383 383 time = ( (unsigned long long int) (time_management_regs->coarse_time & 0x7fffffff) << 16 )
384 384 + time_management_regs->fine_time;
385 385
386 386 return time;
387 387 }
388 388
389 389 void send_dumb_hk( void )
390 390 {
391 391 Packet_TM_LFR_HK_t dummy_hk_packet;
392 392 unsigned char *parameters;
393 393 unsigned int i;
394 394 rtems_id queue_id;
395 395
396 396 dummy_hk_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
397 397 dummy_hk_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
398 398 dummy_hk_packet.reserved = DEFAULT_RESERVED;
399 399 dummy_hk_packet.userApplication = CCSDS_USER_APP;
400 400 dummy_hk_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
401 401 dummy_hk_packet.packetID[1] = (unsigned char) (APID_TM_HK);
402 402 dummy_hk_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
403 403 dummy_hk_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
404 404 dummy_hk_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
405 405 dummy_hk_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
406 406 dummy_hk_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
407 407 dummy_hk_packet.serviceType = TM_TYPE_HK;
408 408 dummy_hk_packet.serviceSubType = TM_SUBTYPE_HK;
409 409 dummy_hk_packet.destinationID = TM_DESTINATION_ID_GROUND;
410 410 dummy_hk_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
411 411 dummy_hk_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
412 412 dummy_hk_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
413 413 dummy_hk_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
414 414 dummy_hk_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
415 415 dummy_hk_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
416 416 dummy_hk_packet.sid = SID_HK;
417 417
418 418 // init status word
419 419 dummy_hk_packet.lfr_status_word[0] = 0xff;
420 420 dummy_hk_packet.lfr_status_word[1] = 0xff;
421 421 // init software version
422 422 dummy_hk_packet.lfr_sw_version[0] = SW_VERSION_N1;
423 423 dummy_hk_packet.lfr_sw_version[1] = SW_VERSION_N2;
424 424 dummy_hk_packet.lfr_sw_version[2] = SW_VERSION_N3;
425 425 dummy_hk_packet.lfr_sw_version[3] = SW_VERSION_N4;
426 426 // init fpga version
427 427 parameters = (unsigned char *) (REGS_ADDR_WAVEFORM_PICKER + 0xb0);
428 428 dummy_hk_packet.lfr_fpga_version[0] = parameters[1]; // n1
429 429 dummy_hk_packet.lfr_fpga_version[1] = parameters[2]; // n2
430 430 dummy_hk_packet.lfr_fpga_version[2] = parameters[3]; // n3
431 431
432 432 parameters = (unsigned char *) &dummy_hk_packet.hk_lfr_cpu_load;
433 433
434 434 for (i=0; i<100; i++)
435 435 {
436 436 parameters[i] = 0xff;
437 437 }
438 438
439 439 get_message_queue_id_send( &queue_id );
440 440
441 441 rtems_message_queue_send( queue_id, &dummy_hk_packet,
442 442 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
443 443 }
444 444
445 445 void get_temperatures( unsigned char *temperatures )
446 446 {
447 447 unsigned char* temp_scm_ptr;
448 448 unsigned char* temp_pcb_ptr;
449 449 unsigned char* temp_fpga_ptr;
450 450
451 451 // SEL1 SEL0
452 452 // 0 0 => PCB
453 453 // 0 1 => FPGA
454 454 // 1 0 => SCM
455 455
456 456 temp_scm_ptr = (unsigned char *) &time_management_regs->temp_scm;
457 457 temp_pcb_ptr = (unsigned char *) &time_management_regs->temp_pcb;
458 458 temp_fpga_ptr = (unsigned char *) &time_management_regs->temp_fpga;
459 459
460 460 temperatures[0] = temp_scm_ptr[2];
461 461 temperatures[1] = temp_scm_ptr[3];
462 462 temperatures[2] = temp_pcb_ptr[2];
463 463 temperatures[3] = temp_pcb_ptr[3];
464 464 temperatures[4] = temp_fpga_ptr[2];
465 465 temperatures[5] = temp_fpga_ptr[3];
466 466 }
467 467
468 468 void get_v_e1_e2_f3( unsigned char *spacecraft_potential )
469 469 {
470 470 unsigned char* v_ptr;
471 471 unsigned char* e1_ptr;
472 472 unsigned char* e2_ptr;
473 473
474 474 v_ptr = (unsigned char *) &waveform_picker_regs->v;
475 475 e1_ptr = (unsigned char *) &waveform_picker_regs->e1;
476 476 e2_ptr = (unsigned char *) &waveform_picker_regs->e2;
477 477
478 478 spacecraft_potential[0] = v_ptr[2];
479 479 spacecraft_potential[1] = v_ptr[3];
480 480 spacecraft_potential[2] = e1_ptr[2];
481 481 spacecraft_potential[3] = e1_ptr[3];
482 482 spacecraft_potential[4] = e2_ptr[2];
483 483 spacecraft_potential[5] = e2_ptr[3];
484 484 }
485 485
486 486 void get_cpu_load( unsigned char *resource_statistics )
487 487 {
488 488 unsigned char cpu_load;
489 489
490 490 cpu_load = lfr_rtems_cpu_usage_report();
491 491
492 492 // HK_LFR_CPU_LOAD
493 493 resource_statistics[0] = cpu_load;
494 494
495 495 // HK_LFR_CPU_LOAD_MAX
496 496 if (cpu_load > resource_statistics[1])
497 497 {
498 498 resource_statistics[1] = cpu_load;
499 499 }
500 500
501 501 // CPU_LOAD_AVE
502 502 resource_statistics[2] = 0;
503 503
504 504 #ifndef PRINT_TASK_STATISTICS
505 505 rtems_cpu_usage_reset();
506 506 #endif
507 507
508 508 }
509 509
510 510
511 511
Show file before | Show file after | Hide comments
@@ -1,1126 +1,1156
1 1 /** Functions related to the SpaceWire interface.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle SpaceWire transmissions:
7 7 * - configuration of the SpaceWire link
8 8 * - SpaceWire related interruption requests processing
9 9 * - transmission of TeleMetry packets by a dedicated RTEMS task
10 10 * - reception of TeleCommands by a dedicated RTEMS task
11 11 *
12 12 */
13 13
14 14 #include "fsw_spacewire.h"
15 15
16 16 rtems_name semq_name;
17 17 rtems_id semq_id;
18 18
19 19 //*****************
20 20 // waveform headers
21 21 Header_TM_LFR_SCIENCE_CWF_t headerCWF;
22 22 Header_TM_LFR_SCIENCE_SWF_t headerSWF;
23 23 Header_TM_LFR_SCIENCE_ASM_t headerASM;
24 24
25 25 //***********
26 26 // RTEMS TASK
27 27 rtems_task spiq_task(rtems_task_argument unused)
28 28 {
29 29 /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver.
30 30 *
31 31 * @param unused is the starting argument of the RTEMS task
32 32 *
33 33 */
34 34
35 35 rtems_event_set event_out;
36 36 rtems_status_code status;
37 37 int linkStatus;
38 38
39 39 BOOT_PRINTF("in SPIQ *** \n")
40 40
41 41 while(true){
42 42 rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT
43 43 PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n")
44 44
45 45 // [0] SUSPEND RECV AND SEND TASKS
46 46 status = rtems_task_suspend( Task_id[ TASKID_RECV ] );
47 47 if ( status != RTEMS_SUCCESSFUL ) {
48 48 PRINTF("in SPIQ *** ERR suspending RECV Task\n")
49 49 }
50 50 status = rtems_task_suspend( Task_id[ TASKID_SEND ] );
51 51 if ( status != RTEMS_SUCCESSFUL ) {
52 52 PRINTF("in SPIQ *** ERR suspending SEND Task\n")
53 53 }
54 54
55 55 // [1] CHECK THE LINK
56 56 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1)
57 57 if ( linkStatus != 5) {
58 58 PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus)
59 59 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
60 60 }
61 61
62 62 // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT
63 63 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2)
64 64 if ( linkStatus != 5 ) // [2.a] not in run state, reset the link
65 65 {
66 66 spacewire_compute_stats_offsets();
67 67 status = spacewire_reset_link( );
68 68 }
69 69 else // [2.b] in run state, start the link
70 70 {
71 71 status = spacewire_stop_and_start_link( fdSPW ); // start the link
72 72 if ( status != RTEMS_SUCCESSFUL)
73 73 {
74 74 PRINTF1("in SPIQ *** ERR spacewire_start_link %d\n", status)
75 75 }
76 76 }
77 77
78 78 // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS
79 79 if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully
80 80 {
81 81 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
82 82 if ( status != RTEMS_SUCCESSFUL ) {
83 83 PRINTF("in SPIQ *** ERR resuming SEND Task\n")
84 84 }
85 85 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
86 86 if ( status != RTEMS_SUCCESSFUL ) {
87 87 PRINTF("in SPIQ *** ERR resuming RECV Task\n")
88 88 }
89 89 }
90 90 else // [3.b] the link is not in run state, go in STANDBY mode
91 91 {
92 92 status = stop_current_mode();
93 93 if ( status != RTEMS_SUCCESSFUL ) {
94 94 PRINTF1("in SPIQ *** ERR stop_current_mode *** code %d\n", status)
95 95 }
96 96 status = enter_mode( LFR_MODE_STANDBY, 0 );
97 97 if ( status != RTEMS_SUCCESSFUL ) {
98 98 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
99 99 }
100 100 // wake the WTDG task up to wait for the link recovery
101 101 status = rtems_event_send ( Task_id[TASKID_WTDG], RTEMS_EVENT_0 );
102 102 status = rtems_task_suspend( RTEMS_SELF );
103 103 }
104 104 }
105 105 }
106 106
107 107 rtems_task recv_task( rtems_task_argument unused )
108 108 {
109 109 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
110 110 *
111 111 * @param unused is the starting argument of the RTEMS task
112 112 *
113 113 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
114 114 * 1. It reads the incoming data.
115 115 * 2. Launches the acceptance procedure.
116 116 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
117 117 *
118 118 */
119 119
120 120 int len;
121 121 ccsdsTelecommandPacket_t currentTC;
122 122 unsigned char computed_CRC[ 2 ];
123 123 unsigned char currentTC_LEN_RCV[ 2 ];
124 124 unsigned char destinationID;
125 125 unsigned int estimatedPacketLength;
126 126 unsigned int parserCode;
127 127 rtems_status_code status;
128 128 rtems_id queue_recv_id;
129 129 rtems_id queue_send_id;
130 130
131 131 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
132 132
133 133 status = get_message_queue_id_recv( &queue_recv_id );
134 134 if (status != RTEMS_SUCCESSFUL)
135 135 {
136 136 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
137 137 }
138 138
139 139 status = get_message_queue_id_send( &queue_send_id );
140 140 if (status != RTEMS_SUCCESSFUL)
141 141 {
142 142 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
143 143 }
144 144
145 145 BOOT_PRINTF("in RECV *** \n")
146 146
147 147 while(1)
148 148 {
149 149 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
150 150 if (len == -1){ // error during the read call
151 151 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
152 152 }
153 153 else {
154 154 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
155 155 PRINTF("in RECV *** packet lenght too short\n")
156 156 }
157 157 else {
158 158 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // => -3 is for Prot ID, Reserved and User App bytes
159 159 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> 8);
160 160 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
161 161 // CHECK THE TC
162 162 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
163 163 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
164 164 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
165 165 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
166 166 || (parserCode == WRONG_SRC_ID) )
167 167 { // send TM_LFR_TC_EXE_CORRUPTED
168 168 PRINTF1("TC corrupted received, with code: %d\n", parserCode)
169 169 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
170 170 &&
171 171 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
172 172 )
173 173 {
174 174 if ( parserCode == WRONG_SRC_ID )
175 175 {
176 176 destinationID = SID_TC_GROUND;
177 177 }
178 178 else
179 179 {
180 180 destinationID = currentTC.sourceID;
181 181 }
182 182 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
183 183 computed_CRC, currentTC_LEN_RCV,
184 184 destinationID );
185 185 }
186 186 }
187 187 else
188 188 { // send valid TC to the action launcher
189 189 status = rtems_message_queue_send( queue_recv_id, &currentTC,
190 190 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + 3);
191 191 }
192 192 }
193 193 }
194 194 }
195 195 }
196 196
197 197 rtems_task send_task( rtems_task_argument argument)
198 198 {
199 199 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
200 200 *
201 201 * @param unused is the starting argument of the RTEMS task
202 202 *
203 203 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
204 204 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
205 205 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
206 206 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
207 207 * data it contains.
208 208 *
209 209 */
210 210
211 211 rtems_status_code status; // RTEMS status code
212 212 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
213 213 ring_node *incomingRingNodePtr;
214 214 int ring_node_address;
215 215 char *charPtr;
216 216 spw_ioctl_pkt_send *spw_ioctl_send;
217 217 size_t size; // size of the incoming TC packet
218 218 u_int32_t count;
219 219 rtems_id queue_id;
220 unsigned char sid;
220 unsigned int sid;
221 221
222 222 incomingRingNodePtr = NULL;
223 223 ring_node_address = 0;
224 224 charPtr = (char *) &ring_node_address;
225 225 sid = 0;
226 226
227 227 init_header_cwf( &headerCWF );
228 228 init_header_swf( &headerSWF );
229 229 init_header_asm( &headerASM );
230 230
231 231 status = get_message_queue_id_send( &queue_id );
232 232 if (status != RTEMS_SUCCESSFUL)
233 233 {
234 234 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
235 235 }
236 236
237 237 BOOT_PRINTF("in SEND *** \n")
238 238
239 239 while(1)
240 240 {
241 241 status = rtems_message_queue_receive( queue_id, incomingData, &size,
242 242 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
243 243
244 244 if (status!=RTEMS_SUCCESSFUL)
245 245 {
246 246 PRINTF1("in SEND *** (1) ERR = %d\n", status)
247 247 }
248 248 else
249 249 {
250 250 if ( size == sizeof(ring_node*) )
251 251 {
252 252 charPtr[0] = incomingData[0];
253 253 charPtr[1] = incomingData[1];
254 254 charPtr[2] = incomingData[2];
255 255 charPtr[3] = incomingData[3];
256 256 incomingRingNodePtr = (ring_node*) ring_node_address;
257 257 sid = incomingRingNodePtr->sid;
258 258 if ( (sid==SID_NORM_CWF_LONG_F3)
259 259 || (sid==SID_BURST_CWF_F2 )
260 260 || (sid==SID_SBM1_CWF_F1 )
261 261 || (sid==SID_SBM2_CWF_F2 ))
262 262 {
263 263 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
264 264 }
265 265 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
266 266 {
267 267 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
268 268 }
269 269 else if ( (sid==SID_NORM_CWF_F3) )
270 270 {
271 271 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
272 272 }
273 else if ( (sid==SID_NORM_ASM_F0) || (SID_NORM_ASM_F1) || (SID_NORM_ASM_F2) )
273 else if ( (sid==SID_NORM_ASM_F0) || (sid==SID_NORM_ASM_F1) || (sid==SID_NORM_ASM_F2) )
274 274 {
275 275 spw_send_asm( incomingRingNodePtr, &headerASM );
276 276 }
277 else if ( sid==TM_CODE_K_DUMP )
278 {
279 spw_send_k_dump( incomingRingNodePtr );
280 }
277 281 else
278 282 {
279 283 printf("unexpected sid = %d\n", sid);
280 284 }
281 285 }
282 286 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
283 287 {
284 288 status = write( fdSPW, incomingData, size );
285 289 if (status == -1){
286 290 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
287 291 }
288 292 }
289 293 else // the incoming message is a spw_ioctl_pkt_send structure
290 294 {
291 295 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
292 296 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
293 297 if (status == -1){
294 298 printf("size = %d, %x, %x, %x, %x, %x\n",
295 299 size,
296 300 incomingData[0],
297 301 incomingData[1],
298 302 incomingData[2],
299 303 incomingData[3],
300 304 incomingData[4]);
301 305 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
302 306 }
303 307 }
304 308 }
305 309
306 310 status = rtems_message_queue_get_number_pending( queue_id, &count );
307 311 if (status != RTEMS_SUCCESSFUL)
308 312 {
309 313 PRINTF1("in SEND *** (3) ERR = %d\n", status)
310 314 }
311 315 else
312 316 {
313 317 if (count > maxCount)
314 318 {
315 319 maxCount = count;
316 320 }
317 321 }
318 322 }
319 323 }
320 324
321 325 rtems_task wtdg_task( rtems_task_argument argument )
322 326 {
323 327 rtems_event_set event_out;
324 328 rtems_status_code status;
325 329 int linkStatus;
326 330
327 331 BOOT_PRINTF("in WTDG ***\n")
328 332
329 333 while(1)
330 334 {
331 335 // wait for an RTEMS_EVENT
332 336 rtems_event_receive( RTEMS_EVENT_0,
333 337 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
334 338 PRINTF("in WTDG *** wait for the link\n")
335 339 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
336 340 while( linkStatus != 5) // wait for the link
337 341 {
338 342 status = rtems_task_wake_after( 10 ); // monitor the link each 100ms
339 343 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
340 344 }
341 345
342 346 status = spacewire_stop_and_start_link( fdSPW );
343 347
344 348 if (status != RTEMS_SUCCESSFUL)
345 349 {
346 350 PRINTF1("in WTDG *** ERR link not started %d\n", status)
347 351 }
348 352 else
349 353 {
350 354 PRINTF("in WTDG *** OK link started\n")
351 355 }
352 356
353 357 // restart the SPIQ task
354 358 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
355 359 if ( status != RTEMS_SUCCESSFUL ) {
356 360 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
357 361 }
358 362
359 363 // restart RECV and SEND
360 364 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
361 365 if ( status != RTEMS_SUCCESSFUL ) {
362 366 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
363 367 }
364 368 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
365 369 if ( status != RTEMS_SUCCESSFUL ) {
366 370 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
367 371 }
368 372 }
369 373 }
370 374
371 375 //****************
372 376 // OTHER FUNCTIONS
373 377 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
374 378 {
375 379 /** This function opens the SpaceWire link.
376 380 *
377 381 * @return a valid file descriptor in case of success, -1 in case of a failure
378 382 *
379 383 */
380 384 rtems_status_code status;
381 385
382 386 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
383 387 if ( fdSPW < 0 ) {
384 388 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
385 389 }
386 390 else
387 391 {
388 392 status = RTEMS_SUCCESSFUL;
389 393 }
390 394
391 395 return status;
392 396 }
393 397
394 398 int spacewire_start_link( int fd )
395 399 {
396 400 rtems_status_code status;
397 401
398 402 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
399 403 // -1 default hardcoded driver timeout
400 404
401 405 return status;
402 406 }
403 407
404 408 int spacewire_stop_and_start_link( int fd )
405 409 {
406 410 rtems_status_code status;
407 411
408 412 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
409 413 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
410 414 // -1 default hardcoded driver timeout
411 415
412 416 return status;
413 417 }
414 418
415 419 int spacewire_configure_link( int fd )
416 420 {
417 421 /** This function configures the SpaceWire link.
418 422 *
419 423 * @return GR-RTEMS-DRIVER directive status codes:
420 424 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
421 425 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
422 426 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
423 427 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
424 428 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
425 429 * - 5 EIO - Error when writing to grswp hardware registers.
426 430 * - 2 ENOENT - No such file or directory
427 431 */
428 432
429 433 rtems_status_code status;
430 434
431 435 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
432 436 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
433 437
434 438 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
435 439 if (status!=RTEMS_SUCCESSFUL) {
436 440 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
437 441 }
438 442 //
439 443 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
440 444 if (status!=RTEMS_SUCCESSFUL) {
441 445 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
442 446 }
443 447 //
444 448 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
445 449 if (status!=RTEMS_SUCCESSFUL) {
446 450 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
447 451 }
448 452 //
449 453 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
450 454 if (status!=RTEMS_SUCCESSFUL) {
451 455 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
452 456 }
453 457 //
454 458 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
455 459 if (status!=RTEMS_SUCCESSFUL) {
456 460 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
457 461 }
458 462 //
459 463 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
460 464 if (status!=RTEMS_SUCCESSFUL) {
461 465 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
462 466 }
463 467 //
464 468 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, 0x0909); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
465 469 if (status!=RTEMS_SUCCESSFUL) {
466 470 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
467 471 }
468 472
469 473 return status;
470 474 }
471 475
472 476 int spacewire_reset_link( void )
473 477 {
474 478 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
475 479 *
476 480 * @return RTEMS directive status code:
477 481 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
478 482 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
479 483 *
480 484 */
481 485
482 486 rtems_status_code status_spw;
483 487 rtems_status_code status;
484 488 int i;
485 489
486 490 for ( i=0; i<SY_LFR_DPU_CONNECT_ATTEMPT; i++ )
487 491 {
488 492 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
489 493
490 494 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
491 495
492 496 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
493 497
494 498 status_spw = spacewire_stop_and_start_link( fdSPW );
495 499 if ( status_spw != RTEMS_SUCCESSFUL )
496 500 {
497 501 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw)
498 502 }
499 503
500 504 if ( status_spw == RTEMS_SUCCESSFUL)
501 505 {
502 506 break;
503 507 }
504 508 }
505 509
506 510 return status_spw;
507 511 }
508 512
509 513 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
510 514 {
511 515 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
512 516 *
513 517 * @param val is the value, 0 or 1, used to set the value of the NP bit.
514 518 * @param regAddr is the address of the GRSPW control register.
515 519 *
516 520 * NP is the bit 20 of the GRSPW control register.
517 521 *
518 522 */
519 523
520 524 unsigned int *spwptr = (unsigned int*) regAddr;
521 525
522 526 if (val == 1) {
523 527 *spwptr = *spwptr | 0x00100000; // [NP] set the No port force bit
524 528 }
525 529 if (val== 0) {
526 530 *spwptr = *spwptr & 0xffdfffff;
527 531 }
528 532 }
529 533
530 534 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
531 535 {
532 536 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
533 537 *
534 538 * @param val is the value, 0 or 1, used to set the value of the RE bit.
535 539 * @param regAddr is the address of the GRSPW control register.
536 540 *
537 541 * RE is the bit 16 of the GRSPW control register.
538 542 *
539 543 */
540 544
541 545 unsigned int *spwptr = (unsigned int*) regAddr;
542 546
543 547 if (val == 1)
544 548 {
545 549 *spwptr = *spwptr | 0x00010000; // [RE] set the RMAP Enable bit
546 550 }
547 551 if (val== 0)
548 552 {
549 553 *spwptr = *spwptr & 0xfffdffff;
550 554 }
551 555 }
552 556
553 557 void spacewire_compute_stats_offsets( void )
554 558 {
555 559 /** This function computes the SpaceWire statistics offsets in case of a SpaceWire related interruption raising.
556 560 *
557 561 * The offsets keep a record of the statistics in case of a reset of the statistics. They are added to the current statistics
558 562 * to keep the counters consistent even after a reset of the SpaceWire driver (the counter are set to zero by the driver when it
559 563 * during the open systel call).
560 564 *
561 565 */
562 566
563 567 spw_stats spacewire_stats_grspw;
564 568 rtems_status_code status;
565 569
566 570 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
567 571
568 572 spacewire_stats_backup.packets_received = spacewire_stats_grspw.packets_received
569 573 + spacewire_stats.packets_received;
570 574 spacewire_stats_backup.packets_sent = spacewire_stats_grspw.packets_sent
571 575 + spacewire_stats.packets_sent;
572 576 spacewire_stats_backup.parity_err = spacewire_stats_grspw.parity_err
573 577 + spacewire_stats.parity_err;
574 578 spacewire_stats_backup.disconnect_err = spacewire_stats_grspw.disconnect_err
575 579 + spacewire_stats.disconnect_err;
576 580 spacewire_stats_backup.escape_err = spacewire_stats_grspw.escape_err
577 581 + spacewire_stats.escape_err;
578 582 spacewire_stats_backup.credit_err = spacewire_stats_grspw.credit_err
579 583 + spacewire_stats.credit_err;
580 584 spacewire_stats_backup.write_sync_err = spacewire_stats_grspw.write_sync_err
581 585 + spacewire_stats.write_sync_err;
582 586 spacewire_stats_backup.rx_rmap_header_crc_err = spacewire_stats_grspw.rx_rmap_header_crc_err
583 587 + spacewire_stats.rx_rmap_header_crc_err;
584 588 spacewire_stats_backup.rx_rmap_data_crc_err = spacewire_stats_grspw.rx_rmap_data_crc_err
585 589 + spacewire_stats.rx_rmap_data_crc_err;
586 590 spacewire_stats_backup.early_ep = spacewire_stats_grspw.early_ep
587 591 + spacewire_stats.early_ep;
588 592 spacewire_stats_backup.invalid_address = spacewire_stats_grspw.invalid_address
589 593 + spacewire_stats.invalid_address;
590 594 spacewire_stats_backup.rx_eep_err = spacewire_stats_grspw.rx_eep_err
591 595 + spacewire_stats.rx_eep_err;
592 596 spacewire_stats_backup.rx_truncated = spacewire_stats_grspw.rx_truncated
593 597 + spacewire_stats.rx_truncated;
594 598 }
595 599
596 600 void spacewire_update_statistics( void )
597 601 {
598 602 rtems_status_code status;
599 603 spw_stats spacewire_stats_grspw;
600 604
601 605 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
602 606
603 607 spacewire_stats.packets_received = spacewire_stats_backup.packets_received
604 608 + spacewire_stats_grspw.packets_received;
605 609 spacewire_stats.packets_sent = spacewire_stats_backup.packets_sent
606 610 + spacewire_stats_grspw.packets_sent;
607 611 spacewire_stats.parity_err = spacewire_stats_backup.parity_err
608 612 + spacewire_stats_grspw.parity_err;
609 613 spacewire_stats.disconnect_err = spacewire_stats_backup.disconnect_err
610 614 + spacewire_stats_grspw.disconnect_err;
611 615 spacewire_stats.escape_err = spacewire_stats_backup.escape_err
612 616 + spacewire_stats_grspw.escape_err;
613 617 spacewire_stats.credit_err = spacewire_stats_backup.credit_err
614 618 + spacewire_stats_grspw.credit_err;
615 619 spacewire_stats.write_sync_err = spacewire_stats_backup.write_sync_err
616 620 + spacewire_stats_grspw.write_sync_err;
617 621 spacewire_stats.rx_rmap_header_crc_err = spacewire_stats_backup.rx_rmap_header_crc_err
618 622 + spacewire_stats_grspw.rx_rmap_header_crc_err;
619 623 spacewire_stats.rx_rmap_data_crc_err = spacewire_stats_backup.rx_rmap_data_crc_err
620 624 + spacewire_stats_grspw.rx_rmap_data_crc_err;
621 625 spacewire_stats.early_ep = spacewire_stats_backup.early_ep
622 626 + spacewire_stats_grspw.early_ep;
623 627 spacewire_stats.invalid_address = spacewire_stats_backup.invalid_address
624 628 + spacewire_stats_grspw.invalid_address;
625 629 spacewire_stats.rx_eep_err = spacewire_stats_backup.rx_eep_err
626 630 + spacewire_stats_grspw.rx_eep_err;
627 631 spacewire_stats.rx_truncated = spacewire_stats_backup.rx_truncated
628 632 + spacewire_stats_grspw.rx_truncated;
629 633 //spacewire_stats.tx_link_err;
630 634
631 635 //****************************
632 636 // DPU_SPACEWIRE_IF_STATISTICS
633 637 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (spacewire_stats.packets_received >> 8);
634 638 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (spacewire_stats.packets_received);
635 639 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (spacewire_stats.packets_sent >> 8);
636 640 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (spacewire_stats.packets_sent);
637 641 //housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt;
638 642 //housekeeping_packet.hk_lfr_dpu_spw_last_timc;
639 643
640 644 //******************************************
641 645 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
642 646 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) spacewire_stats.parity_err;
643 647 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) spacewire_stats.disconnect_err;
644 648 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) spacewire_stats.escape_err;
645 649 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) spacewire_stats.credit_err;
646 650 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) spacewire_stats.write_sync_err;
647 651
648 652 //*********************************************
649 653 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
650 654 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) spacewire_stats.early_ep;
651 655 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) spacewire_stats.invalid_address;
652 656 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) spacewire_stats.rx_eep_err;
653 657 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) spacewire_stats.rx_truncated;
654 658 }
655 659
656 660 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
657 661 {
658 662 // a valid timecode has been received, write it in the HK report
659 663 unsigned int * grspwPtr;
660 664
661 665 grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER);
662 666
663 667 housekeeping_packet.hk_lfr_dpu_spw_last_timc = (unsigned char) (grspwPtr[0] & 0xff); // [11 1111]
664 668
665 669 // update the number of valid timecodes that have been received
666 670 if (housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt == 255)
667 671 {
668 672 housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt = 0;
669 673 }
670 674 else
671 675 {
672 676 housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt = housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt + 1;
673 677 }
674 678 }
675 679
676 680 rtems_timer_service_routine user_routine( rtems_id timer_id, void *user_data )
677 681 {
678 682 int linkStatus;
679 683 rtems_status_code status;
680 684
681 685 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
682 686
683 687 if ( linkStatus == 5) {
684 688 PRINTF("in spacewire_reset_link *** link is running\n")
685 689 status = RTEMS_SUCCESSFUL;
686 690 }
687 691 }
688 692
689 693 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
690 694 {
691 695 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
692 696 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
693 697 header->reserved = DEFAULT_RESERVED;
694 698 header->userApplication = CCSDS_USER_APP;
695 699 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
696 700 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
697 701 header->packetLength[0] = 0x00;
698 702 header->packetLength[1] = 0x00;
699 703 // DATA FIELD HEADER
700 704 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
701 705 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
702 706 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
703 707 header->destinationID = TM_DESTINATION_ID_GROUND;
704 708 header->time[0] = 0x00;
705 709 header->time[0] = 0x00;
706 710 header->time[0] = 0x00;
707 711 header->time[0] = 0x00;
708 712 header->time[0] = 0x00;
709 713 header->time[0] = 0x00;
710 714 // AUXILIARY DATA HEADER
711 715 header->sid = 0x00;
712 716 header->hkBIA = DEFAULT_HKBIA;
713 717 header->blkNr[0] = 0x00;
714 718 header->blkNr[1] = 0x00;
715 719 }
716 720
717 721 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
718 722 {
719 723 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
720 724 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
721 725 header->reserved = DEFAULT_RESERVED;
722 726 header->userApplication = CCSDS_USER_APP;
723 727 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
724 728 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
725 729 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
726 730 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
727 731 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
728 732 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
729 733 // DATA FIELD HEADER
730 734 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
731 735 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
732 736 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
733 737 header->destinationID = TM_DESTINATION_ID_GROUND;
734 738 header->time[0] = 0x00;
735 739 header->time[0] = 0x00;
736 740 header->time[0] = 0x00;
737 741 header->time[0] = 0x00;
738 742 header->time[0] = 0x00;
739 743 header->time[0] = 0x00;
740 744 // AUXILIARY DATA HEADER
741 745 header->sid = 0x00;
742 746 header->hkBIA = DEFAULT_HKBIA;
743 747 header->pktCnt = DEFAULT_PKTCNT; // PKT_CNT
744 748 header->pktNr = 0x00;
745 749 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
746 750 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
747 751 }
748 752
749 753 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
750 754 {
751 755 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
752 756 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
753 757 header->reserved = DEFAULT_RESERVED;
754 758 header->userApplication = CCSDS_USER_APP;
755 759 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
756 760 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
757 761 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
758 762 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
759 763 header->packetLength[0] = 0x00;
760 764 header->packetLength[1] = 0x00;
761 765 // DATA FIELD HEADER
762 766 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
763 767 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
764 768 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
765 769 header->destinationID = TM_DESTINATION_ID_GROUND;
766 770 header->time[0] = 0x00;
767 771 header->time[0] = 0x00;
768 772 header->time[0] = 0x00;
769 773 header->time[0] = 0x00;
770 774 header->time[0] = 0x00;
771 775 header->time[0] = 0x00;
772 776 // AUXILIARY DATA HEADER
773 777 header->sid = 0x00;
774 778 header->biaStatusInfo = 0x00;
775 779 header->pa_lfr_pkt_cnt_asm = 0x00;
776 780 header->pa_lfr_pkt_nr_asm = 0x00;
777 781 header->pa_lfr_asm_blk_nr[0] = 0x00;
778 782 header->pa_lfr_asm_blk_nr[1] = 0x00;
779 783 }
780 784
781 785 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
782 786 Header_TM_LFR_SCIENCE_CWF_t *header )
783 787 {
784 788 /** This function sends CWF CCSDS packets (F2, F1 or F0).
785 789 *
786 790 * @param waveform points to the buffer containing the data that will be send.
787 791 * @param sid is the source identifier of the data that will be sent.
788 792 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
789 793 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
790 794 * contain information to setup the transmission of the data packets.
791 795 *
792 796 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
793 797 *
794 798 */
795 799
796 800 unsigned int i;
797 801 int ret;
798 802 unsigned int coarseTime;
799 803 unsigned int fineTime;
800 804 rtems_status_code status;
801 805 spw_ioctl_pkt_send spw_ioctl_send_CWF;
802 806 int *dataPtr;
803 807 unsigned char sid;
804 808
805 809 spw_ioctl_send_CWF.hlen = TM_HEADER_LEN + 4 + 10; // + 4 is for the protocole extra header, + 10 is for the auxiliary header
806 810 spw_ioctl_send_CWF.options = 0;
807 811
808 812 ret = LFR_DEFAULT;
809 813 sid = (unsigned char) ring_node_to_send->sid;
810 814
811 815 coarseTime = ring_node_to_send->coarseTime;
812 816 fineTime = ring_node_to_send->fineTime;
813 817 dataPtr = (int*) ring_node_to_send->buffer_address;
814 818
815 819 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
816 820 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
817 821 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
818 822 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
819 823 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
820 824
821 825 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
822 826 {
823 827 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
824 828 spw_ioctl_send_CWF.hdr = (char*) header;
825 829 // BUILD THE DATA
826 830 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
827 831
828 832 // SET PACKET SEQUENCE CONTROL
829 833 increment_seq_counter_source_id( header->packetSequenceControl, sid );
830 834
831 835 // SET SID
832 836 header->sid = sid;
833 837
834 838 // SET PACKET TIME
835 839 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
836 840 //
837 841 header->time[0] = header->acquisitionTime[0];
838 842 header->time[1] = header->acquisitionTime[1];
839 843 header->time[2] = header->acquisitionTime[2];
840 844 header->time[3] = header->acquisitionTime[3];
841 845 header->time[4] = header->acquisitionTime[4];
842 846 header->time[5] = header->acquisitionTime[5];
843 847
844 848 // SET PACKET ID
845 849 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
846 850 {
847 851 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8);
848 852 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
849 853 }
850 854 else
851 855 {
852 856 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
853 857 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
854 858 }
855 859
856 860 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
857 861 if (status != RTEMS_SUCCESSFUL) {
858 862 printf("%d-%d, ERR %d\n", sid, i, (int) status);
859 863 ret = LFR_DEFAULT;
860 864 }
861 865 }
862 866
863 867 return ret;
864 868 }
865 869
866 870 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
867 871 Header_TM_LFR_SCIENCE_SWF_t *header )
868 872 {
869 873 /** This function sends SWF CCSDS packets (F2, F1 or F0).
870 874 *
871 875 * @param waveform points to the buffer containing the data that will be send.
872 876 * @param sid is the source identifier of the data that will be sent.
873 877 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
874 878 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
875 879 * contain information to setup the transmission of the data packets.
876 880 *
877 881 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
878 882 *
879 883 */
880 884
881 885 unsigned int i;
882 886 int ret;
883 887 unsigned int coarseTime;
884 888 unsigned int fineTime;
885 889 rtems_status_code status;
886 890 spw_ioctl_pkt_send spw_ioctl_send_SWF;
887 891 int *dataPtr;
888 892 unsigned char sid;
889 893
890 894 spw_ioctl_send_SWF.hlen = TM_HEADER_LEN + 4 + 12; // + 4 is for the protocole extra header, + 12 is for the auxiliary header
891 895 spw_ioctl_send_SWF.options = 0;
892 896
893 897 ret = LFR_DEFAULT;
894 898
895 899 coarseTime = ring_node_to_send->coarseTime;
896 900 fineTime = ring_node_to_send->fineTime;
897 901 dataPtr = (int*) ring_node_to_send->buffer_address;
898 902 sid = ring_node_to_send->sid;
899 903
900 904 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
901 905
902 906 for (i=0; i<7; i++) // send waveform
903 907 {
904 908 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
905 909 spw_ioctl_send_SWF.hdr = (char*) header;
906 910
907 911 // SET PACKET SEQUENCE CONTROL
908 912 increment_seq_counter_source_id( header->packetSequenceControl, sid );
909 913
910 914 // SET PACKET LENGTH AND BLKNR
911 915 if (i == 6)
912 916 {
913 917 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
914 918 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8);
915 919 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
916 920 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> 8);
917 921 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
918 922 }
919 923 else
920 924 {
921 925 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
922 926 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8);
923 927 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
924 928 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> 8);
925 929 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
926 930 }
927 931
928 932 // SET PACKET TIME
929 933 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
930 934 //
931 935 header->time[0] = header->acquisitionTime[0];
932 936 header->time[1] = header->acquisitionTime[1];
933 937 header->time[2] = header->acquisitionTime[2];
934 938 header->time[3] = header->acquisitionTime[3];
935 939 header->time[4] = header->acquisitionTime[4];
936 940 header->time[5] = header->acquisitionTime[5];
937 941
938 942 // SET SID
939 943 header->sid = sid;
940 944
941 945 // SET PKTNR
942 946 header->pktNr = i+1; // PKT_NR
943 947
944 948 // SEND PACKET
945 949 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
946 950 if (status != RTEMS_SUCCESSFUL) {
947 951 printf("%d-%d, ERR %d\n", sid, i, (int) status);
948 952 ret = LFR_DEFAULT;
949 953 }
950 954 }
951 955
952 956 return ret;
953 957 }
954 958
955 959 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
956 960 Header_TM_LFR_SCIENCE_CWF_t *header )
957 961 {
958 962 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
959 963 *
960 964 * @param waveform points to the buffer containing the data that will be send.
961 965 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
962 966 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
963 967 * contain information to setup the transmission of the data packets.
964 968 *
965 969 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
966 970 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
967 971 *
968 972 */
969 973
970 974 unsigned int i;
971 975 int ret;
972 976 unsigned int coarseTime;
973 977 unsigned int fineTime;
974 978 rtems_status_code status;
975 979 spw_ioctl_pkt_send spw_ioctl_send_CWF;
976 980 char *dataPtr;
977 981 unsigned char sid;
978 982
979 983 spw_ioctl_send_CWF.hlen = TM_HEADER_LEN + 4 + 10; // + 4 is for the protocole extra header, + 10 is for the auxiliary header
980 984 spw_ioctl_send_CWF.options = 0;
981 985
982 986 ret = LFR_DEFAULT;
983 987 sid = ring_node_to_send->sid;
984 988
985 989 coarseTime = ring_node_to_send->coarseTime;
986 990 fineTime = ring_node_to_send->fineTime;
987 991 dataPtr = (char*) ring_node_to_send->buffer_address;
988 992
989 993 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> 8);
990 994 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
991 995 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
992 996 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> 8);
993 997 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
994 998
995 999 //*********************
996 1000 // SEND CWF3_light DATA
997 1001 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
998 1002 {
999 1003 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
1000 1004 spw_ioctl_send_CWF.hdr = (char*) header;
1001 1005 // BUILD THE DATA
1002 1006 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
1003 1007
1004 1008 // SET PACKET SEQUENCE COUNTER
1005 1009 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1006 1010
1007 1011 // SET SID
1008 1012 header->sid = sid;
1009 1013
1010 1014 // SET PACKET TIME
1011 1015 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1012 1016 //
1013 1017 header->time[0] = header->acquisitionTime[0];
1014 1018 header->time[1] = header->acquisitionTime[1];
1015 1019 header->time[2] = header->acquisitionTime[2];
1016 1020 header->time[3] = header->acquisitionTime[3];
1017 1021 header->time[4] = header->acquisitionTime[4];
1018 1022 header->time[5] = header->acquisitionTime[5];
1019 1023
1020 1024 // SET PACKET ID
1021 1025 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1022 1026 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1023 1027
1024 1028 // SEND PACKET
1025 1029 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1026 1030 if (status != RTEMS_SUCCESSFUL) {
1027 1031 printf("%d-%d, ERR %d\n", sid, i, (int) status);
1028 1032 ret = LFR_DEFAULT;
1029 1033 }
1030 1034 }
1031 1035
1032 1036 return ret;
1033 1037 }
1034 1038
1035 1039 void spw_send_asm( ring_node *ring_node_to_send,
1036 1040 Header_TM_LFR_SCIENCE_ASM_t *header )
1037 1041 {
1038 1042 unsigned int i;
1039 1043 unsigned int length = 0;
1040 1044 rtems_status_code status;
1041 1045 unsigned int sid;
1042 1046 char *spectral_matrix;
1043 1047 int coarseTime;
1044 1048 int fineTime;
1045 1049 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1046 1050
1047 1051 sid = ring_node_to_send->sid;
1048 1052 spectral_matrix = (char*) ring_node_to_send->buffer_address;
1049 1053 coarseTime = ring_node_to_send->coarseTime;
1050 1054 fineTime = ring_node_to_send->fineTime;
1051 1055
1052 1056 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1053 1057
1054 1058 for (i=0; i<2; i++)
1055 1059 {
1056 1060 // (1) BUILD THE DATA
1057 1061 switch(sid)
1058 1062 {
1059 1063 case SID_NORM_ASM_F0:
1060 1064 spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F0_IN_BYTES / 2; // 2 packets will be sent
1061 1065 spw_ioctl_send_ASM.data = &spectral_matrix[
1062 1066 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0) ) * NB_VALUES_PER_SM ) * 2
1063 1067 ];
1064 1068 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0;
1065 1069 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1066 1070 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0) >> 8 ); // BLK_NR MSB
1067 1071 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0); // BLK_NR LSB
1068 1072 break;
1069 1073 case SID_NORM_ASM_F1:
1070 1074 spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F1_IN_BYTES / 2; // 2 packets will be sent
1071 1075 spw_ioctl_send_ASM.data = &spectral_matrix[
1072 1076 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1) ) * NB_VALUES_PER_SM ) * 2
1073 1077 ];
1074 1078 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1;
1075 1079 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1076 1080 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1) >> 8 ); // BLK_NR MSB
1077 1081 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1); // BLK_NR LSB
1078 1082 break;
1079 1083 case SID_NORM_ASM_F2:
1080 1084 spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F2_IN_BYTES / 2; // 2 packets will be sent
1081 1085 spw_ioctl_send_ASM.data = &spectral_matrix[
1082 1086 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM ) * 2
1083 1087 ];
1084 1088 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1085 1089 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1086 1090 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB
1087 1091 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1088 1092 break;
1089 1093 default:
1090 1094 PRINTF1("ERR *** in spw_send_asm *** unexpected sid %d\n", sid)
1091 1095 break;
1092 1096 }
1093 1097 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM + CCSDS_PROTOCOLE_EXTRA_BYTES;
1094 1098 spw_ioctl_send_ASM.hdr = (char *) header;
1095 1099 spw_ioctl_send_ASM.options = 0;
1096 1100
1097 1101 // (2) BUILD THE HEADER
1098 1102 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1099 1103 header->packetLength[0] = (unsigned char) (length>>8);
1100 1104 header->packetLength[1] = (unsigned char) (length);
1101 1105 header->sid = (unsigned char) sid; // SID
1102 1106 header->pa_lfr_pkt_cnt_asm = 2;
1103 1107 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1104 1108
1105 1109 // (3) SET PACKET TIME
1106 1110 header->time[0] = (unsigned char) (coarseTime>>24);
1107 1111 header->time[1] = (unsigned char) (coarseTime>>16);
1108 1112 header->time[2] = (unsigned char) (coarseTime>>8);
1109 1113 header->time[3] = (unsigned char) (coarseTime);
1110 1114 header->time[4] = (unsigned char) (fineTime>>8);
1111 1115 header->time[5] = (unsigned char) (fineTime);
1112 1116 //
1113 1117 header->acquisitionTime[0] = header->time[0];
1114 1118 header->acquisitionTime[1] = header->time[1];
1115 1119 header->acquisitionTime[2] = header->time[2];
1116 1120 header->acquisitionTime[3] = header->time[3];
1117 1121 header->acquisitionTime[4] = header->time[4];
1118 1122 header->acquisitionTime[5] = header->time[5];
1119 1123
1120 1124 // (4) SEND PACKET
1121 1125 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1122 1126 if (status != RTEMS_SUCCESSFUL) {
1123 1127 printf("in ASM_send *** ERR %d\n", (int) status);
1124 1128 }
1125 1129 }
1126 1130 }
1131
1132 void spw_send_k_dump( ring_node *ring_node_to_send )
1133 {
1134 rtems_status_code status;
1135 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump;
1136 unsigned int packetLength;
1137 unsigned int size;
1138
1139 printf("spw_send_k_dump\n");
1140
1141 kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address;
1142
1143 packetLength = kcoefficients_dump->packetLength[0] * 256 + kcoefficients_dump->packetLength[1];
1144
1145 size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
1146
1147 printf("packetLength %d, size %d\n", packetLength, size );
1148
1149 status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size );
1150
1151 if (status == -1){
1152 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
1153 }
1154
1155 ring_node_to_send->status = 0x00;
1156 }
Show file before | Show file after | Hide comments
@@ -1,437 +1,463
1 1 /** Functions related to TeleCommand acceptance.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TeleCommands parsing.\n
7 7 *
8 8 */
9 9
10 10 #include "tc_acceptance.h"
11 11
12 12 unsigned int lookUpTableForCRC[256];
13 13
14 14 //**********************
15 15 // GENERAL USE FUNCTIONS
16 16 unsigned int Crc_opt( unsigned char D, unsigned int Chk)
17 17 {
18 18 /** This function generate the CRC for one byte and returns the value of the new syndrome.
19 19 *
20 20 * @param D is the current byte of data.
21 21 * @param Chk is the current syndrom value.
22 22 *
23 23 * @return the value of the new syndrome on two bytes.
24 24 *
25 25 */
26 26
27 27 return(((Chk << 8) & 0xff00)^lookUpTableForCRC [(((Chk >> 8)^D) & 0x00ff)]);
28 28 }
29 29
30 30 void initLookUpTableForCRC( void )
31 31 {
32 32 /** This function is used to initiates the look-up table for fast CRC computation.
33 33 *
34 34 * The global table lookUpTableForCRC[256] is initiated.
35 35 *
36 36 */
37 37
38 38 unsigned int i;
39 39 unsigned int tmp;
40 40
41 41 for (i=0; i<256; i++)
42 42 {
43 43 tmp = 0;
44 44 if((i & 1) != 0) {
45 45 tmp = tmp ^ 0x1021;
46 46 }
47 47 if((i & 2) != 0) {
48 48 tmp = tmp ^ 0x2042;
49 49 }
50 50 if((i & 4) != 0) {
51 51 tmp = tmp ^ 0x4084;
52 52 }
53 53 if((i & 8) != 0) {
54 54 tmp = tmp ^ 0x8108;
55 55 }
56 56 if((i & 16) != 0) {
57 57 tmp = tmp ^ 0x1231;
58 58 }
59 59 if((i & 32) != 0) {
60 60 tmp = tmp ^ 0x2462;
61 61 }
62 62 if((i & 64) != 0) {
63 63 tmp = tmp ^ 0x48c4;
64 64 }
65 65 if((i & 128) != 0) {
66 66 tmp = tmp ^ 0x9188;
67 67 }
68 68 lookUpTableForCRC[i] = tmp;
69 69 }
70 70 }
71 71
72 72 void GetCRCAsTwoBytes(unsigned char* data, unsigned char* crcAsTwoBytes, unsigned int sizeOfData)
73 73 {
74 74 /** This function calculates a two bytes Cyclic Redundancy Code.
75 75 *
76 76 * @param data points to a buffer containing the data on which to compute the CRC.
77 77 * @param crcAsTwoBytes points points to a two bytes buffer in which the CRC is stored.
78 78 * @param sizeOfData is the number of bytes of *data* used to compute the CRC.
79 79 *
80 80 * The specification of the Cyclic Redundancy Code is described in the following document: ECSS-E-70-41-A.
81 81 *
82 82 */
83 83
84 84 unsigned int Chk;
85 85 int j;
86 86 Chk = 0xffff; // reset the syndrom to all ones
87 87 for (j=0; j<sizeOfData; j++) {
88 88 Chk = Crc_opt(data[j], Chk);
89 89 }
90 90 crcAsTwoBytes[0] = (unsigned char) (Chk >> 8);
91 91 crcAsTwoBytes[1] = (unsigned char) (Chk & 0x00ff);
92 92 }
93 93
94 94 //*********************
95 95 // ACCEPTANCE FUNCTIONS
96 96 int tc_parser(ccsdsTelecommandPacket_t * TCPacket, unsigned int estimatedPacketLength, unsigned char *computed_CRC)
97 97 {
98 98 /** This function parses TeleCommands.
99 99 *
100 100 * @param TC points to the TeleCommand that will be parsed.
101 101 * @param estimatedPacketLength is the PACKET_LENGTH field calculated from the effective length of the received packet.
102 102 *
103 103 * @return Status code of the parsing.
104 104 *
105 105 * The parsing checks:
106 106 * - process id
107 107 * - category
108 108 * - length: a global check is performed and a per subtype check also
109 109 * - type
110 110 * - subtype
111 111 * - crc
112 112 *
113 113 */
114 114
115 115 int status;
116 116 int status_crc;
117 117 unsigned char pid;
118 118 unsigned char category;
119 119 unsigned int packetLength;
120 120 unsigned char packetType;
121 121 unsigned char packetSubtype;
122 122 unsigned char sid;
123 123
124 124 status = CCSDS_TM_VALID;
125 125
126 126 // APID check *** APID on 2 bytes
127 127 pid = ((TCPacket->packetID[0] & 0x07)<<4) + ( (TCPacket->packetID[1]>>4) & 0x0f ); // PID = 11 *** 7 bits xxxxx210 7654xxxx
128 128 category = (TCPacket->packetID[1] & 0x0f); // PACKET_CATEGORY = 12 *** 4 bits xxxxxxxx xxxx3210
129 129 packetLength = (TCPacket->packetLength[0] * 256) + TCPacket->packetLength[1];
130 130 packetType = TCPacket->serviceType;
131 131 packetSubtype = TCPacket->serviceSubType;
132 132 sid = TCPacket->sourceID;
133 133
134 134 if ( pid != CCSDS_PROCESS_ID ) // CHECK THE PROCESS ID
135 135 {
136 136 status = ILLEGAL_APID;
137 137 }
138 138 if (status == CCSDS_TM_VALID) // CHECK THE CATEGORY
139 139 {
140 140 if ( category != CCSDS_PACKET_CATEGORY )
141 141 {
142 142 status = ILLEGAL_APID;
143 143 }
144 144 }
145 145 if (status == CCSDS_TM_VALID) // CHECK THE PACKET_LENGTH FIELD AND THE ESTIMATED PACKET_LENGTH COMPLIANCE
146 146 {
147 147 if (packetLength != estimatedPacketLength ) {
148 148 status = WRONG_LEN_PKT;
149 149 }
150 150 }
151 151 if (status == CCSDS_TM_VALID) // CHECK THAT THE PACKET DOES NOT EXCEED THE MAX SIZE
152 152 {
153 153 if ( packetLength >= CCSDS_TC_PKT_MAX_SIZE ) {
154 154 status = WRONG_LEN_PKT;
155 155 }
156 156 }
157 157 if (status == CCSDS_TM_VALID) // CHECK THE TYPE
158 158 {
159 159 status = tc_check_type( packetType );
160 160 }
161 161 if (status == CCSDS_TM_VALID) // CHECK THE SUBTYPE
162 162 {
163 163 status = tc_check_type_subtype( packetType, packetSubtype );
164 164 }
165 165 if (status == CCSDS_TM_VALID) // CHECK THE SID
166 166 {
167 167 status = tc_check_sid( sid );
168 168 }
169 169 if (status == CCSDS_TM_VALID) // CHECK THE SUBTYPE AND LENGTH COMPLIANCE
170 170 {
171 171 status = tc_check_length( packetSubtype, packetLength );
172 172 }
173 173 status_crc = tc_check_crc( TCPacket, estimatedPacketLength, computed_CRC );
174 174 if (status == CCSDS_TM_VALID ) // CHECK CRC
175 175 {
176 176 status = status_crc;
177 177 }
178 178
179 179 return status;
180 180 }
181 181
182 182 int tc_check_type( unsigned char packetType )
183 183 {
184 184 /** This function checks that the type of a TeleCommand is valid.
185 185 *
186 186 * @param packetType is the type to check.
187 187 *
188 188 * @return Status code CCSDS_TM_VALID or ILL_TYPE.
189 189 *
190 190 */
191 191
192 192 int status;
193 193
194 194 if ( (packetType == TC_TYPE_GEN) || (packetType == TC_TYPE_TIME))
195 195 {
196 196 status = CCSDS_TM_VALID;
197 197 }
198 198 else
199 199 {
200 200 status = ILL_TYPE;
201 201 }
202 202
203 203 return status;
204 204 }
205 205
206 206 int tc_check_type_subtype( unsigned char packetType, unsigned char packetSubType )
207 207 {
208 208 /** This function checks that the subtype of a TeleCommand is valid and coherent with the type.
209 209 *
210 210 * @param packetType is the type of the TC.
211 211 * @param packetSubType is the subtype to check.
212 212 *
213 213 * @return Status code CCSDS_TM_VALID or ILL_SUBTYPE.
214 214 *
215 215 */
216 216
217 217 int status;
218 218
219 219 switch(packetType)
220 220 {
221 221 case TC_TYPE_GEN:
222 222 if ( (packetSubType == TC_SUBTYPE_RESET)
223 223 || (packetSubType == TC_SUBTYPE_LOAD_COMM)
224 224 || (packetSubType == TC_SUBTYPE_LOAD_NORM) || (packetSubType == TC_SUBTYPE_LOAD_BURST)
225 225 || (packetSubType == TC_SUBTYPE_LOAD_SBM1) || (packetSubType == TC_SUBTYPE_LOAD_SBM2)
226 226 || (packetSubType == TC_SUBTYPE_DUMP)
227 227 || (packetSubType == TC_SUBTYPE_ENTER)
228 228 || (packetSubType == TC_SUBTYPE_UPDT_INFO)
229 || (packetSubType == TC_SUBTYPE_EN_CAL) || (packetSubType == TC_SUBTYPE_DIS_CAL) )
229 || (packetSubType == TC_SUBTYPE_EN_CAL) || (packetSubType == TC_SUBTYPE_DIS_CAL)
230 || (packetSubType == TC_SUBTYPE_LOAD_K) || (packetSubType == TC_SUBTYPE_DUMP_K)
231 || (packetSubType == TC_SUBTYPE_LOAD_FBINS) )
230 232 {
231 233 status = CCSDS_TM_VALID;
232 234 }
233 235 else
234 236 {
235 237 status = ILL_SUBTYPE;
236 238 }
237 239 break;
238 240
239 241 case TC_TYPE_TIME:
240 242 if (packetSubType == TC_SUBTYPE_UPDT_TIME)
241 243 {
242 244 status = CCSDS_TM_VALID;
243 245 }
244 246 else
245 247 {
246 248 status = ILL_SUBTYPE;
247 249 }
248 250 break;
249 251
250 252 default:
251 253 status = ILL_SUBTYPE;
252 254 break;
253 255 }
254 256
255 257 return status;
256 258 }
257 259
258 260 int tc_check_sid( unsigned char sid )
259 261 {
260 262 /** This function checks that the sid of a TeleCommand is valid.
261 263 *
262 264 * @param sid is the sid to check.
263 265 *
264 266 * @return Status code CCSDS_TM_VALID or CORRUPTED.
265 267 *
266 268 */
267 269
268 270 int status;
269 271
270 272 if ( (sid == SID_TC_MISSION_TIMELINE) || (sid == SID_TC_TC_SEQUENCES) || (sid == SID_TC_RECOVERY_ACTION_CMD)
271 273 || (sid == SID_TC_BACKUP_MISSION_TIMELINE)
272 274 || (sid == SID_TC_DIRECT_CMD) || (sid == SID_TC_SPARE_GRD_SRC1) || (sid == SID_TC_SPARE_GRD_SRC2)
273 275 || (sid == SID_TC_OBCP) || (sid == SID_TC_SYSTEM_CONTROL) || (sid == SID_TC_AOCS)
274 276 || (sid == SID_TC_RPW_INTERNAL))
275 277 {
276 278 status = CCSDS_TM_VALID;
277 279 }
278 280 else
279 281 {
280 282 status = WRONG_SRC_ID;
281 283 }
282 284
283 285 return status;
284 286 }
285 287
286 288 int tc_check_length( unsigned char packetSubType, unsigned int length )
287 289 {
288 290 /** This function checks that the subtype and the length are compliant.
289 291 *
290 292 * @param packetSubType is the subtype to check.
291 293 * @param length is the length to check.
292 294 *
293 295 * @return Status code CCSDS_TM_VALID or ILL_TYPE.
294 296 *
295 297 */
296 298
297 299 int status;
298 300
299 301 status = LFR_SUCCESSFUL;
300 302
301 303 switch(packetSubType)
302 304 {
303 305 case TC_SUBTYPE_RESET:
304 306 if (length!=(TC_LEN_RESET-CCSDS_TC_TM_PACKET_OFFSET)) {
305 307 status = WRONG_LEN_PKT;
306 308 }
307 309 else {
308 310 status = CCSDS_TM_VALID;
309 311 }
310 312 break;
311 313 case TC_SUBTYPE_LOAD_COMM:
312 314 if (length!=(TC_LEN_LOAD_COMM-CCSDS_TC_TM_PACKET_OFFSET)) {
313 315 status = WRONG_LEN_PKT;
314 316 }
315 317 else {
316 318 status = CCSDS_TM_VALID;
317 319 }
318 320 break;
319 321 case TC_SUBTYPE_LOAD_NORM:
320 322 if (length!=(TC_LEN_LOAD_NORM-CCSDS_TC_TM_PACKET_OFFSET)) {
321 323 status = WRONG_LEN_PKT;
322 324 }
323 325 else {
324 326 status = CCSDS_TM_VALID;
325 327 }
326 328 break;
327 329 case TC_SUBTYPE_LOAD_BURST:
328 330 if (length!=(TC_LEN_LOAD_BURST-CCSDS_TC_TM_PACKET_OFFSET)) {
329 331 status = WRONG_LEN_PKT;
330 332 }
331 333 else {
332 334 status = CCSDS_TM_VALID;
333 335 }
334 336 break;
335 337 case TC_SUBTYPE_LOAD_SBM1:
336 338 if (length!=(TC_LEN_LOAD_SBM1-CCSDS_TC_TM_PACKET_OFFSET)) {
337 339 status = WRONG_LEN_PKT;
338 340 }
339 341 else {
340 342 status = CCSDS_TM_VALID;
341 343 }
342 344 break;
343 345 case TC_SUBTYPE_LOAD_SBM2:
344 346 if (length!=(TC_LEN_LOAD_SBM2-CCSDS_TC_TM_PACKET_OFFSET)) {
345 347 status = WRONG_LEN_PKT;
346 348 }
347 349 else {
348 350 status = CCSDS_TM_VALID;
349 351 }
350 352 break;
351 353 case TC_SUBTYPE_DUMP:
352 354 if (length!=(TC_LEN_DUMP-CCSDS_TC_TM_PACKET_OFFSET)) {
353 355 status = WRONG_LEN_PKT;
354 356 }
355 357 else {
356 358 status = CCSDS_TM_VALID;
357 359 }
358 360 break;
359 361 case TC_SUBTYPE_ENTER:
360 362 if (length!=(TC_LEN_ENTER-CCSDS_TC_TM_PACKET_OFFSET)) {
361 363 status = WRONG_LEN_PKT;
362 364 }
363 365 else {
364 366 status = CCSDS_TM_VALID;
365 367 }
366 368 break;
367 369 case TC_SUBTYPE_UPDT_INFO:
368 370 if (length!=(TC_LEN_UPDT_INFO-CCSDS_TC_TM_PACKET_OFFSET)) {
369 371 status = WRONG_LEN_PKT;
370 372 }
371 373 else {
372 374 status = CCSDS_TM_VALID;
373 375 }
374 376 break;
375 377 case TC_SUBTYPE_EN_CAL:
376 378 if (length!=(TC_LEN_EN_CAL-CCSDS_TC_TM_PACKET_OFFSET)) {
377 379 status = WRONG_LEN_PKT;
378 380 }
379 381 else {
380 382 status = CCSDS_TM_VALID;
381 383 }
382 384 break;
383 385 case TC_SUBTYPE_DIS_CAL:
384 386 if (length!=(TC_LEN_DIS_CAL-CCSDS_TC_TM_PACKET_OFFSET)) {
385 387 status = WRONG_LEN_PKT;
386 388 }
387 389 else {
388 390 status = CCSDS_TM_VALID;
389 391 }
390 392 break;
393 case TC_SUBTYPE_LOAD_K:
394 if (length!=(TC_LEN_LOAD_K-CCSDS_TC_TM_PACKET_OFFSET)) {
395 status = WRONG_LEN_PKT;
396 }
397 else {
398 status = CCSDS_TM_VALID;
399 }
400 break;
401 case TC_SUBTYPE_DUMP_K:
402 if (length!=(TC_LEN_DUMP_K-CCSDS_TC_TM_PACKET_OFFSET)) {
403 status = WRONG_LEN_PKT;
404 }
405 else {
406 status = CCSDS_TM_VALID;
407 }
408 break;
409 case TC_SUBTYPE_LOAD_FBINS:
410 if (length!=(TC_LEN_LOAD_FBINS-CCSDS_TC_TM_PACKET_OFFSET)) {
411 status = WRONG_LEN_PKT;
412 }
413 else {
414 status = CCSDS_TM_VALID;
415 }
416 break;
391 417 case TC_SUBTYPE_UPDT_TIME:
392 418 if (length!=(TC_LEN_UPDT_TIME-CCSDS_TC_TM_PACKET_OFFSET)) {
393 419 status = WRONG_LEN_PKT;
394 420 }
395 421 else {
396 422 status = CCSDS_TM_VALID;
397 423 }
398 424 break;
399 425 default: // if the subtype is not a legal value, return ILL_SUBTYPE
400 426 status = ILL_SUBTYPE;
401 427 break ;
402 428 }
403 429
404 430 return status;
405 431 }
406 432
407 433 int tc_check_crc( ccsdsTelecommandPacket_t * TCPacket, unsigned int length, unsigned char *computed_CRC )
408 434 {
409 435 /** This function checks the CRC validity of the corresponding TeleCommand packet.
410 436 *
411 437 * @param TCPacket points to the TeleCommand packet to check.
412 438 * @param length is the length of the TC packet.
413 439 *
414 440 * @return Status code CCSDS_TM_VALID or INCOR_CHECKSUM.
415 441 *
416 442 */
417 443
418 444 int status;
419 445 unsigned char * CCSDSContent;
420 446
421 447 CCSDSContent = (unsigned char*) TCPacket->packetID;
422 448 GetCRCAsTwoBytes(CCSDSContent, computed_CRC, length + CCSDS_TC_TM_PACKET_OFFSET - 2); // 2 CRC bytes removed from the calculation of the CRC
423 449 if (computed_CRC[0] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -2]) {
424 450 status = INCOR_CHECKSUM;
425 451 }
426 452 else if (computed_CRC[1] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -1]) {
427 453 status = INCOR_CHECKSUM;
428 454 }
429 455 else {
430 456 status = CCSDS_TM_VALID;
431 457 }
432 458
433 459 return status;
434 460 }
435 461
436 462
437 463
Show file before | Show file after | Hide comments
@@ -1,1133 +1,1121
1 1 /** Functions and tasks related to TeleCommand handling.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TeleCommands:\n
7 7 * action launching\n
8 8 * TC parsing\n
9 9 * ...
10 10 *
11 11 */
12 12
13 13 #include "tc_handler.h"
14 14 #include "math.h"
15 15
16 16 //***********
17 17 // RTEMS TASK
18 18
19 19 rtems_task actn_task( rtems_task_argument unused )
20 20 {
21 21 /** This RTEMS task is responsible for launching actions upton the reception of valid TeleCommands.
22 22 *
23 23 * @param unused is the starting argument of the RTEMS task
24 24 *
25 25 * The ACTN task waits for data coming from an RTEMS msesage queue. When data arrives, it launches specific actions depending
26 26 * on the incoming TeleCommand.
27 27 *
28 28 */
29 29
30 30 int result;
31 31 rtems_status_code status; // RTEMS status code
32 32 ccsdsTelecommandPacket_t TC; // TC sent to the ACTN task
33 33 size_t size; // size of the incoming TC packet
34 34 unsigned char subtype; // subtype of the current TC packet
35 35 unsigned char time[6];
36 36 rtems_id queue_rcv_id;
37 37 rtems_id queue_snd_id;
38 38
39 39 status = get_message_queue_id_recv( &queue_rcv_id );
40 40 if (status != RTEMS_SUCCESSFUL)
41 41 {
42 42 PRINTF1("in ACTN *** ERR get_message_queue_id_recv %d\n", status)
43 43 }
44 44
45 45 status = get_message_queue_id_send( &queue_snd_id );
46 46 if (status != RTEMS_SUCCESSFUL)
47 47 {
48 48 PRINTF1("in ACTN *** ERR get_message_queue_id_send %d\n", status)
49 49 }
50 50
51 51 result = LFR_SUCCESSFUL;
52 52 subtype = 0; // subtype of the current TC packet
53 53
54 54 BOOT_PRINTF("in ACTN *** \n")
55 55
56 56 while(1)
57 57 {
58 58 status = rtems_message_queue_receive( queue_rcv_id, (char*) &TC, &size,
59 59 RTEMS_WAIT, RTEMS_NO_TIMEOUT);
60 60 getTime( time ); // set time to the current time
61 61 if (status!=RTEMS_SUCCESSFUL)
62 62 {
63 63 PRINTF1("ERR *** in task ACTN *** error receiving a message, code %d \n", status)
64 64 }
65 65 else
66 66 {
67 67 subtype = TC.serviceSubType;
68 68 switch(subtype)
69 69 {
70 70 case TC_SUBTYPE_RESET:
71 71 result = action_reset( &TC, queue_snd_id, time );
72 72 close_action( &TC, result, queue_snd_id );
73 73 break;
74 74 case TC_SUBTYPE_LOAD_COMM:
75 75 result = action_load_common_par( &TC );
76 76 close_action( &TC, result, queue_snd_id );
77 77 break;
78 78 case TC_SUBTYPE_LOAD_NORM:
79 79 result = action_load_normal_par( &TC, queue_snd_id, time );
80 80 close_action( &TC, result, queue_snd_id );
81 81 break;
82 82 case TC_SUBTYPE_LOAD_BURST:
83 83 result = action_load_burst_par( &TC, queue_snd_id, time );
84 84 close_action( &TC, result, queue_snd_id );
85 85 break;
86 86 case TC_SUBTYPE_LOAD_SBM1:
87 87 result = action_load_sbm1_par( &TC, queue_snd_id, time );
88 88 close_action( &TC, result, queue_snd_id );
89 89 break;
90 90 case TC_SUBTYPE_LOAD_SBM2:
91 91 result = action_load_sbm2_par( &TC, queue_snd_id, time );
92 92 close_action( &TC, result, queue_snd_id );
93 93 break;
94 94 case TC_SUBTYPE_DUMP:
95 95 result = action_dump_par( queue_snd_id );
96 96 close_action( &TC, result, queue_snd_id );
97 97 break;
98 98 case TC_SUBTYPE_ENTER:
99 99 result = action_enter_mode( &TC, queue_snd_id );
100 100 close_action( &TC, result, queue_snd_id );
101 101 break;
102 102 case TC_SUBTYPE_UPDT_INFO:
103 103 result = action_update_info( &TC, queue_snd_id );
104 104 close_action( &TC, result, queue_snd_id );
105 105 break;
106 106 case TC_SUBTYPE_EN_CAL:
107 107 result = action_enable_calibration( &TC, queue_snd_id, time );
108 108 close_action( &TC, result, queue_snd_id );
109 109 break;
110 110 case TC_SUBTYPE_DIS_CAL:
111 111 result = action_disable_calibration( &TC, queue_snd_id, time );
112 112 close_action( &TC, result, queue_snd_id );
113 113 break;
114 114 case TC_SUBTYPE_LOAD_K:
115 printf("TC_SUBTYPE_LOAD_K\n");
115 116 result = action_load_kcoefficients( &TC, queue_snd_id, time );
116 117 close_action( &TC, result, queue_snd_id );
117 118 break;
118 119 case TC_SUBTYPE_DUMP_K:
119 120 result = action_dump_kcoefficients( &TC, queue_snd_id, time );
120 121 close_action( &TC, result, queue_snd_id );
121 122 break;
122 123 case TC_SUBTYPE_LOAD_FBINS:
123 124 result = action_load_fbins_mask( &TC, queue_snd_id, time );
124 125 close_action( &TC, result, queue_snd_id );
125 126 break;
126 127 case TC_SUBTYPE_UPDT_TIME:
127 128 result = action_update_time( &TC );
128 129 close_action( &TC, result, queue_snd_id );
129 130 break;
130 131 default:
131 132 break;
132 133 }
133 134 }
134 135 }
135 136 }
136 137
137 138 //***********
138 139 // TC ACTIONS
139 140
140 141 int action_reset(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
141 142 {
142 143 /** This function executes specific actions when a TC_LFR_RESET TeleCommand has been received.
143 144 *
144 145 * @param TC points to the TeleCommand packet that is being processed
145 146 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
146 147 *
147 148 */
148 149
149 150 printf("this is the end!!!\n");
150 151 exit(0);
151 152 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
152 153 return LFR_DEFAULT;
153 154 }
154 155
155 156 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
156 157 {
157 158 /** This function executes specific actions when a TC_LFR_ENTER_MODE TeleCommand has been received.
158 159 *
159 160 * @param TC points to the TeleCommand packet that is being processed
160 161 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
161 162 *
162 163 */
163 164
164 165 rtems_status_code status;
165 166 unsigned char requestedMode;
166 167 unsigned int *transitionCoarseTime_ptr;
167 168 unsigned int transitionCoarseTime;
168 169 unsigned char * bytePosPtr;
169 170
170 printTaskID();
171
172 171 bytePosPtr = (unsigned char *) &TC->packetID;
173 172
174 173 requestedMode = bytePosPtr[ BYTE_POS_CP_MODE_LFR_SET ];
175 174 transitionCoarseTime_ptr = (unsigned int *) ( &bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME ] );
176 175 transitionCoarseTime = (*transitionCoarseTime_ptr) & 0x7fffffff;
177 176
178 177 status = check_mode_value( requestedMode );
179 178
180 179 if ( status != LFR_SUCCESSFUL ) // the mode value is inconsistent
181 180 {
182 181 send_tm_lfr_tc_exe_inconsistent( TC, queue_id, BYTE_POS_CP_MODE_LFR_SET, requestedMode );
183 182 }
184 183 else // the mode value is consistent, check the transition
185 184 {
186 185 status = check_mode_transition(requestedMode);
187 186 if (status != LFR_SUCCESSFUL)
188 187 {
189 188 PRINTF("ERR *** in action_enter_mode *** check_mode_transition\n")
190 189 send_tm_lfr_tc_exe_not_executable( TC, queue_id );
191 190 }
192 191 }
193 192
194 193 if ( status == LFR_SUCCESSFUL ) // the transition is valid, enter the mode
195 194 {
196 195 status = check_transition_date( transitionCoarseTime );
197 196 if (status != LFR_SUCCESSFUL)
198 197 {
199 198 PRINTF("ERR *** in action_enter_mode *** check_transition_date\n")
200 199 send_tm_lfr_tc_exe_inconsistent( TC, queue_id,
201 200 BYTE_POS_CP_LFR_ENTER_MODE_TIME,
202 201 bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME + 3 ] );
203 202 }
204 203 }
205 204
206 205 if ( status == LFR_SUCCESSFUL ) // the date is valid, enter the mode
207 206 {
208 207 PRINTF1("OK *** in action_enter_mode *** enter mode %d\n", requestedMode);
209 208 status = enter_mode( requestedMode, transitionCoarseTime );
210 209 }
211 210
212 211 return status;
213 212 }
214 213
215 214 int action_update_info(ccsdsTelecommandPacket_t *TC, rtems_id queue_id)
216 215 {
217 216 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
218 217 *
219 218 * @param TC points to the TeleCommand packet that is being processed
220 219 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
221 220 *
222 221 * @return LFR directive status code:
223 222 * - LFR_DEFAULT
224 223 * - LFR_SUCCESSFUL
225 224 *
226 225 */
227 226
228 227 unsigned int val;
229 228 int result;
230 229 unsigned int status;
231 230 unsigned char mode;
232 231 unsigned char * bytePosPtr;
233 232
234 233 bytePosPtr = (unsigned char *) &TC->packetID;
235 234
236 235 // check LFR mode
237 236 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET5 ] & 0x1e) >> 1;
238 237 status = check_update_info_hk_lfr_mode( mode );
239 238 if (status == LFR_SUCCESSFUL) // check TDS mode
240 239 {
241 240 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0xf0) >> 4;
242 241 status = check_update_info_hk_tds_mode( mode );
243 242 }
244 243 if (status == LFR_SUCCESSFUL) // check THR mode
245 244 {
246 245 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0x0f);
247 246 status = check_update_info_hk_thr_mode( mode );
248 247 }
249 248 if (status == LFR_SUCCESSFUL) // if the parameter check is successful
250 249 {
251 250 val = housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * 256
252 251 + housekeeping_packet.hk_lfr_update_info_tc_cnt[1];
253 252 val++;
254 253 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> 8);
255 254 housekeeping_packet.hk_lfr_update_info_tc_cnt[1] = (unsigned char) (val);
256 255 }
257 256
258 257 result = status;
259 258
260 259 return result;
261 260 }
262 261
263 262 int action_enable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
264 263 {
265 264 /** This function executes specific actions when a TC_LFR_ENABLE_CALIBRATION TeleCommand has been received.
266 265 *
267 266 * @param TC points to the TeleCommand packet that is being processed
268 267 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
269 268 *
270 269 */
271 270
272 271 int result;
273 272
274 273 result = LFR_DEFAULT;
275 274
276 275 startCalibration();
277 276
278 277 result = LFR_SUCCESSFUL;
279 278
280 279 return result;
281 280 }
282 281
283 282 int action_disable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
284 283 {
285 284 /** This function executes specific actions when a TC_LFR_DISABLE_CALIBRATION TeleCommand has been received.
286 285 *
287 286 * @param TC points to the TeleCommand packet that is being processed
288 287 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
289 288 *
290 289 */
291 290
292 291 int result;
293 292
294 293 result = LFR_DEFAULT;
295 294
296 295 stopCalibration();
297 296
298 297 result = LFR_SUCCESSFUL;
299 298
300 299 return result;
301 300 }
302 301
303 302 int action_update_time(ccsdsTelecommandPacket_t *TC)
304 303 {
305 304 /** This function executes specific actions when a TC_LFR_UPDATE_TIME TeleCommand has been received.
306 305 *
307 306 * @param TC points to the TeleCommand packet that is being processed
308 307 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
309 308 *
310 309 * @return LFR_SUCCESSFUL
311 310 *
312 311 */
313 312
314 313 unsigned int val;
315 314
316 315 time_management_regs->coarse_time_load = (TC->dataAndCRC[0] << 24)
317 316 + (TC->dataAndCRC[1] << 16)
318 317 + (TC->dataAndCRC[2] << 8)
319 318 + TC->dataAndCRC[3];
320 319
321 320 val = housekeeping_packet.hk_lfr_update_time_tc_cnt[0] * 256
322 321 + housekeeping_packet.hk_lfr_update_time_tc_cnt[1];
323 322 val++;
324 323 housekeeping_packet.hk_lfr_update_time_tc_cnt[0] = (unsigned char) (val >> 8);
325 324 housekeeping_packet.hk_lfr_update_time_tc_cnt[1] = (unsigned char) (val);
326 325
327 326 return LFR_SUCCESSFUL;
328 327 }
329 328
330 329 //*******************
331 330 // ENTERING THE MODES
332 331 int check_mode_value( unsigned char requestedMode )
333 332 {
334 333 int status;
335 334
336 335 if ( (requestedMode != LFR_MODE_STANDBY)
337 336 && (requestedMode != LFR_MODE_NORMAL) && (requestedMode != LFR_MODE_BURST)
338 337 && (requestedMode != LFR_MODE_SBM1) && (requestedMode != LFR_MODE_SBM2) )
339 338 {
340 339 status = LFR_DEFAULT;
341 340 }
342 341 else
343 342 {
344 343 status = LFR_SUCCESSFUL;
345 344 }
346 345
347 346 return status;
348 347 }
349 348
350 349 int check_mode_transition( unsigned char requestedMode )
351 350 {
352 351 /** This function checks the validity of the transition requested by the TC_LFR_ENTER_MODE.
353 352 *
354 353 * @param requestedMode is the mode requested by the TC_LFR_ENTER_MODE
355 354 *
356 355 * @return LFR directive status codes:
357 356 * - LFR_SUCCESSFUL - the transition is authorized
358 357 * - LFR_DEFAULT - the transition is not authorized
359 358 *
360 359 */
361 360
362 361 int status;
363 362
364 363 switch (requestedMode)
365 364 {
366 365 case LFR_MODE_STANDBY:
367 366 if ( lfrCurrentMode == LFR_MODE_STANDBY ) {
368 367 status = LFR_DEFAULT;
369 368 }
370 369 else
371 370 {
372 371 status = LFR_SUCCESSFUL;
373 372 }
374 373 break;
375 374 case LFR_MODE_NORMAL:
376 375 if ( lfrCurrentMode == LFR_MODE_NORMAL ) {
377 376 status = LFR_DEFAULT;
378 377 }
379 378 else {
380 379 status = LFR_SUCCESSFUL;
381 380 }
382 381 break;
383 382 case LFR_MODE_BURST:
384 383 if ( lfrCurrentMode == LFR_MODE_BURST ) {
385 384 status = LFR_DEFAULT;
386 385 }
387 386 else {
388 387 status = LFR_SUCCESSFUL;
389 388 }
390 389 break;
391 390 case LFR_MODE_SBM1:
392 391 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
393 392 status = LFR_DEFAULT;
394 393 }
395 394 else {
396 395 status = LFR_SUCCESSFUL;
397 396 }
398 397 break;
399 398 case LFR_MODE_SBM2:
400 399 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
401 400 status = LFR_DEFAULT;
402 401 }
403 402 else {
404 403 status = LFR_SUCCESSFUL;
405 404 }
406 405 break;
407 406 default:
408 407 status = LFR_DEFAULT;
409 408 break;
410 409 }
411 410
412 411 return status;
413 412 }
414 413
415 414 int check_transition_date( unsigned int transitionCoarseTime )
416 415 {
417 416 int status;
418 417 unsigned int localCoarseTime;
419 418 unsigned int deltaCoarseTime;
420 419
421 420 status = LFR_SUCCESSFUL;
422 421
423 422 if (transitionCoarseTime == 0) // transition time = 0 means an instant transition
424 423 {
425 424 status = LFR_SUCCESSFUL;
426 425 }
427 426 else
428 427 {
429 428 localCoarseTime = time_management_regs->coarse_time & 0x7fffffff;
430 429
431 430 PRINTF2("localTime = %x, transitionTime = %x\n", localCoarseTime, transitionCoarseTime)
432 431
433 432 if ( transitionCoarseTime <= localCoarseTime ) // SSS-CP-EQS-322
434 433 {
435 434 status = LFR_DEFAULT;
436 435 PRINTF("ERR *** in check_transition_date *** transitionCoarseTime <= localCoarseTime\n")
437 436 }
438 437
439 438 if (status == LFR_SUCCESSFUL)
440 439 {
441 440 deltaCoarseTime = transitionCoarseTime - localCoarseTime;
442 441 if ( deltaCoarseTime > 3 ) // SSS-CP-EQS-323
443 442 {
444 443 status = LFR_DEFAULT;
445 444 PRINTF1("ERR *** in check_transition_date *** deltaCoarseTime = %x\n", deltaCoarseTime)
446 445 }
447 446 }
448 447 }
449 448
450 449 return status;
451 450 }
452 451
453 452 int stop_current_mode( void )
454 453 {
455 454 /** This function stops the current mode by masking interrupt lines and suspending science tasks.
456 455 *
457 456 * @return RTEMS directive status codes:
458 457 * - RTEMS_SUCCESSFUL - task restarted successfully
459 458 * - RTEMS_INVALID_ID - task id invalid
460 459 * - RTEMS_ALREADY_SUSPENDED - task already suspended
461 460 *
462 461 */
463 462
464 463 rtems_status_code status;
465 464
466 465 status = RTEMS_SUCCESSFUL;
467 466
468 467 // (1) mask interruptions
469 468 LEON_Mask_interrupt( IRQ_WAVEFORM_PICKER ); // mask waveform picker interrupt
470 469 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
471 470
472 471 // (2) reset waveform picker registers
473 472 reset_wfp_burst_enable(); // reset burst and enable bits
474 473 reset_wfp_status(); // reset all the status bits
475 474
476 475 // (3) reset spectral matrices registers
477 476 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
478 477 reset_sm_status();
479 478
480 479 // reset lfr VHDL module
481 480 reset_lfr();
482 481
483 482 reset_extractSWF(); // reset the extractSWF flag to false
484 483
485 484 // (4) clear interruptions
486 485 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER ); // clear waveform picker interrupt
487 486 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
488 487
489 488 // <Spectral Matrices simulator>
490 489 LEON_Mask_interrupt( IRQ_SM_SIMULATOR ); // mask spectral matrix interrupt simulator
491 490 timer_stop( (gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR );
492 491 LEON_Clear_interrupt( IRQ_SM_SIMULATOR ); // clear spectral matrix interrupt simulator
493 492 // </Spectral Matrices simulator>
494 493
495 494 // suspend several tasks
496 495 if (lfrCurrentMode != LFR_MODE_STANDBY) {
497 496 status = suspend_science_tasks();
498 497 }
499 498
500 499 if (status != RTEMS_SUCCESSFUL)
501 500 {
502 501 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
503 502 }
504 503
505 504 return status;
506 505 }
507 506
508 507 int enter_mode( unsigned char mode, unsigned int transitionCoarseTime )
509 508 {
510 509 /** This function is launched after a mode transition validation.
511 510 *
512 511 * @param mode is the mode in which LFR will be put.
513 512 *
514 513 * @return RTEMS directive status codes:
515 514 * - RTEMS_SUCCESSFUL - the mode has been entered successfully
516 515 * - RTEMS_NOT_SATISFIED - the mode has not been entered successfully
517 516 *
518 517 */
519 518
520 519 rtems_status_code status;
521 520
522 521 //**********************
523 522 // STOP THE CURRENT MODE
524 523 status = stop_current_mode();
525 524 if (status != RTEMS_SUCCESSFUL)
526 525 {
527 526 PRINTF1("ERR *** in enter_mode *** stop_current_mode with mode = %d\n", mode)
528 527 }
529 528
530 529 //*************************
531 530 // ENTER THE REQUESTED MODE
532 531 if ( (mode == LFR_MODE_NORMAL) || (mode == LFR_MODE_BURST)
533 532 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2) )
534 533 {
535 534 #ifdef PRINT_TASK_STATISTICS
536 535 rtems_cpu_usage_reset();
537 536 maxCount = 0;
538 537 #endif
539 538 status = restart_science_tasks( mode );
540 539 launch_spectral_matrix( );
541 540 launch_waveform_picker( mode, transitionCoarseTime );
542 541 // launch_spectral_matrix_simu( );
543 542 }
544 543 else if ( mode == LFR_MODE_STANDBY )
545 544 {
546 545 #ifdef PRINT_TASK_STATISTICS
547 546 rtems_cpu_usage_report();
548 547 #endif
549 548
550 549 #ifdef PRINT_STACK_REPORT
551 550 PRINTF("stack report selected\n")
552 551 rtems_stack_checker_report_usage();
553 552 #endif
554 553 PRINTF1("maxCount = %d\n", maxCount)
555 554 }
556 555 else
557 556 {
558 557 status = RTEMS_UNSATISFIED;
559 558 }
560 559
561 560 if (status != RTEMS_SUCCESSFUL)
562 561 {
563 562 PRINTF1("ERR *** in enter_mode *** status = %d\n", status)
564 563 status = RTEMS_UNSATISFIED;
565 564 }
566 565
567 566 return status;
568 567 }
569 568
570 569 int restart_science_tasks(unsigned char lfrRequestedMode )
571 570 {
572 571 /** This function is used to restart all science tasks.
573 572 *
574 573 * @return RTEMS directive status codes:
575 574 * - RTEMS_SUCCESSFUL - task restarted successfully
576 575 * - RTEMS_INVALID_ID - task id invalid
577 576 * - RTEMS_INCORRECT_STATE - task never started
578 577 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
579 578 *
580 579 * Science tasks are AVF0, PRC0, WFRM, CWF3, CW2, CWF1
581 580 *
582 581 */
583 582
584 583 rtems_status_code status[10];
585 584 rtems_status_code ret;
586 585
587 586 ret = RTEMS_SUCCESSFUL;
588 587
589 588 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
590 589 if (status[0] != RTEMS_SUCCESSFUL)
591 590 {
592 591 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
593 592 }
594 593
595 594 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
596 595 if (status[1] != RTEMS_SUCCESSFUL)
597 596 {
598 597 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
599 598 }
600 599
601 600 status[2] = rtems_task_restart( Task_id[TASKID_WFRM],1 );
602 601 if (status[2] != RTEMS_SUCCESSFUL)
603 602 {
604 603 PRINTF1("in restart_science_task *** WFRM ERR %d\n", status[2])
605 604 }
606 605
607 606 status[3] = rtems_task_restart( Task_id[TASKID_CWF3],1 );
608 607 if (status[3] != RTEMS_SUCCESSFUL)
609 608 {
610 609 PRINTF1("in restart_science_task *** CWF3 ERR %d\n", status[3])
611 610 }
612 611
613 612 status[4] = rtems_task_restart( Task_id[TASKID_CWF2],1 );
614 613 if (status[4] != RTEMS_SUCCESSFUL)
615 614 {
616 615 PRINTF1("in restart_science_task *** CWF2 ERR %d\n", status[4])
617 616 }
618 617
619 618 status[5] = rtems_task_restart( Task_id[TASKID_CWF1],1 );
620 619 if (status[5] != RTEMS_SUCCESSFUL)
621 620 {
622 621 PRINTF1("in restart_science_task *** CWF1 ERR %d\n", status[5])
623 622 }
624 623
625 624 status[6] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
626 625 if (status[6] != RTEMS_SUCCESSFUL)
627 626 {
628 627 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[6])
629 628 }
630 629
631 630 status[7] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
632 631 if (status[7] != RTEMS_SUCCESSFUL)
633 632 {
634 633 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[7])
635 634 }
636 635
637 636 status[8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
638 637 if (status[8] != RTEMS_SUCCESSFUL)
639 638 {
640 639 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[8])
641 640 }
642 641
643 642 status[9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
644 643 if (status[9] != RTEMS_SUCCESSFUL)
645 644 {
646 645 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[9])
647 646 }
648 647
649 648 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
650 649 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
651 650 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) ||
652 651 (status[6] != RTEMS_SUCCESSFUL) || (status[7] != RTEMS_SUCCESSFUL) ||
653 652 (status[8] != RTEMS_SUCCESSFUL) || (status[9] != RTEMS_SUCCESSFUL) )
654 653 {
655 654 ret = RTEMS_UNSATISFIED;
656 655 }
657 656
658 657 return ret;
659 658 }
660 659
661 660 int suspend_science_tasks()
662 661 {
663 662 /** This function suspends the science tasks.
664 663 *
665 664 * @return RTEMS directive status codes:
666 665 * - RTEMS_SUCCESSFUL - task restarted successfully
667 666 * - RTEMS_INVALID_ID - task id invalid
668 667 * - RTEMS_ALREADY_SUSPENDED - task already suspended
669 668 *
670 669 */
671 670
672 671 rtems_status_code status;
673 672
674 673 printf("in suspend_science_tasks\n");
675 printTaskID();
676 674
677 675 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
678 676 if (status != RTEMS_SUCCESSFUL)
679 677 {
680 678 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
681 679 }
682 680 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
683 681 {
684 682 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
685 683 if (status != RTEMS_SUCCESSFUL)
686 684 {
687 685 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
688 686 }
689 687 }
690 688 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
691 689 {
692 690 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
693 691 if (status != RTEMS_SUCCESSFUL)
694 692 {
695 693 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
696 694 }
697 695 }
698 696 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
699 697 {
700 698 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
701 699 if (status != RTEMS_SUCCESSFUL)
702 700 {
703 701 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
704 702 }
705 703 }
706 704 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
707 705 {
708 706 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
709 707 if (status != RTEMS_SUCCESSFUL)
710 708 {
711 709 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
712 710 }
713 711 }
714 712 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
715 713 {
716 714 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
717 715 if (status != RTEMS_SUCCESSFUL)
718 716 {
719 717 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
720 718 }
721 719 }
722 720 if (status == RTEMS_SUCCESSFUL) // suspend WFRM
723 721 {
724 722 status = rtems_task_suspend( Task_id[TASKID_WFRM] );
725 723 if (status != RTEMS_SUCCESSFUL)
726 724 {
727 725 PRINTF1("in suspend_science_task *** WFRM ERR %d\n", status)
728 726 }
729 727 }
730 728 if (status == RTEMS_SUCCESSFUL) // suspend CWF3
731 729 {
732 730 status = rtems_task_suspend( Task_id[TASKID_CWF3] );
733 731 if (status != RTEMS_SUCCESSFUL)
734 732 {
735 733 PRINTF1("in suspend_science_task *** CWF3 ERR %d\n", status)
736 734 }
737 735 }
738 736 if (status == RTEMS_SUCCESSFUL) // suspend CWF2
739 737 {
740 738 status = rtems_task_suspend( Task_id[TASKID_CWF2] );
741 739 if (status != RTEMS_SUCCESSFUL)
742 740 {
743 741 PRINTF1("in suspend_science_task *** CWF2 ERR %d\n", status)
744 742 }
745 743 }
746 744 if (status == RTEMS_SUCCESSFUL) // suspend CWF1
747 745 {
748 746 status = rtems_task_suspend( Task_id[TASKID_CWF1] );
749 747 if (status != RTEMS_SUCCESSFUL)
750 748 {
751 749 PRINTF1("in suspend_science_task *** CWF1 ERR %d\n", status)
752 750 }
753 751 }
754 752
755 753 return status;
756 754 }
757 755
758 756 void launch_waveform_picker( unsigned char mode, unsigned int transitionCoarseTime )
759 757 {
760 758 WFP_reset_current_ring_nodes();
761 759
762 760 reset_waveform_picker_regs();
763 761
764 762 set_wfp_burst_enable_register( mode );
765 763
766 764 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER );
767 765 LEON_Unmask_interrupt( IRQ_WAVEFORM_PICKER );
768 766
769 767 if (transitionCoarseTime == 0)
770 768 {
771 769 waveform_picker_regs->start_date = time_management_regs->coarse_time;
772 770 }
773 771 else
774 772 {
775 773 waveform_picker_regs->start_date = transitionCoarseTime;
776 774 }
777 775
778 776 }
779 777
780 778 void launch_spectral_matrix( void )
781 779 {
782 780 SM_reset_current_ring_nodes();
783 781
784 782 reset_spectral_matrix_regs();
785 783
786 784 reset_nb_sm();
787 785
788 786 set_sm_irq_onNewMatrix( 1 );
789 787
790 788 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX );
791 789 LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRIX );
792 790
793 791 }
794 792
795 793 void launch_spectral_matrix_simu( void )
796 794 {
797 795 SM_reset_current_ring_nodes();
798 796 reset_spectral_matrix_regs();
799 797 reset_nb_sm();
800 798
801 799 // Spectral Matrices simulator
802 800 timer_start( (gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR );
803 801 LEON_Clear_interrupt( IRQ_SM_SIMULATOR );
804 802 LEON_Unmask_interrupt( IRQ_SM_SIMULATOR );
805 803 }
806 804
807 805 void set_sm_irq_onNewMatrix( unsigned char value )
808 806 {
809 807 if (value == 1)
810 808 {
811 809 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x01;
812 810 }
813 811 else
814 812 {
815 813 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffe; // 1110
816 814 }
817 815 }
818 816
819 817 void set_sm_irq_onError( unsigned char value )
820 818 {
821 819 if (value == 1)
822 820 {
823 821 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x02;
824 822 }
825 823 else
826 824 {
827 825 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffd; // 1101
828 826 }
829 827 }
830 828
831 829 //*****************************
832 830 // CONFIGURE CALIBRATION SIGNAL
833 831 void setCalibrationPrescaler( unsigned int prescaler )
834 832 {
835 833 // prescaling of the master clock (25 MHz)
836 834 // master clock is divided by 2^prescaler
837 835 time_management_regs->calPrescaler = prescaler;
838 836 }
839 837
840 838 void setCalibrationDivisor( unsigned int divisionFactor )
841 839 {
842 840 // division of the prescaled clock by the division factor
843 841 time_management_regs->calDivisor = divisionFactor;
844 842 }
845 843
846 844 void setCalibrationData( void ){
847 845 unsigned int k;
848 846 unsigned short data;
849 847 float val;
850 848 float f0;
851 849 float f1;
852 850 float fs;
853 851 float Ts;
854 852 float scaleFactor;
855 853
856 854 f0 = 625;
857 855 f1 = 10000;
858 856 fs = 160256.410;
859 857 Ts = 1. / fs;
860 858 scaleFactor = 0.125 / 0.000654; // 191, 500 mVpp, 2 sinus waves => 250 mVpp each, amplitude = 125 mV
861 859
862 860 time_management_regs->calDataPtr = 0x00;
863 861
864 862 // build the signal for the SCM calibration
865 863 for (k=0; k<256; k++)
866 864 {
867 865 val = sin( 2 * pi * f0 * k * Ts )
868 866 + sin( 2 * pi * f1 * k * Ts );
869 867 data = (unsigned short) ((val * scaleFactor) + 2048);
870 868 time_management_regs->calData = data & 0xfff;
871 869 }
872 870 }
873 871
874 872 void setCalibrationDataInterleaved( void ){
875 873 unsigned int k;
876 874 float val;
877 875 float f0;
878 876 float f1;
879 877 float fs;
880 878 float Ts;
881 879 unsigned short data[384];
882 880 unsigned char *dataPtr;
883 881
884 882 f0 = 625;
885 883 f1 = 10000;
886 884 fs = 240384.615;
887 885 Ts = 1. / fs;
888 886
889 887 time_management_regs->calDataPtr = 0x00;
890 888
891 889 // build the signal for the SCM calibration
892 890 for (k=0; k<384; k++)
893 891 {
894 892 val = sin( 2 * pi * f0 * k * Ts )
895 893 + sin( 2 * pi * f1 * k * Ts );
896 894 data[k] = (unsigned short) (val * 512 + 2048);
897 895 }
898 896
899 897 // write the signal in interleaved mode
900 898 for (k=0; k<128; k++)
901 899 {
902 900 dataPtr = (unsigned char*) &data[k*3 + 2];
903 901 time_management_regs->calData = (data[k*3] & 0xfff)
904 902 + ( (dataPtr[0] & 0x3f) << 12);
905 903 time_management_regs->calData = (data[k*3 + 1] & 0xfff)
906 904 + ( (dataPtr[1] & 0x3f) << 12);
907 905 }
908 906 }
909 907
910 908 void setCalibrationReload( bool state)
911 909 {
912 910 if (state == true)
913 911 {
914 912 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000010; // [0001 0000]
915 913 }
916 914 else
917 915 {
918 916 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffef; // [1110 1111]
919 917 }
920 918 }
921 919
922 920 void setCalibrationEnable( bool state )
923 921 {
924 922 // this bit drives the multiplexer
925 923 if (state == true)
926 924 {
927 925 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000040; // [0100 0000]
928 926 }
929 927 else
930 928 {
931 929 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffbf; // [1011 1111]
932 930 }
933 931 }
934 932
935 933 void setCalibrationInterleaved( bool state )
936 934 {
937 935 // this bit drives the multiplexer
938 936 if (state == true)
939 937 {
940 938 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000020; // [0010 0000]
941 939 }
942 940 else
943 941 {
944 942 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffdf; // [1101 1111]
945 943 }
946 944 }
947 945
948 946 void startCalibration( void )
949 947 {
950 948 setCalibrationEnable( true );
951 949 setCalibrationReload( false );
952 950 }
953 951
954 952 void stopCalibration( void )
955 953 {
956 954 setCalibrationEnable( false );
957 955 setCalibrationReload( true );
958 956 }
959 957
960 958 void configureCalibration( bool interleaved )
961 959 {
962 960 stopCalibration();
963 961 if ( interleaved == true )
964 962 {
965 963 setCalibrationInterleaved( true );
966 964 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
967 965 setCalibrationDivisor( 26 ); // => 240 384
968 966 setCalibrationDataInterleaved();
969 967 }
970 968 else
971 969 {
972 970 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
973 971 setCalibrationDivisor( 38 ); // => 160 256 (39 - 1)
974 972 setCalibrationData();
975 973 }
976 974 }
977 975
978 976 //****************
979 977 // CLOSING ACTIONS
980 978 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC, unsigned char * time )
981 979 {
982 980 /** This function is used to update the HK packets statistics after a successful TC execution.
983 981 *
984 982 * @param TC points to the TC being processed
985 983 * @param time is the time used to date the TC execution
986 984 *
987 985 */
988 986
989 987 unsigned int val;
990 988
991 989 housekeeping_packet.hk_lfr_last_exe_tc_id[0] = TC->packetID[0];
992 990 housekeeping_packet.hk_lfr_last_exe_tc_id[1] = TC->packetID[1];
993 991 housekeeping_packet.hk_lfr_last_exe_tc_type[0] = 0x00;
994 992 housekeeping_packet.hk_lfr_last_exe_tc_type[1] = TC->serviceType;
995 993 housekeeping_packet.hk_lfr_last_exe_tc_subtype[0] = 0x00;
996 994 housekeeping_packet.hk_lfr_last_exe_tc_subtype[1] = TC->serviceSubType;
997 995 housekeeping_packet.hk_lfr_last_exe_tc_time[0] = time[0];
998 996 housekeeping_packet.hk_lfr_last_exe_tc_time[1] = time[1];
999 997 housekeeping_packet.hk_lfr_last_exe_tc_time[2] = time[2];
1000 998 housekeeping_packet.hk_lfr_last_exe_tc_time[3] = time[3];
1001 999 housekeeping_packet.hk_lfr_last_exe_tc_time[4] = time[4];
1002 1000 housekeeping_packet.hk_lfr_last_exe_tc_time[5] = time[5];
1003 1001
1004 1002 val = housekeeping_packet.hk_lfr_exe_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_exe_tc_cnt[1];
1005 1003 val++;
1006 1004 housekeeping_packet.hk_lfr_exe_tc_cnt[0] = (unsigned char) (val >> 8);
1007 1005 housekeeping_packet.hk_lfr_exe_tc_cnt[1] = (unsigned char) (val);
1008 1006 }
1009 1007
1010 1008 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC, unsigned char * time )
1011 1009 {
1012 1010 /** This function is used to update the HK packets statistics after a TC rejection.
1013 1011 *
1014 1012 * @param TC points to the TC being processed
1015 1013 * @param time is the time used to date the TC rejection
1016 1014 *
1017 1015 */
1018 1016
1019 1017 unsigned int val;
1020 1018
1021 1019 housekeeping_packet.hk_lfr_last_rej_tc_id[0] = TC->packetID[0];
1022 1020 housekeeping_packet.hk_lfr_last_rej_tc_id[1] = TC->packetID[1];
1023 1021 housekeeping_packet.hk_lfr_last_rej_tc_type[0] = 0x00;
1024 1022 housekeeping_packet.hk_lfr_last_rej_tc_type[1] = TC->serviceType;
1025 1023 housekeeping_packet.hk_lfr_last_rej_tc_subtype[0] = 0x00;
1026 1024 housekeeping_packet.hk_lfr_last_rej_tc_subtype[1] = TC->serviceSubType;
1027 1025 housekeeping_packet.hk_lfr_last_rej_tc_time[0] = time[0];
1028 1026 housekeeping_packet.hk_lfr_last_rej_tc_time[1] = time[1];
1029 1027 housekeeping_packet.hk_lfr_last_rej_tc_time[2] = time[2];
1030 1028 housekeeping_packet.hk_lfr_last_rej_tc_time[3] = time[3];
1031 1029 housekeeping_packet.hk_lfr_last_rej_tc_time[4] = time[4];
1032 1030 housekeeping_packet.hk_lfr_last_rej_tc_time[5] = time[5];
1033 1031
1034 1032 val = housekeeping_packet.hk_lfr_rej_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_rej_tc_cnt[1];
1035 1033 val++;
1036 1034 housekeeping_packet.hk_lfr_rej_tc_cnt[0] = (unsigned char) (val >> 8);
1037 1035 housekeeping_packet.hk_lfr_rej_tc_cnt[1] = (unsigned char) (val);
1038 1036 }
1039 1037
1040 1038 void close_action(ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id )
1041 1039 {
1042 1040 /** This function is the last step of the TC execution workflow.
1043 1041 *
1044 1042 * @param TC points to the TC being processed
1045 1043 * @param result is the result of the TC execution (LFR_SUCCESSFUL / LFR_DEFAULT)
1046 1044 * @param queue_id is the id of the RTEMS message queue used to send TM packets
1047 1045 * @param time is the time used to date the TC execution
1048 1046 *
1049 1047 */
1050 1048
1051 1049 unsigned char requestedMode;
1052 1050
1053 1051 if (result == LFR_SUCCESSFUL)
1054 1052 {
1055 1053 if ( !( (TC->serviceType==TC_TYPE_TIME) & (TC->serviceSubType==TC_SUBTYPE_UPDT_TIME) )
1056 1054 &
1057 1055 !( (TC->serviceType==TC_TYPE_GEN) & (TC->serviceSubType==TC_SUBTYPE_UPDT_INFO))
1058 1056 )
1059 1057 {
1060 1058 send_tm_lfr_tc_exe_success( TC, queue_id );
1061 1059 }
1062 1060 if ( (TC->serviceType == TC_TYPE_GEN) & (TC->serviceSubType == TC_SUBTYPE_ENTER) )
1063 1061 {
1064 1062 //**********************************
1065 1063 // UPDATE THE LFRMODE LOCAL VARIABLE
1066 1064 requestedMode = TC->dataAndCRC[1];
1067 1065 housekeeping_packet.lfr_status_word[0] = (unsigned char) ((requestedMode << 4) + 0x0d);
1068 1066 updateLFRCurrentMode();
1069 1067 }
1070 1068 }
1071 1069 else if (result == LFR_EXE_ERROR)
1072 1070 {
1073 1071 send_tm_lfr_tc_exe_error( TC, queue_id );
1074 1072 }
1075 1073 }
1076 1074
1077 1075 //***************************
1078 1076 // Interrupt Service Routines
1079 1077 rtems_isr commutation_isr1( rtems_vector_number vector )
1080 1078 {
1081 1079 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1082 1080 printf("In commutation_isr1 *** Error sending event to DUMB\n");
1083 1081 }
1084 1082 }
1085 1083
1086 1084 rtems_isr commutation_isr2( rtems_vector_number vector )
1087 1085 {
1088 1086 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1089 1087 printf("In commutation_isr2 *** Error sending event to DUMB\n");
1090 1088 }
1091 1089 }
1092 1090
1093 1091 //****************
1094 1092 // OTHER FUNCTIONS
1095 1093 void updateLFRCurrentMode()
1096 1094 {
1097 1095 /** This function updates the value of the global variable lfrCurrentMode.
1098 1096 *
1099 1097 * lfrCurrentMode is a parameter used by several functions to know in which mode LFR is running.
1100 1098 *
1101 1099 */
1102 1100 // update the local value of lfrCurrentMode with the value contained in the housekeeping_packet structure
1103 1101 lfrCurrentMode = (housekeeping_packet.lfr_status_word[0] & 0xf0) >> 4;
1104 1102 }
1105 1103
1106 1104 void set_lfr_soft_reset( unsigned char value )
1107 1105 {
1108 1106 if (value == 1)
1109 1107 {
1110 1108 time_management_regs->ctrl = time_management_regs->ctrl | 0x00000004; // [0100]
1111 1109 }
1112 1110 else
1113 1111 {
1114 1112 time_management_regs->ctrl = time_management_regs->ctrl & 0xfffffffb; // [1011]
1115 1113 }
1116 1114 }
1117 1115
1118 1116 void reset_lfr( void )
1119 1117 {
1120 1118 set_lfr_soft_reset( 1 );
1121 1119
1122 1120 set_lfr_soft_reset( 0 );
1123 1121 }
1124
1125 void printTaskID( void )
1126 {
1127 unsigned int i;
1128
1129 for (i=0; i<20;i++)
1130 {
1131 printf("ID %d = %d\n", i, (unsigned int) Task_id[i]);
1132 }
1133 }
Show file before | Show file after | Hide comments
@@ -1,852 +1,1092
1 1 /** Functions to load and dump parameters in the LFR registers.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TC related to parameter loading and dumping.\n
7 7 * TC_LFR_LOAD_COMMON_PAR\n
8 8 * TC_LFR_LOAD_NORMAL_PAR\n
9 9 * TC_LFR_LOAD_BURST_PAR\n
10 10 * TC_LFR_LOAD_SBM1_PAR\n
11 11 * TC_LFR_LOAD_SBM2_PAR\n
12 12 *
13 13 */
14 14
15 15 #include "tc_load_dump_parameters.h"
16 16
17 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_1;
18 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2;
19 ring_node kcoefficient_node_1;
20 ring_node kcoefficient_node_2;
21
17 22 int action_load_common_par(ccsdsTelecommandPacket_t *TC)
18 23 {
19 24 /** This function updates the LFR registers with the incoming common parameters.
20 25 *
21 26 * @param TC points to the TeleCommand packet that is being processed
22 27 *
23 28 *
24 29 */
25 30
26 31 parameter_dump_packet.unused0 = TC->dataAndCRC[0];
27 32 parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
28 33 set_wfp_data_shaping( );
29 34 return LFR_SUCCESSFUL;
30 35 }
31 36
32 37 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
33 38 {
34 39 /** This function updates the LFR registers with the incoming normal parameters.
35 40 *
36 41 * @param TC points to the TeleCommand packet that is being processed
37 42 * @param queue_id is the id of the queue which handles TM related to this execution step
38 43 *
39 44 */
40 45
41 46 int result;
42 47 int flag;
43 48 rtems_status_code status;
44 49
45 50 flag = LFR_SUCCESSFUL;
46 51
47 52 if ( (lfrCurrentMode == LFR_MODE_NORMAL) ||
48 53 (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) {
49 54 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
50 55 flag = LFR_DEFAULT;
51 56 }
52 57
53 58 // CHECK THE PARAMETERS SET CONSISTENCY
54 59 if (flag == LFR_SUCCESSFUL)
55 60 {
56 61 flag = check_common_par_consistency( TC, queue_id );
57 62 }
58 63
59 64 // SET THE PARAMETERS IF THEY ARE CONSISTENT
60 65 if (flag == LFR_SUCCESSFUL)
61 66 {
62 67 result = set_sy_lfr_n_swf_l( TC );
63 68 result = set_sy_lfr_n_swf_p( TC );
64 69 result = set_sy_lfr_n_bp_p0( TC );
65 70 result = set_sy_lfr_n_bp_p1( TC );
66 71 result = set_sy_lfr_n_asm_p( TC );
67 72 result = set_sy_lfr_n_cwf_long_f3( TC );
68 73 }
69 74
70 75 return flag;
71 76 }
72 77
73 78 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
74 79 {
75 80 /** This function updates the LFR registers with the incoming burst parameters.
76 81 *
77 82 * @param TC points to the TeleCommand packet that is being processed
78 83 * @param queue_id is the id of the queue which handles TM related to this execution step
79 84 *
80 85 */
81 86
82 87 int flag;
83 88 rtems_status_code status;
84 89 unsigned char sy_lfr_b_bp_p0;
85 90 unsigned char sy_lfr_b_bp_p1;
86 91 float aux;
87 92
88 93 flag = LFR_SUCCESSFUL;
89 94
90 95 if ( lfrCurrentMode == LFR_MODE_BURST ) {
91 96 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
92 97 flag = LFR_DEFAULT;
93 98 }
94 99
95 100 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
96 101 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
97 102
98 103 // sy_lfr_b_bp_p0
99 104 if (flag == LFR_SUCCESSFUL)
100 105 {
101 106 if (sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0 )
102 107 {
103 108 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0+10, sy_lfr_b_bp_p0 );
104 109 flag = WRONG_APP_DATA;
105 110 }
106 111 }
107 112 // sy_lfr_b_bp_p1
108 113 if (flag == LFR_SUCCESSFUL)
109 114 {
110 115 if (sy_lfr_b_bp_p1 < DEFAULT_SY_LFR_B_BP_P1 )
111 116 {
112 117 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P1+10, sy_lfr_b_bp_p1 );
113 118 flag = WRONG_APP_DATA;
114 119 }
115 120 }
116 121 //****************************************************************
117 122 // check the consistency between sy_lfr_b_bp_p0 and sy_lfr_b_bp_p1
118 123 if (flag == LFR_SUCCESSFUL)
119 124 {
120 125 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
121 126 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
122 127 aux = ( (float ) sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0 ) - floor(sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0);
123 128 if (aux > FLOAT_EQUAL_ZERO)
124 129 {
125 130 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0+10, sy_lfr_b_bp_p0 );
126 131 flag = LFR_DEFAULT;
127 132 }
128 133 }
129 134
130 135 // SET HTE PARAMETERS
131 136 if (flag == LFR_SUCCESSFUL)
132 137 {
133 138 flag = set_sy_lfr_b_bp_p0( TC );
134 139 flag = set_sy_lfr_b_bp_p1( TC );
135 140 }
136 141
137 142 return flag;
138 143 }
139 144
140 145 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
141 146 {
142 147 /** This function updates the LFR registers with the incoming sbm1 parameters.
143 148 *
144 149 * @param TC points to the TeleCommand packet that is being processed
145 150 * @param queue_id is the id of the queue which handles TM related to this execution step
146 151 *
147 152 */
148 153
149 154 int flag;
150 155 rtems_status_code status;
151 156 unsigned char sy_lfr_s1_bp_p0;
152 157 unsigned char sy_lfr_s1_bp_p1;
153 158 float aux;
154 159
155 160 flag = LFR_SUCCESSFUL;
156 161
157 162 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
158 163 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
159 164 flag = LFR_DEFAULT;
160 165 }
161 166
162 167 sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
163 168 sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
164 169
165 170 // sy_lfr_s1_bp_p0
166 171 if (flag == LFR_SUCCESSFUL)
167 172 {
168 173 if (sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0 )
169 174 {
170 175 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0+10, sy_lfr_s1_bp_p0 );
171 176 flag = WRONG_APP_DATA;
172 177 }
173 178 }
174 179 // sy_lfr_s1_bp_p1
175 180 if (flag == LFR_SUCCESSFUL)
176 181 {
177 182 if (sy_lfr_s1_bp_p1 < DEFAULT_SY_LFR_S1_BP_P1 )
178 183 {
179 184 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P1+10, sy_lfr_s1_bp_p1 );
180 185 flag = WRONG_APP_DATA;
181 186 }
182 187 }
183 188 //******************************************************************
184 189 // check the consistency between sy_lfr_s1_bp_p0 and sy_lfr_s1_bp_p1
185 190 if (flag == LFR_SUCCESSFUL)
186 191 {
187 192 aux = ( (float ) sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0*0.25) ) - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0*0.25));
188 193 if (aux > FLOAT_EQUAL_ZERO)
189 194 {
190 195 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0+10, sy_lfr_s1_bp_p0 );
191 196 flag = LFR_DEFAULT;
192 197 }
193 198 }
194 199
195 200 // SET THE PARAMETERS
196 201 if (flag == LFR_SUCCESSFUL)
197 202 {
198 203 flag = set_sy_lfr_s1_bp_p0( TC );
199 204 flag = set_sy_lfr_s1_bp_p1( TC );
200 205 }
201 206
202 207 return flag;
203 208 }
204 209
205 210 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
206 211 {
207 212 /** This function updates the LFR registers with the incoming sbm2 parameters.
208 213 *
209 214 * @param TC points to the TeleCommand packet that is being processed
210 215 * @param queue_id is the id of the queue which handles TM related to this execution step
211 216 *
212 217 */
213 218
214 219 int flag;
215 220 rtems_status_code status;
216 221 unsigned char sy_lfr_s2_bp_p0;
217 222 unsigned char sy_lfr_s2_bp_p1;
218 223 float aux;
219 224
220 225 flag = LFR_SUCCESSFUL;
221 226
222 227 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
223 228 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
224 229 flag = LFR_DEFAULT;
225 230 }
226 231
227 232 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
228 233 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
229 234
230 235 // sy_lfr_s2_bp_p0
231 236 if (flag == LFR_SUCCESSFUL)
232 237 {
233 238 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
234 239 {
235 240 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
236 241 flag = WRONG_APP_DATA;
237 242 }
238 243 }
239 244 // sy_lfr_s2_bp_p1
240 245 if (flag == LFR_SUCCESSFUL)
241 246 {
242 247 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
243 248 {
244 249 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P1+10, sy_lfr_s2_bp_p1 );
245 250 flag = WRONG_APP_DATA;
246 251 }
247 252 }
248 253 //******************************************************************
249 254 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
250 255 if (flag == LFR_SUCCESSFUL)
251 256 {
252 257 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
253 258 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
254 259 aux = ( (float ) sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0 ) - floor(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
255 260 if (aux > FLOAT_EQUAL_ZERO)
256 261 {
257 262 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
258 263 flag = LFR_DEFAULT;
259 264 }
260 265 }
261 266
262 267 // SET THE PARAMETERS
263 268 if (flag == LFR_SUCCESSFUL)
264 269 {
265 270 flag = set_sy_lfr_s2_bp_p0( TC );
266 271 flag = set_sy_lfr_s2_bp_p1( TC );
267 272 }
268 273
269 274 return flag;
270 275 }
271 276
272 277 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
273 278 {
274 279 /** This function updates the LFR registers with the incoming sbm2 parameters.
275 280 *
276 281 * @param TC points to the TeleCommand packet that is being processed
277 282 * @param queue_id is the id of the queue which handles TM related to this execution step
278 283 *
279 284 */
280 285
281 286 int flag;
282 287
283 288 flag = LFR_DEFAULT;
284 289
285 // NB_BINS_COMPRESSED_SM_F0;
286 // NB_BINS_COMPRESSED_SM_F1;
287 // NB_BINS_COMPRESSED_SM_F2;
288
289 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
290 flag = set_sy_lfr_kcoeff( TC );
290 291
291 292 return flag;
292 293 }
293 294
294 295 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
295 296 {
296 297 /** This function updates the LFR registers with the incoming sbm2 parameters.
297 298 *
298 299 * @param TC points to the TeleCommand packet that is being processed
299 300 * @param queue_id is the id of the queue which handles TM related to this execution step
300 301 *
301 302 */
302 303
303 304 int flag;
304 305
305 306 flag = LFR_DEFAULT;
306 307
307 308 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
308 309
309 310 return flag;
310 311 }
311 312
313 void printKCoefficients(unsigned int freq, unsigned int bin, float *k_coeff)
314 {
315 printf("freq = %d *** bin = %d *** (0) %f *** (1) %f *** (2) %f *** (3) %f *** (4) %f\n",
316 freq,
317 bin,
318 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 0 ],
319 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 1 ],
320 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 2 ],
321 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 3 ],
322 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 4 ]);
323 }
324
312 325 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
313 326 {
314 327 /** This function updates the LFR registers with the incoming sbm2 parameters.
315 328 *
316 329 * @param TC points to the TeleCommand packet that is being processed
317 330 * @param queue_id is the id of the queue which handles TM related to this execution step
318 331 *
319 332 */
320 333
321 int flag;
334 unsigned int address;
335 rtems_status_code status;
336 unsigned int freq;
337 unsigned int bin;
338 unsigned int coeff;
339 unsigned char *kCoeffPtr;
340 unsigned char *kCoeffDumpPtr;
322 341
323 flag = LFR_DEFAULT;
342 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
343 // F0 => 11 bins
344 // F1 => 13 bins
345 // F2 => 12 bins
346 // 36 bins to dump in two packets (30 bins max per packet)
324 347
325 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
348 //*********
349 // PACKET 1
350 // 11 F0 bins, 13 F1 bins and 6 F2 bins
351 kcoefficients_dump_1.packetSequenceControl[0] = (unsigned char) (sequenceCounterParameterDump >> 8);
352 kcoefficients_dump_1.packetSequenceControl[1] = (unsigned char) (sequenceCounterParameterDump );
353 increment_seq_counter( &sequenceCounterParameterDump );
354 for( freq=0;
355 freq<NB_BINS_COMPRESSED_SM_F0;
356 freq++ )
357 {
358 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1] = freq;
359 bin = freq;
360 printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
361 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
362 {
363 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
364 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
365 kCoeffDumpPtr[0] = kCoeffPtr[0];
366 kCoeffDumpPtr[1] = kCoeffPtr[1];
367 kCoeffDumpPtr[2] = kCoeffPtr[2];
368 kCoeffDumpPtr[3] = kCoeffPtr[3];
369 }
370 }
371 for( freq=NB_BINS_COMPRESSED_SM_F0;
372 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
373 freq++ )
374 {
375 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
376 bin = freq - NB_BINS_COMPRESSED_SM_F0;
377 printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
378 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
379 {
380 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
381 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
382 kCoeffDumpPtr[0] = kCoeffPtr[0];
383 kCoeffDumpPtr[1] = kCoeffPtr[1];
384 kCoeffDumpPtr[2] = kCoeffPtr[2];
385 kCoeffDumpPtr[3] = kCoeffPtr[3];
386 }
387 }
388 for( freq=(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
389 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1+6);
390 freq++ )
391 {
392 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
393 bin = freq - (NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
394 printKCoefficients( freq, bin, k_coeff_intercalib_f2);
395 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
396 {
397 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
398 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
399 kCoeffDumpPtr[0] = kCoeffPtr[0];
400 kCoeffDumpPtr[1] = kCoeffPtr[1];
401 kCoeffDumpPtr[2] = kCoeffPtr[2];
402 kCoeffDumpPtr[3] = kCoeffPtr[3];
403 }
404 }
405 kcoefficients_dump_1.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
406 kcoefficients_dump_1.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
407 kcoefficients_dump_1.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
408 kcoefficients_dump_1.time[3] = (unsigned char) (time_management_regs->coarse_time);
409 kcoefficients_dump_1.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
410 kcoefficients_dump_1.time[5] = (unsigned char) (time_management_regs->fine_time);
411 // SEND DATA
412 kcoefficient_node_1.status = 1;
413 address = (unsigned int) &kcoefficient_node_1;
414 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
415 if (status != RTEMS_SUCCESSFUL) {
416 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
417 }
326 418
327 return flag;
419 //********
420 // PACKET 2
421 // 6 F2 bins
422 kcoefficients_dump_2.packetSequenceControl[0] = (unsigned char) (sequenceCounterParameterDump >> 8);
423 kcoefficients_dump_2.packetSequenceControl[1] = (unsigned char) (sequenceCounterParameterDump );
424 increment_seq_counter( &sequenceCounterParameterDump );
425 for( freq=0; freq<6; freq++ )
426 {
427 kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + 6 + freq;
428 bin = freq + 6;
429 printKCoefficients( freq, bin, k_coeff_intercalib_f2);
430 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
431 {
432 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
433 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
434 kCoeffDumpPtr[0] = kCoeffPtr[0];
435 kCoeffDumpPtr[1] = kCoeffPtr[1];
436 kCoeffDumpPtr[2] = kCoeffPtr[2];
437 kCoeffDumpPtr[3] = kCoeffPtr[3];
438 }
439 }
440 kcoefficients_dump_2.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
441 kcoefficients_dump_2.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
442 kcoefficients_dump_2.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
443 kcoefficients_dump_2.time[3] = (unsigned char) (time_management_regs->coarse_time);
444 kcoefficients_dump_2.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
445 kcoefficients_dump_2.time[5] = (unsigned char) (time_management_regs->fine_time);
446 // SEND DATA
447 kcoefficient_node_2.status = 1;
448 address = (unsigned int) &kcoefficient_node_2;
449 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
450 if (status != RTEMS_SUCCESSFUL) {
451 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
452 }
453
454 return status;
328 455 }
329 456
330 457 int action_dump_par( rtems_id queue_id )
331 458 {
332 459 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
333 460 *
334 461 * @param queue_id is the id of the queue which handles TM related to this execution step.
335 462 *
336 463 * @return RTEMS directive status codes:
337 464 * - RTEMS_SUCCESSFUL - message sent successfully
338 465 * - RTEMS_INVALID_ID - invalid queue id
339 466 * - RTEMS_INVALID_SIZE - invalid message size
340 467 * - RTEMS_INVALID_ADDRESS - buffer is NULL
341 468 * - RTEMS_UNSATISFIED - out of message buffers
342 469 * - RTEMS_TOO_MANY - queue s limit has been reached
343 470 *
344 471 */
345 472
346 473 int status;
347 474
348 475 // UPDATE TIME
349 476 parameter_dump_packet.packetSequenceControl[0] = (unsigned char) (sequenceCounterParameterDump >> 8);
350 477 parameter_dump_packet.packetSequenceControl[1] = (unsigned char) (sequenceCounterParameterDump );
351 478 increment_seq_counter( &sequenceCounterParameterDump );
352 479
353 480 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
354 481 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
355 482 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
356 483 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
357 484 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
358 485 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
359 486 // SEND DATA
360 487 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
361 488 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
362 489 if (status != RTEMS_SUCCESSFUL) {
363 490 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
364 491 }
365 492
366 493 return status;
367 494 }
368 495
369 496 //***********************
370 497 // NORMAL MODE PARAMETERS
371 498
372 499 int check_common_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
373 500 {
374 501 unsigned char msb;
375 502 unsigned char lsb;
376 503 int flag;
377 504 float aux;
378 505 rtems_status_code status;
379 506
380 507 unsigned int sy_lfr_n_swf_l;
381 508 unsigned int sy_lfr_n_swf_p;
382 509 unsigned int sy_lfr_n_asm_p;
383 510 unsigned char sy_lfr_n_bp_p0;
384 511 unsigned char sy_lfr_n_bp_p1;
385 512 unsigned char sy_lfr_n_cwf_long_f3;
386 513
387 514 flag = LFR_SUCCESSFUL;
388 515
389 516 //***************
390 517 // get parameters
391 518 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
392 519 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
393 520 sy_lfr_n_swf_l = msb * 256 + lsb;
394 521
395 522 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
396 523 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
397 524 sy_lfr_n_swf_p = msb * 256 + lsb;
398 525
399 526 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
400 527 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
401 528 sy_lfr_n_asm_p = msb * 256 + lsb;
402 529
403 530 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
404 531
405 532 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
406 533
407 534 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
408 535
409 536 //******************
410 537 // check consistency
411 538 // sy_lfr_n_swf_l
412 539 if (sy_lfr_n_swf_l != 2048)
413 540 {
414 541 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L+10, sy_lfr_n_swf_l );
415 542 flag = WRONG_APP_DATA;
416 543 }
417 544 // sy_lfr_n_swf_p
418 545 if (flag == LFR_SUCCESSFUL)
419 546 {
420 547 if ( sy_lfr_n_swf_p < 16 )
421 548 {
422 549 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P+10, sy_lfr_n_swf_p );
423 550 flag = WRONG_APP_DATA;
424 551 }
425 552 }
426 553 // sy_lfr_n_bp_p0
427 554 if (flag == LFR_SUCCESSFUL)
428 555 {
429 556 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
430 557 {
431 558 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0+10, sy_lfr_n_bp_p0 );
432 559 flag = WRONG_APP_DATA;
433 560 }
434 561 }
435 562 // sy_lfr_n_asm_p
436 563 if (flag == LFR_SUCCESSFUL)
437 564 {
438 565 if (sy_lfr_n_asm_p == 0)
439 566 {
440 567 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
441 568 flag = WRONG_APP_DATA;
442 569 }
443 570 }
444 571 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
445 572 if (flag == LFR_SUCCESSFUL)
446 573 {
447 574 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
448 575 if (aux > FLOAT_EQUAL_ZERO)
449 576 {
450 577 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
451 578 flag = WRONG_APP_DATA;
452 579 }
453 580 }
454 581 // sy_lfr_n_bp_p1
455 582 if (flag == LFR_SUCCESSFUL)
456 583 {
457 584 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
458 585 {
459 586 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
460 587 flag = WRONG_APP_DATA;
461 588 }
462 589 }
463 590 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
464 591 if (flag == LFR_SUCCESSFUL)
465 592 {
466 593 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
467 594 if (aux > FLOAT_EQUAL_ZERO)
468 595 {
469 596 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
470 597 flag = LFR_DEFAULT;
471 598 }
472 599 }
473 600 // sy_lfr_n_cwf_long_f3
474 601
475 602 return flag;
476 603 }
477 604
478 605 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
479 606 {
480 607 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
481 608 *
482 609 * @param TC points to the TeleCommand packet that is being processed
483 610 * @param queue_id is the id of the queue which handles TM related to this execution step
484 611 *
485 612 */
486 613
487 614 int result;
488 615
489 616 result = LFR_SUCCESSFUL;
490 617
491 618 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
492 619 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
493 620
494 621 return result;
495 622 }
496 623
497 624 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
498 625 {
499 626 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
500 627 *
501 628 * @param TC points to the TeleCommand packet that is being processed
502 629 * @param queue_id is the id of the queue which handles TM related to this execution step
503 630 *
504 631 */
505 632
506 633 int result;
507 634
508 635 result = LFR_SUCCESSFUL;
509 636
510 637 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
511 638 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
512 639
513 640 return result;
514 641 }
515 642
516 643 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
517 644 {
518 645 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
519 646 *
520 647 * @param TC points to the TeleCommand packet that is being processed
521 648 * @param queue_id is the id of the queue which handles TM related to this execution step
522 649 *
523 650 */
524 651
525 652 int result;
526 653
527 654 result = LFR_SUCCESSFUL;
528 655
529 656 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
530 657 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
531 658
532 659 return result;
533 660 }
534 661
535 662 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
536 663 {
537 664 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
538 665 *
539 666 * @param TC points to the TeleCommand packet that is being processed
540 667 * @param queue_id is the id of the queue which handles TM related to this execution step
541 668 *
542 669 */
543 670
544 671 int status;
545 672
546 673 status = LFR_SUCCESSFUL;
547 674
548 675 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
549 676
550 677 return status;
551 678 }
552 679
553 680 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
554 681 {
555 682 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
556 683 *
557 684 * @param TC points to the TeleCommand packet that is being processed
558 685 * @param queue_id is the id of the queue which handles TM related to this execution step
559 686 *
560 687 */
561 688
562 689 int status;
563 690
564 691 status = LFR_SUCCESSFUL;
565 692
566 693 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
567 694
568 695 return status;
569 696 }
570 697
571 698 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
572 699 {
573 700 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
574 701 *
575 702 * @param TC points to the TeleCommand packet that is being processed
576 703 * @param queue_id is the id of the queue which handles TM related to this execution step
577 704 *
578 705 */
579 706
580 707 int status;
581 708
582 709 status = LFR_SUCCESSFUL;
583 710
584 711 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
585 712
586 713 return status;
587 714 }
588 715
589 716 //**********************
590 717 // BURST MODE PARAMETERS
591 718 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
592 719 {
593 720 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
594 721 *
595 722 * @param TC points to the TeleCommand packet that is being processed
596 723 * @param queue_id is the id of the queue which handles TM related to this execution step
597 724 *
598 725 */
599 726
600 727 int status;
601 728
602 729 status = LFR_SUCCESSFUL;
603 730
604 731 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
605 732
606 733 return status;
607 734 }
608 735
609 736 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
610 737 {
611 738 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
612 739 *
613 740 * @param TC points to the TeleCommand packet that is being processed
614 741 * @param queue_id is the id of the queue which handles TM related to this execution step
615 742 *
616 743 */
617 744
618 745 int status;
619 746
620 747 status = LFR_SUCCESSFUL;
621 748
622 749 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
623 750
624 751 return status;
625 752 }
626 753
627 754 //*********************
628 755 // SBM1 MODE PARAMETERS
629 756 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
630 757 {
631 758 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
632 759 *
633 760 * @param TC points to the TeleCommand packet that is being processed
634 761 * @param queue_id is the id of the queue which handles TM related to this execution step
635 762 *
636 763 */
637 764
638 765 int status;
639 766
640 767 status = LFR_SUCCESSFUL;
641 768
642 769 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
643 770
644 771 return status;
645 772 }
646 773
647 774 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
648 775 {
649 776 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
650 777 *
651 778 * @param TC points to the TeleCommand packet that is being processed
652 779 * @param queue_id is the id of the queue which handles TM related to this execution step
653 780 *
654 781 */
655 782
656 783 int status;
657 784
658 785 status = LFR_SUCCESSFUL;
659 786
660 787 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
661 788
662 789 return status;
663 790 }
664 791
665 792 //*********************
666 793 // SBM2 MODE PARAMETERS
667 794 int set_sy_lfr_s2_bp_p0(ccsdsTelecommandPacket_t *TC)
668 795 {
669 796 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
670 797 *
671 798 * @param TC points to the TeleCommand packet that is being processed
672 799 * @param queue_id is the id of the queue which handles TM related to this execution step
673 800 *
674 801 */
675 802
676 803 int status;
677 804
678 805 status = LFR_SUCCESSFUL;
679 806
680 807 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
681 808
682 809 return status;
683 810 }
684 811
685 812 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
686 813 {
687 814 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
688 815 *
689 816 * @param TC points to the TeleCommand packet that is being processed
690 817 * @param queue_id is the id of the queue which handles TM related to this execution step
691 818 *
692 819 */
693 820
694 821 int status;
695 822
696 823 status = LFR_SUCCESSFUL;
697 824
698 825 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
699 826
700 827 return status;
701 828 }
702 829
703 830 //*******************
704 831 // TC_LFR_UPDATE_INFO
705 832 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
706 833 {
707 834 unsigned int status;
708 835
709 836 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
710 837 || (mode == LFR_MODE_BURST)
711 838 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
712 839 {
713 840 status = LFR_SUCCESSFUL;
714 841 }
715 842 else
716 843 {
717 844 status = LFR_DEFAULT;
718 845 }
719 846
720 847 return status;
721 848 }
722 849
723 850 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
724 851 {
725 852 unsigned int status;
726 853
727 854 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
728 855 || (mode == TDS_MODE_BURST)
729 856 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
730 857 || (mode == TDS_MODE_LFM))
731 858 {
732 859 status = LFR_SUCCESSFUL;
733 860 }
734 861 else
735 862 {
736 863 status = LFR_DEFAULT;
737 864 }
738 865
739 866 return status;
740 867 }
741 868
742 869 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
743 870 {
744 871 unsigned int status;
745 872
746 873 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
747 874 || (mode == THR_MODE_BURST))
748 875 {
749 876 status = LFR_SUCCESSFUL;
750 877 }
751 878 else
752 879 {
753 880 status = LFR_DEFAULT;
754 881 }
755 882
756 883 return status;
757 884 }
758 885
759 886 //**************
760 887 // KCOEFFICIENTS
761 888 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC )
762 889 {
890 unsigned int i;
763 891 unsigned short sy_lfr_kcoeff_frequency;
892 unsigned short bin;
764 893 unsigned short *freqPtr;
894 float *kcoeffPtr_norm;
895 float *kcoeffPtr_sbm;
765 896 int status;
897 unsigned char *kcoeffLoadPtr;
898 unsigned char *kcoeffNormPtr;
766 899
767 900 status = LFR_SUCCESSFUL;
768 901
769 freqPtr = (unsigned short *) &TC->dataAndCRC[0];
902 kcoeffPtr_norm = NULL;
903 kcoeffPtr_sbm = NULL;
904 bin = 0;
905
906 freqPtr = (unsigned short *) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY];
770 907 sy_lfr_kcoeff_frequency = *freqPtr;
771 908
772 PRINTF1("sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
773
774 if (sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM)
909 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
775 910 {
776 911 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
777 912 }
913 else
914 {
915 if ( ( sy_lfr_kcoeff_frequency >= 0 )
916 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
917 {
918 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
919 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
920 bin = sy_lfr_kcoeff_frequency;
921 }
922 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
923 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
924 {
925 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
926 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
927 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
928 }
929 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
930 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
931 {
932 kcoeffPtr_norm = k_coeff_intercalib_f2;
933 kcoeffPtr_sbm = NULL;
934 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
935 }
936 }
937
938 if (kcoeffPtr_norm != NULL )
939 {
940 printf("freq = %d, bin = %d\n", sy_lfr_kcoeff_frequency, bin);
941 for (i=0; i<NB_K_COEFF_PER_BIN; i++)
942 {
943 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * i];
944 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + i ];
945 kcoeffNormPtr[0] = kcoeffLoadPtr[0];
946 kcoeffNormPtr[1] = kcoeffLoadPtr[1];
947 kcoeffNormPtr[2] = kcoeffLoadPtr[2];
948 kcoeffNormPtr[3] = kcoeffLoadPtr[3];
949 printf("kcoeffPtr: %x %x %x %x *** %f \n",
950 kcoeffLoadPtr[0],
951 kcoeffLoadPtr[1],
952 kcoeffLoadPtr[2],
953 kcoeffLoadPtr[3],
954 kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + i ]);
955 }
956 }
778 957
779 958 return status;
780 959 }
781 960
782 961 //**********
783 962 // init dump
784 963
785 964 void init_parameter_dump( void )
786 965 {
787 966 /** This function initialize the parameter_dump_packet global variable with default values.
788 967 *
789 968 */
790 969
791 970 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
792 971 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
793 972 parameter_dump_packet.reserved = CCSDS_RESERVED;
794 973 parameter_dump_packet.userApplication = CCSDS_USER_APP;
795 974 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);
796 975 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
797 976 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
798 977 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
799 978 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> 8);
800 979 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
801 980 // DATA FIELD HEADER
802 981 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
803 982 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
804 983 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
805 984 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
806 985 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
807 986 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
808 987 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
809 988 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
810 989 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
811 990 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
812 991 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
813 992
814 993 //******************
815 994 // COMMON PARAMETERS
816 995 parameter_dump_packet.unused0 = DEFAULT_SY_LFR_COMMON0;
817 996 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
818 997
819 998 //******************
820 999 // NORMAL PARAMETERS
821 1000 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> 8);
822 1001 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
823 1002 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> 8);
824 1003 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
825 1004 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> 8);
826 1005 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
827 1006 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
828 1007 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
829 1008 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
830 1009
831 1010 //*****************
832 1011 // BURST PARAMETERS
833 1012 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
834 1013 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
835 1014
836 1015 //****************
837 1016 // SBM1 PARAMETERS
838 1017 parameter_dump_packet.sy_lfr_s1_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P0; // min value is 0.25 s for the period
839 1018 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
840 1019
841 1020 //****************
842 1021 // SBM2 PARAMETERS
843 1022 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
844 1023 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
845 1024 }
846 1025
1026 void init_kcoefficients_dump( void )
1027 {
1028 init_kcoefficients_dump_packet( &kcoefficients_dump_1, 1, 30 );
1029 init_kcoefficients_dump_packet( &kcoefficients_dump_2, 2, 6 );
1030
1031 kcoefficient_node_1.previous = NULL;
1032 kcoefficient_node_1.next = NULL;
1033 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1034 kcoefficient_node_1.coarseTime = 0x00;
1035 kcoefficient_node_1.fineTime = 0x00;
1036 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1037 kcoefficient_node_1.status = 0x00;
1038
1039 kcoefficient_node_2.previous = NULL;
1040 kcoefficient_node_2.next = NULL;
1041 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1042 kcoefficient_node_2.coarseTime = 0x00;
1043 kcoefficient_node_2.fineTime = 0x00;
1044 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1045 kcoefficient_node_2.status = 0x00;
1046 }
1047
1048 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1049 {
1050 unsigned int k;
1051 unsigned int packetLength;
1052
1053 packetLength = blk_nr * 130 + 20 - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1054
1055 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1056 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1057 kcoefficients_dump->reserved = CCSDS_RESERVED;
1058 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1059 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);;
1060 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;;
1061 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1062 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1063 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> 8);
1064 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1065 // DATA FIELD HEADER
1066 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1067 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1068 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1069 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1070 kcoefficients_dump->time[0] = 0x00;
1071 kcoefficients_dump->time[1] = 0x00;
1072 kcoefficients_dump->time[2] = 0x00;
1073 kcoefficients_dump->time[3] = 0x00;
1074 kcoefficients_dump->time[4] = 0x00;
1075 kcoefficients_dump->time[5] = 0x00;
1076 kcoefficients_dump->sid = SID_K_DUMP;
1077
1078 kcoefficients_dump->pkt_cnt = 2;
1079 kcoefficients_dump->pkt_nr = pkt_nr;
1080 kcoefficients_dump->blk_nr = blk_nr;
1081
1082 //******************
1083 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1084 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1085 for (k=0; k<3900; k++)
1086 {
1087 kcoefficients_dump->kcoeff_blks[k] = 0x00;
1088 }
1089 }
847 1090
848 1091
849 1092
850
851
852
Show file before | Show file after | Hide comments
@@ -1,1402 +1,1402
1 1 /** Functions and tasks related to waveform packet generation.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle waveforms, in snapshot or continuous format.\n
7 7 *
8 8 */
9 9
10 10 #include "wf_handler.h"
11 11
12 12 //***************
13 13 // waveform rings
14 14 // F0
15 15 ring_node waveform_ring_f0[NB_RING_NODES_F0];
16 16 ring_node *current_ring_node_f0;
17 17 ring_node *ring_node_to_send_swf_f0;
18 18 // F1
19 19 ring_node waveform_ring_f1[NB_RING_NODES_F1];
20 20 ring_node *current_ring_node_f1;
21 21 ring_node *ring_node_to_send_swf_f1;
22 22 ring_node *ring_node_to_send_cwf_f1;
23 23 // F2
24 24 ring_node waveform_ring_f2[NB_RING_NODES_F2];
25 25 ring_node *current_ring_node_f2;
26 26 ring_node *ring_node_to_send_swf_f2;
27 27 ring_node *ring_node_to_send_cwf_f2;
28 28 // F3
29 29 ring_node waveform_ring_f3[NB_RING_NODES_F3];
30 30 ring_node *current_ring_node_f3;
31 31 ring_node *ring_node_to_send_cwf_f3;
32 32 char wf_cont_f3_light[ (NB_SAMPLES_PER_SNAPSHOT) * NB_BYTES_CWF3_LIGHT_BLK ];
33 33
34 34 bool extractSWF = false;
35 35 bool swf_f0_ready = false;
36 36 bool swf_f1_ready = false;
37 37 bool swf_f2_ready = false;
38 38
39 39 int wf_snap_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ];
40 40 ring_node ring_node_wf_snap_extracted;
41 41
42 42 //*********************
43 43 // Interrupt SubRoutine
44 44
45 45 ring_node * getRingNodeToSendCWF( unsigned char frequencyChannel)
46 46 {
47 47 ring_node *node;
48 48
49 49 node = NULL;
50 50 switch ( frequencyChannel ) {
51 51 case 1:
52 52 node = ring_node_to_send_cwf_f1;
53 53 break;
54 54 case 2:
55 55 node = ring_node_to_send_cwf_f2;
56 56 break;
57 57 case 3:
58 58 node = ring_node_to_send_cwf_f3;
59 59 break;
60 60 default:
61 61 break;
62 62 }
63 63
64 64 return node;
65 65 }
66 66
67 67 ring_node * getRingNodeToSendSWF( unsigned char frequencyChannel)
68 68 {
69 69 ring_node *node;
70 70
71 71 node = NULL;
72 72 switch ( frequencyChannel ) {
73 73 case 0:
74 74 node = ring_node_to_send_swf_f0;
75 75 break;
76 76 case 1:
77 77 node = ring_node_to_send_swf_f1;
78 78 break;
79 79 case 2:
80 80 node = ring_node_to_send_swf_f2;
81 81 break;
82 82 default:
83 83 break;
84 84 }
85 85
86 86 return node;
87 87 }
88 88
89 89 void reset_extractSWF( void )
90 90 {
91 91 extractSWF = false;
92 92 swf_f0_ready = false;
93 93 swf_f1_ready = false;
94 94 swf_f2_ready = false;
95 95 }
96 96
97 97 inline void waveforms_isr_f3( void )
98 98 {
99 99 rtems_status_code spare_status;
100 100
101 101 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_BURST) // in BURST the data are used to place v, e1 and e2 in the HK packet
102 102 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
103 103 { // in modes other than STANDBY and BURST, send the CWF_F3 data
104 104 //***
105 105 // F3
106 106 if ( (waveform_picker_regs->status & 0xc0) != 0x00 ) { // [1100 0000] check the f3 full bits
107 107 ring_node_to_send_cwf_f3 = current_ring_node_f3->previous;
108 108 current_ring_node_f3 = current_ring_node_f3->next;
109 109 if ((waveform_picker_regs->status & 0x40) == 0x40){ // [0100 0000] f3 buffer 0 is full
110 110 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_0_coarse_time;
111 111 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_0_fine_time;
112 112 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->buffer_address;
113 113 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008840; // [1000 1000 0100 0000]
114 114 }
115 115 else if ((waveform_picker_regs->status & 0x80) == 0x80){ // [1000 0000] f3 buffer 1 is full
116 116 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_1_coarse_time;
117 117 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_1_fine_time;
118 118 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address;
119 119 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008880; // [1000 1000 1000 0000]
120 120 }
121 121 if (rtems_event_send( Task_id[TASKID_CWF3], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
122 122 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
123 123 }
124 124 }
125 125 }
126 126 }
127 127
128 128 inline void waveforms_isr_normal( void )
129 129 {
130 130 rtems_status_code status;
131 131
132 132 if ( ( (waveform_picker_regs->status & 0x30) != 0x00 ) // [0011 0000] check the f2 full bits
133 133 && ( (waveform_picker_regs->status & 0x0c) != 0x00 ) // [0000 1100] check the f1 full bits
134 134 && ( (waveform_picker_regs->status & 0x03) != 0x00 )) // [0000 0011] check the f0 full bits
135 135 {
136 136 //***
137 137 // F0
138 138 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
139 139 current_ring_node_f0 = current_ring_node_f0->next;
140 140 if ( (waveform_picker_regs->status & 0x01) == 0x01)
141 141 {
142 142
143 143 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
144 144 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
145 145 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
146 146 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
147 147 }
148 148 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
149 149 {
150 150 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
151 151 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
152 152 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
153 153 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
154 154 }
155 155
156 156 //***
157 157 // F1
158 158 ring_node_to_send_swf_f1 = current_ring_node_f1->previous;
159 159 current_ring_node_f1 = current_ring_node_f1->next;
160 160 if ( (waveform_picker_regs->status & 0x04) == 0x04)
161 161 {
162 162 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
163 163 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
164 164 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
165 165 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
166 166 }
167 167 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
168 168 {
169 169 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
170 170 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
171 171 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
172 172 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
173 173 }
174 174
175 175 //***
176 176 // F2
177 177 ring_node_to_send_swf_f2 = current_ring_node_f2->previous;
178 178 current_ring_node_f2 = current_ring_node_f2->next;
179 179 if ( (waveform_picker_regs->status & 0x10) == 0x10)
180 180 {
181 181 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
182 182 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
183 183 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
184 184 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
185 185 }
186 186 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
187 187 {
188 188 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
189 189 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
190 190 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
191 191 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
192 192 }
193 193 //
194 194 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_NORMAL );
195 195 if ( status != RTEMS_SUCCESSFUL)
196 196 {
197 197 status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
198 198 }
199 199 }
200 200 }
201 201
202 202 inline void waveforms_isr_burst( void )
203 203 {
204 204 unsigned char status;
205 205 rtems_status_code spare_status;
206 206
207 207 status = (waveform_picker_regs->status & 0x30) >> 4; // [0011 0000] get the status bits for f2
208 208
209 209
210 210 switch(status)
211 211 {
212 212 case 1:
213 213 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
214 214 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
215 215 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
216 216 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
217 217 current_ring_node_f2 = current_ring_node_f2->next;
218 218 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
219 219 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
220 220 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
221 221 }
222 222 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
223 223 break;
224 224 case 2:
225 225 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
226 226 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
227 227 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
228 228 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
229 229 current_ring_node_f2 = current_ring_node_f2->next;
230 230 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
231 231 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
232 232 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
233 233 }
234 234 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
235 235 break;
236 236 default:
237 237 break;
238 238 }
239 239 }
240 240
241 241 inline void waveforms_isr_sbm1( void )
242 242 {
243 243 rtems_status_code status;
244 244
245 245 //***
246 246 // F1
247 247 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bits
248 248 // (1) change the receiving buffer for the waveform picker
249 249 ring_node_to_send_cwf_f1 = current_ring_node_f1->previous;
250 250 current_ring_node_f1 = current_ring_node_f1->next;
251 251 if ( (waveform_picker_regs->status & 0x04) == 0x04)
252 252 {
253 253 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
254 254 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
255 255 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
256 256 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
257 257 }
258 258 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
259 259 {
260 260 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
261 261 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
262 262 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
263 263 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
264 264 }
265 265 // (2) send an event for the the CWF1 task for transmission (and snapshot extraction if needed)
266 266 status = rtems_event_send( Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_SBM1 );
267 267 }
268 268
269 269 //***
270 270 // F0
271 271 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) { // [0000 0011] check the f0 full bits
272 272 swf_f0_ready = true;
273 273 // change f0 buffer
274 274 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
275 275 current_ring_node_f0 = current_ring_node_f0->next;
276 276 if ( (waveform_picker_regs->status & 0x01) == 0x01)
277 277 {
278 278
279 279 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
280 280 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
281 281 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
282 282 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
283 283 }
284 284 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
285 285 {
286 286 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
287 287 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
288 288 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
289 289 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
290 290 }
291 291 }
292 292
293 293 //***
294 294 // F2
295 295 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bits
296 296 swf_f2_ready = true;
297 297 // change f2 buffer
298 298 ring_node_to_send_swf_f2 = current_ring_node_f2->previous;
299 299 current_ring_node_f2 = current_ring_node_f2->next;
300 300 if ( (waveform_picker_regs->status & 0x10) == 0x10)
301 301 {
302 302 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
303 303 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
304 304 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
305 305 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
306 306 }
307 307 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
308 308 {
309 309 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
310 310 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
311 311 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
312 312 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
313 313 }
314 314 }
315 315 }
316 316
317 317 inline void waveforms_isr_sbm2( void )
318 318 {
319 319 rtems_status_code status;
320 320
321 321 //***
322 322 // F2
323 323 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bit
324 324 // (1) change the receiving buffer for the waveform picker
325 325 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
326 326 ring_node_to_send_cwf_f2->sid = SID_SBM2_CWF_F2;
327 327 current_ring_node_f2 = current_ring_node_f2->next;
328 328 if ( (waveform_picker_regs->status & 0x10) == 0x10)
329 329 {
330 330 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
331 331 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
332 332 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
333 333 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
334 334 }
335 335 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
336 336 {
337 337 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
338 338 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
339 339 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
340 340 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
341 341 }
342 342 // (2) send an event for the waveforms transmission
343 343 status = rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_SBM2 );
344 344 }
345 345
346 346 //***
347 347 // F0
348 348 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) { // [0000 0011] check the f0 full bit
349 349 swf_f0_ready = true;
350 350 // change f0 buffer
351 351 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
352 352 current_ring_node_f0 = current_ring_node_f0->next;
353 353 if ( (waveform_picker_regs->status & 0x01) == 0x01)
354 354 {
355 355
356 356 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
357 357 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
358 358 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
359 359 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
360 360 }
361 361 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
362 362 {
363 363 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
364 364 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
365 365 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
366 366 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
367 367 }
368 368 }
369 369
370 370 //***
371 371 // F1
372 372 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bit
373 373 swf_f1_ready = true;
374 374 ring_node_to_send_swf_f1 = current_ring_node_f1->previous;
375 375 current_ring_node_f1 = current_ring_node_f1->next;
376 376 if ( (waveform_picker_regs->status & 0x04) == 0x04)
377 377 {
378 378 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
379 379 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
380 380 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
381 381 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
382 382 }
383 383 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
384 384 {
385 385 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
386 386 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
387 387 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
388 388 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
389 389 }
390 390 }
391 391 }
392 392
393 393 rtems_isr waveforms_isr( rtems_vector_number vector )
394 394 {
395 395 /** This is the interrupt sub routine called by the waveform picker core.
396 396 *
397 397 * This ISR launch different actions depending mainly on two pieces of information:
398 398 * 1. the values read in the registers of the waveform picker.
399 399 * 2. the current LFR mode.
400 400 *
401 401 */
402 402
403 403 // STATUS
404 404 // new error error buffer full
405 405 // 15 14 13 12 11 10 9 8
406 406 // f3 f2 f1 f0 f3 f2 f1 f0
407 407 //
408 408 // ready buffer
409 409 // 7 6 5 4 3 2 1 0
410 410 // f3_1 f3_0 f2_1 f2_0 f1_1 f1_0 f0_1 f0_0
411 411
412 412 rtems_status_code spare_status;
413 413
414 414 waveforms_isr_f3();
415 415
416 416 if ( (waveform_picker_regs->status & 0xff00) != 0x00) // [1111 1111 0000 0000] check the error bits
417 417 {
418 418 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_10 );
419 419 }
420 420
421 421 switch(lfrCurrentMode)
422 422 {
423 423 //********
424 424 // STANDBY
425 425 case(LFR_MODE_STANDBY):
426 426 break;
427 427
428 428 //******
429 429 // NORMAL
430 430 case(LFR_MODE_NORMAL):
431 431 waveforms_isr_normal();
432 432 break;
433 433
434 434 //******
435 435 // BURST
436 436 case(LFR_MODE_BURST):
437 437 waveforms_isr_burst();
438 438 break;
439 439
440 440 //*****
441 441 // SBM1
442 442 case(LFR_MODE_SBM1):
443 443 waveforms_isr_sbm1();
444 444 break;
445 445
446 446 //*****
447 447 // SBM2
448 448 case(LFR_MODE_SBM2):
449 449 waveforms_isr_sbm2();
450 450 break;
451 451
452 452 //********
453 453 // DEFAULT
454 454 default:
455 455 break;
456 456 }
457 457 }
458 458
459 459 //************
460 460 // RTEMS TASKS
461 461
462 462 rtems_task wfrm_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
463 463 {
464 464 /** This RTEMS task is dedicated to the transmission of snapshots of the NORMAL mode.
465 465 *
466 466 * @param unused is the starting argument of the RTEMS task
467 467 *
468 468 * The following data packets are sent by this task:
469 469 * - TM_LFR_SCIENCE_NORMAL_SWF_F0
470 470 * - TM_LFR_SCIENCE_NORMAL_SWF_F1
471 471 * - TM_LFR_SCIENCE_NORMAL_SWF_F2
472 472 *
473 473 */
474 474
475 475 rtems_event_set event_out;
476 476 rtems_id queue_id;
477 477 rtems_status_code status;
478 478 bool resynchronisationEngaged;
479 479 ring_node *ring_node_wf_snap_extracted_ptr;
480 480
481 481 ring_node_wf_snap_extracted_ptr = (ring_node *) &ring_node_wf_snap_extracted;
482 482
483 483 resynchronisationEngaged = false;
484 484
485 485 status = get_message_queue_id_send( &queue_id );
486 486 if (status != RTEMS_SUCCESSFUL)
487 487 {
488 488 PRINTF1("in WFRM *** ERR get_message_queue_id_send %d\n", status)
489 489 }
490 490
491 491 BOOT_PRINTF("in WFRM ***\n")
492 492
493 493 while(1){
494 494 // wait for an RTEMS_EVENT
495 495 rtems_event_receive(RTEMS_EVENT_MODE_NORMAL | RTEMS_EVENT_MODE_SBM1
496 496 | RTEMS_EVENT_MODE_SBM2 | RTEMS_EVENT_MODE_SBM2_WFRM,
497 497 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
498 498 if(resynchronisationEngaged == false)
499 499 { // engage resynchronisation
500 500 snapshot_resynchronization( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
501 501 resynchronisationEngaged = true;
502 502 }
503 503 else
504 504 { // reset delta_snapshot to the nominal value
505 505 PRINTF("no resynchronisation, reset delta_snapshot to the nominal value\n")
506 506 set_wfp_delta_snapshot();
507 507 resynchronisationEngaged = false;
508 508 }
509 509 //
510 510
511 511 if (event_out == RTEMS_EVENT_MODE_NORMAL)
512 512 {
513 513 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_NORMAL\n")
514 514 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
515 515 ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1;
516 516 ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2;
517 517 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
518 518 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) );
519 519 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) );
520 520 }
521 521 if (event_out == RTEMS_EVENT_MODE_SBM1)
522 522 {
523 523 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM1\n")
524 524 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
525 525 ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F1;
526 526 ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2;
527 527 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
528 528 status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) );
529 529 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) );
530 530 }
531 531 if (event_out == RTEMS_EVENT_MODE_SBM2)
532 532 {
533 533 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM2\n")
534 534 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
535 535 ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1;
536 536 ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F2;
537 537 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
538 538 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) );
539 539 status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) );
540 540 }
541 541 }
542 542 }
543 543
544 544 rtems_task cwf3_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
545 545 {
546 546 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f3.
547 547 *
548 548 * @param unused is the starting argument of the RTEMS task
549 549 *
550 550 * The following data packet is sent by this task:
551 551 * - TM_LFR_SCIENCE_NORMAL_CWF_F3
552 552 *
553 553 */
554 554
555 555 rtems_event_set event_out;
556 556 rtems_id queue_id;
557 557 rtems_status_code status;
558 558 ring_node ring_node_cwf3_light;
559 559
560 560 status = get_message_queue_id_send( &queue_id );
561 561 if (status != RTEMS_SUCCESSFUL)
562 562 {
563 563 PRINTF1("in CWF3 *** ERR get_message_queue_id_send %d\n", status)
564 564 }
565 565
566 566 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
567 567
568 568 // init the ring_node_cwf3_light structure
569 569 ring_node_cwf3_light.buffer_address = (int) wf_cont_f3_light;
570 570 ring_node_cwf3_light.coarseTime = 0x00;
571 571 ring_node_cwf3_light.fineTime = 0x00;
572 572 ring_node_cwf3_light.next = NULL;
573 573 ring_node_cwf3_light.previous = NULL;
574 574 ring_node_cwf3_light.sid = SID_NORM_CWF_F3;
575 575 ring_node_cwf3_light.status = 0x00;
576 576
577 577 BOOT_PRINTF("in CWF3 ***\n")
578 578
579 579 while(1){
580 580 // wait for an RTEMS_EVENT
581 581 rtems_event_receive( RTEMS_EVENT_0,
582 582 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
583 583 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
584 584 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode==LFR_MODE_SBM2) )
585 585 {
586 586 if ( (parameter_dump_packet.sy_lfr_n_cwf_long_f3 & 0x01) == 0x01)
587 587 {
588 588 PRINTF("send CWF_LONG_F3\n")
589 589 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
590 590 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf_f3, sizeof( ring_node* ) );
591 591 }
592 592 else
593 593 {
594 594 PRINTF("send CWF_F3 (light)\n")
595 595 send_waveform_CWF3_light( ring_node_to_send_cwf_f3, &ring_node_cwf3_light, queue_id );
596 596 }
597 597
598 598 }
599 599 else
600 600 {
601 601 PRINTF1("in CWF3 *** lfrCurrentMode is %d, no data will be sent\n", lfrCurrentMode)
602 602 }
603 603 }
604 604 }
605 605
606 606 rtems_task cwf2_task(rtems_task_argument argument) // ONLY USED IN BURST AND SBM2
607 607 {
608 608 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f2.
609 609 *
610 610 * @param unused is the starting argument of the RTEMS task
611 611 *
612 612 * The following data packet is sent by this function:
613 613 * - TM_LFR_SCIENCE_BURST_CWF_F2
614 614 * - TM_LFR_SCIENCE_SBM2_CWF_F2
615 615 *
616 616 */
617 617
618 618 rtems_event_set event_out;
619 619 rtems_id queue_id;
620 620 rtems_status_code status;
621 621 ring_node *ring_node_to_send;
622 622 unsigned long long int acquisitionTimeF0_asLong;
623 623
624 624 acquisitionTimeF0_asLong = 0x00;
625 625
626 626 status = get_message_queue_id_send( &queue_id );
627 627 if (status != RTEMS_SUCCESSFUL)
628 628 {
629 629 PRINTF1("in CWF2 *** ERR get_message_queue_id_send %d\n", status)
630 630 }
631 631
632 632 BOOT_PRINTF("in CWF2 ***\n")
633 633
634 634 while(1){
635 635 // wait for an RTEMS_EVENT
636 636 rtems_event_receive( RTEMS_EVENT_MODE_BURST | RTEMS_EVENT_MODE_SBM2,
637 637 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
638 638 ring_node_to_send = getRingNodeToSendCWF( 2 );
639 639 if (event_out == RTEMS_EVENT_MODE_BURST)
640 640 {
641 641 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
642 642 }
643 643 if (event_out == RTEMS_EVENT_MODE_SBM2)
644 644 {
645 645 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
646 646 // launch snapshot extraction if needed
647 647 if (extractSWF == true)
648 648 {
649 649 ring_node_to_send_swf_f2 = ring_node_to_send_cwf_f2;
650 650 // extract the snapshot
651 651 build_snapshot_from_ring( ring_node_to_send_swf_f2, 2, acquisitionTimeF0_asLong );
652 652 // send the snapshot when built
653 653 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM2 );
654 654 extractSWF = false;
655 655 }
656 656 if (swf_f0_ready && swf_f1_ready)
657 657 {
658 658 extractSWF = true;
659 659 // record the acquition time of the fΓ  snapshot to use to build the snapshot at f2
660 660 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
661 661 swf_f0_ready = false;
662 662 swf_f1_ready = false;
663 663 }
664 664 }
665 665 }
666 666 }
667 667
668 668 rtems_task cwf1_task(rtems_task_argument argument) // ONLY USED IN SBM1
669 669 {
670 670 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f1.
671 671 *
672 672 * @param unused is the starting argument of the RTEMS task
673 673 *
674 674 * The following data packet is sent by this function:
675 675 * - TM_LFR_SCIENCE_SBM1_CWF_F1
676 676 *
677 677 */
678 678
679 679 rtems_event_set event_out;
680 680 rtems_id queue_id;
681 681 rtems_status_code status;
682 682
683 ring_node * ring_node_to_send_cwf;
683 ring_node *ring_node_to_send_cwf;
684 684
685 685 status = get_message_queue_id_send( &queue_id );
686 686 if (status != RTEMS_SUCCESSFUL)
687 687 {
688 688 PRINTF1("in CWF1 *** ERR get_message_queue_id_send %d\n", status)
689 689 }
690 690
691 691 BOOT_PRINTF("in CWF1 ***\n")
692 692
693 693 while(1){
694 694 // wait for an RTEMS_EVENT
695 695 rtems_event_receive( RTEMS_EVENT_MODE_SBM1,
696 696 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
697 697 ring_node_to_send_cwf = getRingNodeToSendCWF( 1 );
698 698 ring_node_to_send_cwf_f1->sid = SID_SBM1_CWF_F1;
699 699 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
700 700 // launch snapshot extraction if needed
701 701 if (extractSWF == true)
702 702 {
703 703 ring_node_to_send_swf_f1 = ring_node_to_send_cwf;
704 704 // launch the snapshot extraction
705 705 status = rtems_event_send( Task_id[TASKID_SWBD], RTEMS_EVENT_MODE_SBM1 );
706 706 extractSWF = false;
707 707 }
708 708 if (swf_f0_ready == true)
709 709 {
710 710 extractSWF = true;
711 711 swf_f0_ready = false; // this step shall be executed only one time
712 712 }
713 713 if ((swf_f1_ready == true) && (swf_f2_ready == true)) // swf_f1 is ready after the extraction
714 714 {
715 715 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM1 );
716 716 swf_f1_ready = false;
717 717 swf_f2_ready = false;
718 718 }
719 719 }
720 720 }
721 721
722 722 rtems_task swbd_task(rtems_task_argument argument)
723 723 {
724 724 /** This RTEMS task is dedicated to the building of snapshots from different continuous waveforms buffers.
725 725 *
726 726 * @param unused is the starting argument of the RTEMS task
727 727 *
728 728 */
729 729
730 730 rtems_event_set event_out;
731 731 unsigned long long int acquisitionTimeF0_asLong;
732 732
733 733 acquisitionTimeF0_asLong = 0x00;
734 734
735 735 BOOT_PRINTF("in SWBD ***\n")
736 736
737 737 while(1){
738 738 // wait for an RTEMS_EVENT
739 739 rtems_event_receive( RTEMS_EVENT_MODE_SBM1 | RTEMS_EVENT_MODE_SBM2,
740 740 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
741 741 if (event_out == RTEMS_EVENT_MODE_SBM1)
742 742 {
743 743 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
744 744 build_snapshot_from_ring( ring_node_to_send_swf_f1, 1, acquisitionTimeF0_asLong );
745 745 swf_f1_ready = true; // the snapshot has been extracted and is ready to be sent
746 746 }
747 747 else
748 748 {
749 749 PRINTF1("in SWBD *** unexpected rtems event received %x\n", (int) event_out)
750 750 }
751 751 }
752 752 }
753 753
754 754 //******************
755 755 // general functions
756 756
757 757 void WFP_init_rings( void )
758 758 {
759 759 // F0 RING
760 760 init_ring( waveform_ring_f0, NB_RING_NODES_F0, wf_buffer_f0, WFRM_BUFFER );
761 761 // F1 RING
762 762 init_ring( waveform_ring_f1, NB_RING_NODES_F1, wf_buffer_f1, WFRM_BUFFER );
763 763 // F2 RING
764 764 init_ring( waveform_ring_f2, NB_RING_NODES_F2, wf_buffer_f2, WFRM_BUFFER );
765 765 // F3 RING
766 766 init_ring( waveform_ring_f3, NB_RING_NODES_F3, wf_buffer_f3, WFRM_BUFFER );
767 767
768 768 ring_node_wf_snap_extracted.buffer_address = (int) wf_snap_extracted;
769 769
770 770 DEBUG_PRINTF1("waveform_ring_f0 @%x\n", (unsigned int) waveform_ring_f0)
771 771 DEBUG_PRINTF1("waveform_ring_f1 @%x\n", (unsigned int) waveform_ring_f1)
772 772 DEBUG_PRINTF1("waveform_ring_f2 @%x\n", (unsigned int) waveform_ring_f2)
773 773 DEBUG_PRINTF1("waveform_ring_f3 @%x\n", (unsigned int) waveform_ring_f3)
774 774 DEBUG_PRINTF1("wf_buffer_f0 @%x\n", (unsigned int) wf_buffer_f0)
775 775 DEBUG_PRINTF1("wf_buffer_f1 @%x\n", (unsigned int) wf_buffer_f1)
776 776 DEBUG_PRINTF1("wf_buffer_f2 @%x\n", (unsigned int) wf_buffer_f2)
777 777 DEBUG_PRINTF1("wf_buffer_f3 @%x\n", (unsigned int) wf_buffer_f3)
778 778
779 779 }
780 780
781 781 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
782 782 {
783 783 unsigned char i;
784 784
785 785 //***************
786 786 // BUFFER ADDRESS
787 787 for(i=0; i<nbNodes; i++)
788 788 {
789 789 ring[i].coarseTime = 0x00;
790 790 ring[i].fineTime = 0x00;
791 791 ring[i].sid = 0x00;
792 792 ring[i].status = 0x00;
793 793 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
794 794 }
795 795
796 796 //*****
797 797 // NEXT
798 798 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
799 799 for(i=0; i<nbNodes-1; i++)
800 800 {
801 801 ring[i].next = (ring_node*) &ring[ i + 1 ];
802 802 }
803 803
804 804 //*********
805 805 // PREVIOUS
806 806 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
807 807 for(i=1; i<nbNodes; i++)
808 808 {
809 809 ring[i].previous = (ring_node*) &ring[ i - 1 ];
810 810 }
811 811 }
812 812
813 813 void WFP_reset_current_ring_nodes( void )
814 814 {
815 815 current_ring_node_f0 = waveform_ring_f0[0].next;
816 816 current_ring_node_f1 = waveform_ring_f1[0].next;
817 817 current_ring_node_f2 = waveform_ring_f2[0].next;
818 818 current_ring_node_f3 = waveform_ring_f3[0].next;
819 819
820 820 ring_node_to_send_swf_f0 = waveform_ring_f0;
821 821 ring_node_to_send_swf_f1 = waveform_ring_f1;
822 822 ring_node_to_send_swf_f2 = waveform_ring_f2;
823 823
824 824 ring_node_to_send_cwf_f1 = waveform_ring_f1;
825 825 ring_node_to_send_cwf_f2 = waveform_ring_f2;
826 826 ring_node_to_send_cwf_f3 = waveform_ring_f3;
827 827 }
828 828
829 829 int send_waveform_CWF3_light( ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id )
830 830 {
831 831 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
832 832 *
833 833 * @param waveform points to the buffer containing the data that will be send.
834 834 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
835 835 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
836 836 * contain information to setup the transmission of the data packets.
837 837 *
838 838 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
839 839 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
840 840 *
841 841 */
842 842
843 843 unsigned int i;
844 844 int ret;
845 845 rtems_status_code status;
846 846
847 847 char *sample;
848 848 int *dataPtr;
849 849
850 850 ret = LFR_DEFAULT;
851 851
852 852 dataPtr = (int*) ring_node_to_send->buffer_address;
853 853
854 854 ring_node_cwf3_light->coarseTime = ring_node_to_send->coarseTime;
855 855 ring_node_cwf3_light->fineTime = ring_node_to_send->fineTime;
856 856
857 857 //**********************
858 858 // BUILD CWF3_light DATA
859 859 for ( i=0; i< NB_SAMPLES_PER_SNAPSHOT; i++)
860 860 {
861 861 sample = (char*) &dataPtr[ (i * NB_WORDS_SWF_BLK) ];
862 862 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) ] = sample[ 0 ];
863 863 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 1 ] = sample[ 1 ];
864 864 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 2 ] = sample[ 2 ];
865 865 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 3 ] = sample[ 3 ];
866 866 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 4 ] = sample[ 4 ];
867 867 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 5 ] = sample[ 5 ];
868 868 }
869 869
870 870 // SEND PACKET
871 871 status = rtems_message_queue_send( queue_id, &ring_node_cwf3_light, sizeof( ring_node* ) );
872 872 if (status != RTEMS_SUCCESSFUL) {
873 873 printf("%d-%d, ERR %d\n", SID_NORM_CWF_F3, i, (int) status);
874 874 ret = LFR_DEFAULT;
875 875 }
876 876
877 877 return ret;
878 878 }
879 879
880 880 void compute_acquisition_time( unsigned int coarseTime, unsigned int fineTime,
881 881 unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char * acquisitionTime )
882 882 {
883 883 unsigned long long int acquisitionTimeAsLong;
884 884 unsigned char localAcquisitionTime[6];
885 885 double deltaT;
886 886
887 887 deltaT = 0.;
888 888
889 889 localAcquisitionTime[0] = (unsigned char) ( coarseTime >> 24 );
890 890 localAcquisitionTime[1] = (unsigned char) ( coarseTime >> 16 );
891 891 localAcquisitionTime[2] = (unsigned char) ( coarseTime >> 8 );
892 892 localAcquisitionTime[3] = (unsigned char) ( coarseTime );
893 893 localAcquisitionTime[4] = (unsigned char) ( fineTime >> 8 );
894 894 localAcquisitionTime[5] = (unsigned char) ( fineTime );
895 895
896 896 acquisitionTimeAsLong = ( (unsigned long long int) localAcquisitionTime[0] << 40 )
897 897 + ( (unsigned long long int) localAcquisitionTime[1] << 32 )
898 898 + ( (unsigned long long int) localAcquisitionTime[2] << 24 )
899 899 + ( (unsigned long long int) localAcquisitionTime[3] << 16 )
900 900 + ( (unsigned long long int) localAcquisitionTime[4] << 8 )
901 901 + ( (unsigned long long int) localAcquisitionTime[5] );
902 902
903 903 switch( sid )
904 904 {
905 905 case SID_NORM_SWF_F0:
906 906 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 24576. ;
907 907 break;
908 908
909 909 case SID_NORM_SWF_F1:
910 910 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 4096. ;
911 911 break;
912 912
913 913 case SID_NORM_SWF_F2:
914 914 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 256. ;
915 915 break;
916 916
917 917 case SID_SBM1_CWF_F1:
918 918 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 4096. ;
919 919 break;
920 920
921 921 case SID_SBM2_CWF_F2:
922 922 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
923 923 break;
924 924
925 925 case SID_BURST_CWF_F2:
926 926 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
927 927 break;
928 928
929 929 case SID_NORM_CWF_F3:
930 930 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF_SHORT_F3 * 65536. / 16. ;
931 931 break;
932 932
933 933 case SID_NORM_CWF_LONG_F3:
934 934 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 16. ;
935 935 break;
936 936
937 937 default:
938 938 PRINTF1("in compute_acquisition_time *** ERR unexpected sid %d\n", sid)
939 939 deltaT = 0.;
940 940 break;
941 941 }
942 942
943 943 acquisitionTimeAsLong = acquisitionTimeAsLong + (unsigned long long int) deltaT;
944 944 //
945 945 acquisitionTime[0] = (unsigned char) (acquisitionTimeAsLong >> 40);
946 946 acquisitionTime[1] = (unsigned char) (acquisitionTimeAsLong >> 32);
947 947 acquisitionTime[2] = (unsigned char) (acquisitionTimeAsLong >> 24);
948 948 acquisitionTime[3] = (unsigned char) (acquisitionTimeAsLong >> 16);
949 949 acquisitionTime[4] = (unsigned char) (acquisitionTimeAsLong >> 8 );
950 950 acquisitionTime[5] = (unsigned char) (acquisitionTimeAsLong );
951 951
952 952 }
953 953
954 954 void build_snapshot_from_ring( ring_node *ring_node_to_send, unsigned char frequencyChannel, unsigned long long int acquisitionTimeF0_asLong )
955 955 {
956 956 unsigned int i;
957 957 unsigned long long int centerTime_asLong;
958 958 unsigned long long int acquisitionTime_asLong;
959 959 unsigned long long int bufferAcquisitionTime_asLong;
960 960 unsigned char *ptr1;
961 961 unsigned char *ptr2;
962 962 unsigned char *timeCharPtr;
963 963 unsigned char nb_ring_nodes;
964 964 unsigned long long int frequency_asLong;
965 965 unsigned long long int nbTicksPerSample_asLong;
966 966 unsigned long long int nbSamplesPart1_asLong;
967 967 unsigned long long int sampleOffset_asLong;
968 968
969 969 unsigned int deltaT_F0;
970 970 unsigned int deltaT_F1;
971 971 unsigned long long int deltaT_F2;
972 972
973 973 deltaT_F0 = 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
974 974 deltaT_F1 = 16384; // (2048. / 4096. / 2.) * 65536. = 16384;
975 975 deltaT_F2 = 262144; // (2048. / 256. / 2.) * 65536. = 262144;
976 976 sampleOffset_asLong = 0x00;
977 977
978 978 // (1) get the f0 acquisition time => the value is passed in argument
979 979
980 980 // (2) compute the central reference time
981 981 centerTime_asLong = acquisitionTimeF0_asLong + deltaT_F0;
982 982
983 983 // (3) compute the acquisition time of the current snapshot
984 984 switch(frequencyChannel)
985 985 {
986 986 case 1: // 1 is for F1 = 4096 Hz
987 987 acquisitionTime_asLong = centerTime_asLong - deltaT_F1;
988 988 nb_ring_nodes = NB_RING_NODES_F1;
989 989 frequency_asLong = 4096;
990 990 nbTicksPerSample_asLong = 16; // 65536 / 4096;
991 991 break;
992 992 case 2: // 2 is for F2 = 256 Hz
993 993 acquisitionTime_asLong = centerTime_asLong - deltaT_F2;
994 994 nb_ring_nodes = NB_RING_NODES_F2;
995 995 frequency_asLong = 256;
996 996 nbTicksPerSample_asLong = 256; // 65536 / 256;
997 997 break;
998 998 default:
999 999 acquisitionTime_asLong = centerTime_asLong;
1000 1000 frequency_asLong = 256;
1001 1001 nbTicksPerSample_asLong = 256;
1002 1002 break;
1003 1003 }
1004 1004
1005 1005 //****************************************************************************
1006 1006 // (4) search the ring_node with the acquisition time <= acquisitionTime_asLong
1007 1007 for (i=0; i<nb_ring_nodes; i++)
1008 1008 {
1009 1009 PRINTF1("%d ... ", i)
1010 1010 bufferAcquisitionTime_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send->coarseTime );
1011 1011 if (bufferAcquisitionTime_asLong <= acquisitionTime_asLong)
1012 1012 {
1013 1013 PRINTF1("buffer found with acquisition time = %llx\n", bufferAcquisitionTime_asLong)
1014 1014 break;
1015 1015 }
1016 1016 ring_node_to_send = ring_node_to_send->previous;
1017 1017 }
1018 1018
1019 1019 // (5) compute the number of samples to take in the current buffer
1020 1020 sampleOffset_asLong = ((acquisitionTime_asLong - bufferAcquisitionTime_asLong) * frequency_asLong ) >> 16;
1021 1021 nbSamplesPart1_asLong = NB_SAMPLES_PER_SNAPSHOT - sampleOffset_asLong;
1022 1022 PRINTF2("sampleOffset_asLong = %lld, nbSamplesPart1_asLong = %lld\n", sampleOffset_asLong, nbSamplesPart1_asLong)
1023 1023
1024 1024 // (6) compute the final acquisition time
1025 1025 acquisitionTime_asLong = bufferAcquisitionTime_asLong +
1026 1026 sampleOffset_asLong * nbTicksPerSample_asLong;
1027 1027
1028 1028 // (7) copy the acquisition time at the beginning of the extrated snapshot
1029 1029 ptr1 = (unsigned char*) &acquisitionTime_asLong;
1030 1030 // fine time
1031 1031 ptr2 = (unsigned char*) &ring_node_wf_snap_extracted.fineTime;
1032 1032 ptr2[2] = ptr1[ 4 + 2 ];
1033 1033 ptr2[3] = ptr1[ 5 + 2 ];
1034 1034 // coarse time
1035 1035 ptr2 = (unsigned char*) &ring_node_wf_snap_extracted.coarseTime;
1036 1036 ptr2[0] = ptr1[ 0 + 2 ];
1037 1037 ptr2[1] = ptr1[ 1 + 2 ];
1038 1038 ptr2[2] = ptr1[ 2 + 2 ];
1039 1039 ptr2[3] = ptr1[ 3 + 2 ];
1040 1040
1041 1041 // re set the synchronization bit
1042 1042 timeCharPtr = (unsigned char*) &ring_node_to_send->coarseTime;
1043 1043 ptr2[0] = ptr2[0] | (timeCharPtr[0] & 0x80); // [1000 0000]
1044 1044
1045 1045 if ( (nbSamplesPart1_asLong >= NB_SAMPLES_PER_SNAPSHOT) | (nbSamplesPart1_asLong < 0) )
1046 1046 {
1047 1047 nbSamplesPart1_asLong = 0;
1048 1048 }
1049 1049 // copy the part 1 of the snapshot in the extracted buffer
1050 1050 for ( i = 0; i < (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i++ )
1051 1051 {
1052 1052 wf_snap_extracted[i] =
1053 1053 ((int*) ring_node_to_send->buffer_address)[ i + (sampleOffset_asLong * NB_WORDS_SWF_BLK) ];
1054 1054 }
1055 1055 // copy the part 2 of the snapshot in the extracted buffer
1056 1056 ring_node_to_send = ring_node_to_send->next;
1057 1057 for ( i = (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i < (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK); i++ )
1058 1058 {
1059 1059 wf_snap_extracted[i] =
1060 1060 ((int*) ring_node_to_send->buffer_address)[ (i-(nbSamplesPart1_asLong * NB_WORDS_SWF_BLK)) ];
1061 1061 }
1062 1062 }
1063 1063
1064 1064 void snapshot_resynchronization( unsigned char *timePtr )
1065 1065 {
1066 1066 unsigned long long int acquisitionTime;
1067 1067 unsigned long long int centerTime;
1068 1068 unsigned long long int previousTick;
1069 1069 unsigned long long int nextTick;
1070 1070 unsigned long long int deltaPreviousTick;
1071 1071 unsigned long long int deltaNextTick;
1072 1072 unsigned int deltaTickInF2;
1073 1073 double deltaPrevious;
1074 1074 double deltaNext;
1075 1075
1076 1076 acquisitionTime = get_acquisition_time( timePtr );
1077 1077
1078 1078 // compute center time
1079 1079 centerTime = acquisitionTime + 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
1080 1080 previousTick = centerTime - (centerTime & 0xffff);
1081 1081 nextTick = previousTick + 65536;
1082 1082
1083 1083 deltaPreviousTick = centerTime - previousTick;
1084 1084 deltaNextTick = nextTick - centerTime;
1085 1085
1086 1086 deltaPrevious = ((double) deltaPreviousTick) / 65536. * 1000.;
1087 1087 deltaNext = ((double) deltaNextTick) / 65536. * 1000.;
1088 1088
1089 1089 PRINTF2("delta previous = %f ms, delta next = %f ms\n", deltaPrevious, deltaNext)
1090 1090 PRINTF2("delta previous = %llu, delta next = %llu\n", deltaPreviousTick, deltaNextTick)
1091 1091
1092 1092 // which tick is the closest
1093 1093 if (deltaPreviousTick > deltaNextTick)
1094 1094 {
1095 1095 deltaTickInF2 = floor( (deltaNext * 256. / 1000.) ); // the division by 2 is important here
1096 1096 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot + deltaTickInF2;
1097 1097 printf("correction of = + %u\n", deltaTickInF2);
1098 1098 }
1099 1099 else
1100 1100 {
1101 1101 deltaTickInF2 = floor( (deltaPrevious * 256. / 1000.) ); // the division by 2 is important here
1102 1102 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot - deltaTickInF2;
1103 1103 printf("correction of = - %u\n", deltaTickInF2);
1104 1104 }
1105 1105 }
1106 1106
1107 1107 //**************
1108 1108 // wfp registers
1109 1109 void reset_wfp_burst_enable( void )
1110 1110 {
1111 1111 /** This function resets the waveform picker burst_enable register.
1112 1112 *
1113 1113 * The burst bits [f2 f1 f0] and the enable bits [f3 f2 f1 f0] are set to 0.
1114 1114 *
1115 1115 */
1116 1116
1117 1117 // [1000 000] burst f2, f1, f0 enable f3, f2, f1, f0
1118 1118 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable & 0x80;
1119 1119 }
1120 1120
1121 1121 void reset_wfp_status( void )
1122 1122 {
1123 1123 /** This function resets the waveform picker status register.
1124 1124 *
1125 1125 * All status bits are set to 0 [new_err full_err full].
1126 1126 *
1127 1127 */
1128 1128
1129 1129 waveform_picker_regs->status = 0xffff;
1130 1130 }
1131 1131
1132 1132 void reset_wfp_buffer_addresses( void )
1133 1133 {
1134 1134 // F0
1135 1135 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->previous->buffer_address; // 0x08
1136 1136 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address; // 0x0c
1137 1137 // F1
1138 1138 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->previous->buffer_address; // 0x10
1139 1139 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address; // 0x14
1140 1140 // F2
1141 1141 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->previous->buffer_address; // 0x18
1142 1142 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; // 0x1c
1143 1143 // F3
1144 1144 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->previous->buffer_address; // 0x20
1145 1145 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address; // 0x24
1146 1146 }
1147 1147
1148 1148 void reset_waveform_picker_regs( void )
1149 1149 {
1150 1150 /** This function resets the waveform picker module registers.
1151 1151 *
1152 1152 * The registers affected by this function are located at the following offset addresses:
1153 1153 * - 0x00 data_shaping
1154 1154 * - 0x04 run_burst_enable
1155 1155 * - 0x08 addr_data_f0
1156 1156 * - 0x0C addr_data_f1
1157 1157 * - 0x10 addr_data_f2
1158 1158 * - 0x14 addr_data_f3
1159 1159 * - 0x18 status
1160 1160 * - 0x1C delta_snapshot
1161 1161 * - 0x20 delta_f0
1162 1162 * - 0x24 delta_f0_2
1163 1163 * - 0x28 delta_f1
1164 1164 * - 0x2c delta_f2
1165 1165 * - 0x30 nb_data_by_buffer
1166 1166 * - 0x34 nb_snapshot_param
1167 1167 * - 0x38 start_date
1168 1168 * - 0x3c nb_word_in_buffer
1169 1169 *
1170 1170 */
1171 1171
1172 1172 set_wfp_data_shaping(); // 0x00 *** R1 R0 SP1 SP0 BW
1173 1173
1174 1174 reset_wfp_burst_enable(); // 0x04 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
1175 1175
1176 1176 reset_wfp_buffer_addresses();
1177 1177
1178 1178 reset_wfp_status(); // 0x18
1179 1179
1180 1180 set_wfp_delta_snapshot(); // 0x1c *** 300 s => 0x12bff
1181 1181
1182 1182 set_wfp_delta_f0_f0_2(); // 0x20, 0x24
1183 1183
1184 1184 set_wfp_delta_f1(); // 0x28
1185 1185
1186 1186 set_wfp_delta_f2(); // 0x2c
1187 1187
1188 1188 DEBUG_PRINTF1("delta_snapshot %x\n", waveform_picker_regs->delta_snapshot)
1189 1189 DEBUG_PRINTF1("delta_f0 %x\n", waveform_picker_regs->delta_f0)
1190 1190 DEBUG_PRINTF1("delta_f0_2 %x\n", waveform_picker_regs->delta_f0_2)
1191 1191 DEBUG_PRINTF1("delta_f1 %x\n", waveform_picker_regs->delta_f1)
1192 1192 DEBUG_PRINTF1("delta_f2 %x\n", waveform_picker_regs->delta_f2)
1193 1193 // 2688 = 8 * 336
1194 1194 waveform_picker_regs->nb_data_by_buffer = 0xa7f; // 0x30 *** 2688 - 1 => nb samples -1
1195 1195 waveform_picker_regs->snapshot_param = 0xa80; // 0x34 *** 2688 => nb samples
1196 1196 waveform_picker_regs->start_date = 0x7fffffff; // 0x38
1197 1197 //
1198 1198 // coarse time and fine time registers are not initialized, they are volatile
1199 1199 //
1200 1200 waveform_picker_regs->buffer_length = 0x1f8;// buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
1201 1201 }
1202 1202
1203 1203 void set_wfp_data_shaping( void )
1204 1204 {
1205 1205 /** This function sets the data_shaping register of the waveform picker module.
1206 1206 *
1207 1207 * The value is read from one field of the parameter_dump_packet structure:\n
1208 1208 * bw_sp0_sp1_r0_r1
1209 1209 *
1210 1210 */
1211 1211
1212 1212 unsigned char data_shaping;
1213 1213
1214 1214 // get the parameters for the data shaping [BW SP0 SP1 R0 R1] in sy_lfr_common1 and configure the register
1215 1215 // waveform picker : [R1 R0 SP1 SP0 BW]
1216 1216
1217 1217 data_shaping = parameter_dump_packet.sy_lfr_common_parameters;
1218 1218
1219 1219 waveform_picker_regs->data_shaping =
1220 1220 ( (data_shaping & 0x10) >> 4 ) // BW
1221 1221 + ( (data_shaping & 0x08) >> 2 ) // SP0
1222 1222 + ( (data_shaping & 0x04) ) // SP1
1223 1223 + ( (data_shaping & 0x02) << 2 ) // R0
1224 1224 + ( (data_shaping & 0x01) << 4 ); // R1
1225 1225
1226 1226 // this is a temporary way to set R2, compatible with the release 2 of the flight software
1227 1227 waveform_picker_regs->data_shaping = waveform_picker_regs->data_shaping + ( (0x1) << 5 ); // R2
1228 1228 }
1229 1229
1230 1230 void set_wfp_burst_enable_register( unsigned char mode )
1231 1231 {
1232 1232 /** This function sets the waveform picker burst_enable register depending on the mode.
1233 1233 *
1234 1234 * @param mode is the LFR mode to launch.
1235 1235 *
1236 1236 * The burst bits shall be before the enable bits.
1237 1237 *
1238 1238 */
1239 1239
1240 1240 // [0000 0000] burst f2, f1, f0 enable f3 f2 f1 f0
1241 1241 // the burst bits shall be set first, before the enable bits
1242 1242 switch(mode) {
1243 1243 case(LFR_MODE_NORMAL):
1244 1244 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enable
1245 1245 waveform_picker_regs->run_burst_enable = 0x0f; // [0000 1111] enable f3 f2 f1 f0
1246 1246 break;
1247 1247 case(LFR_MODE_BURST):
1248 1248 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1249 1249 // waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x04; // [0100] enable f2
1250 1250 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0c; // [1100] enable f3 AND f2
1251 1251 break;
1252 1252 case(LFR_MODE_SBM1):
1253 1253 waveform_picker_regs->run_burst_enable = 0x20; // [0010 0000] f1 burst enabled
1254 1254 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1255 1255 break;
1256 1256 case(LFR_MODE_SBM2):
1257 1257 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1258 1258 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1259 1259 break;
1260 1260 default:
1261 1261 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enabled, no waveform enabled
1262 1262 break;
1263 1263 }
1264 1264 }
1265 1265
1266 1266 void set_wfp_delta_snapshot( void )
1267 1267 {
1268 1268 /** This function sets the delta_snapshot register of the waveform picker module.
1269 1269 *
1270 1270 * The value is read from two (unsigned char) of the parameter_dump_packet structure:
1271 1271 * - sy_lfr_n_swf_p[0]
1272 1272 * - sy_lfr_n_swf_p[1]
1273 1273 *
1274 1274 */
1275 1275
1276 1276 unsigned int delta_snapshot;
1277 1277 unsigned int delta_snapshot_in_T2;
1278 1278
1279 1279 delta_snapshot = parameter_dump_packet.sy_lfr_n_swf_p[0]*256
1280 1280 + parameter_dump_packet.sy_lfr_n_swf_p[1];
1281 1281
1282 1282 delta_snapshot_in_T2 = delta_snapshot * 256;
1283 1283 waveform_picker_regs->delta_snapshot = delta_snapshot_in_T2 - 1; // max 4 bytes
1284 1284 }
1285 1285
1286 1286 void set_wfp_delta_f0_f0_2( void )
1287 1287 {
1288 1288 unsigned int delta_snapshot;
1289 1289 unsigned int nb_samples_per_snapshot;
1290 1290 float delta_f0_in_float;
1291 1291
1292 1292 delta_snapshot = waveform_picker_regs->delta_snapshot;
1293 1293 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1294 1294 delta_f0_in_float =nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 24576.) * 256.;
1295 1295
1296 1296 waveform_picker_regs->delta_f0 = delta_snapshot - floor( delta_f0_in_float );
1297 1297 waveform_picker_regs->delta_f0_2 = 0x30; // 48 = 11 0000, max 7 bits
1298 1298 }
1299 1299
1300 1300 void set_wfp_delta_f1( void )
1301 1301 {
1302 1302 unsigned int delta_snapshot;
1303 1303 unsigned int nb_samples_per_snapshot;
1304 1304 float delta_f1_in_float;
1305 1305
1306 1306 delta_snapshot = waveform_picker_regs->delta_snapshot;
1307 1307 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1308 1308 delta_f1_in_float = nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 4096.) * 256.;
1309 1309
1310 1310 waveform_picker_regs->delta_f1 = delta_snapshot - floor( delta_f1_in_float );
1311 1311 }
1312 1312
1313 1313 void set_wfp_delta_f2()
1314 1314 {
1315 1315 unsigned int delta_snapshot;
1316 1316 unsigned int nb_samples_per_snapshot;
1317 1317
1318 1318 delta_snapshot = waveform_picker_regs->delta_snapshot;
1319 1319 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1320 1320
1321 1321 waveform_picker_regs->delta_f2 = delta_snapshot - nb_samples_per_snapshot / 2;
1322 1322 }
1323 1323
1324 1324 //*****************
1325 1325 // local parameters
1326 1326
1327 1327 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid )
1328 1328 {
1329 1329 /** This function increments the parameter "sequence_cnt" depending on the sid passed in argument.
1330 1330 *
1331 1331 * @param packet_sequence_control is a pointer toward the parameter sequence_cnt to update.
1332 1332 * @param sid is the source identifier of the packet being updated.
1333 1333 *
1334 1334 * REQ-LFR-SRS-5240 / SSS-CP-FS-590
1335 1335 * The sequence counters shall wrap around from 2^14 to zero.
1336 1336 * The sequence counter shall start at zero at startup.
1337 1337 *
1338 1338 * REQ-LFR-SRS-5239 / SSS-CP-FS-580
1339 1339 * All TM_LFR_SCIENCE_ packets are sent to ground, i.e. destination id = 0
1340 1340 *
1341 1341 */
1342 1342
1343 1343 unsigned short *sequence_cnt;
1344 1344 unsigned short segmentation_grouping_flag;
1345 1345 unsigned short new_packet_sequence_control;
1346 1346 rtems_mode initial_mode_set;
1347 1347 rtems_mode current_mode_set;
1348 1348 rtems_status_code status;
1349 1349
1350 1350 //******************************************
1351 1351 // CHANGE THE MODE OF THE CALLING RTEMS TASK
1352 1352 status = rtems_task_mode( RTEMS_NO_PREEMPT, RTEMS_PREEMPT_MASK, &initial_mode_set );
1353 1353
1354 1354 if ( (sid == SID_NORM_SWF_F0) || (sid == SID_NORM_SWF_F1) || (sid == SID_NORM_SWF_F2)
1355 1355 || (sid == SID_NORM_CWF_F3) || (sid == SID_NORM_CWF_LONG_F3)
1356 1356 || (sid == SID_BURST_CWF_F2)
1357 1357 || (sid == SID_NORM_ASM_F0) || (sid == SID_NORM_ASM_F1) || (sid == SID_NORM_ASM_F2)
1358 1358 || (sid == SID_NORM_BP1_F0) || (sid == SID_NORM_BP1_F1) || (sid == SID_NORM_BP1_F2)
1359 1359 || (sid == SID_NORM_BP2_F0) || (sid == SID_NORM_BP2_F1) || (sid == SID_NORM_BP2_F2)
1360 1360 || (sid == SID_BURST_BP1_F0) || (sid == SID_BURST_BP2_F0)
1361 1361 || (sid == SID_BURST_BP1_F1) || (sid == SID_BURST_BP2_F1) )
1362 1362 {
1363 1363 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_NORMAL_BURST;
1364 1364 }
1365 1365 else if ( (sid ==SID_SBM1_CWF_F1) || (sid ==SID_SBM2_CWF_F2)
1366 1366 || (sid == SID_SBM1_BP1_F0) || (sid == SID_SBM1_BP2_F0)
1367 1367 || (sid == SID_SBM2_BP1_F0) || (sid == SID_SBM2_BP2_F0)
1368 1368 || (sid == SID_SBM2_BP1_F1) || (sid == SID_SBM2_BP2_F1) )
1369 1369 {
1370 1370 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_SBM1_SBM2;
1371 1371 }
1372 1372 else
1373 1373 {
1374 1374 sequence_cnt = (unsigned short *) NULL;
1375 1375 PRINTF1("in increment_seq_counter_source_id *** ERR apid_destid %d not known\n", sid)
1376 1376 }
1377 1377
1378 1378 if (sequence_cnt != NULL)
1379 1379 {
1380 1380 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
1381 1381 *sequence_cnt = (*sequence_cnt) & 0x3fff;
1382 1382
1383 1383 new_packet_sequence_control = segmentation_grouping_flag | (*sequence_cnt) ;
1384 1384
1385 1385 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
1386 1386 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1387 1387
1388 1388 // increment the sequence counter
1389 1389 if ( *sequence_cnt < SEQ_CNT_MAX)
1390 1390 {
1391 1391 *sequence_cnt = *sequence_cnt + 1;
1392 1392 }
1393 1393 else
1394 1394 {
1395 1395 *sequence_cnt = 0;
1396 1396 }
1397 1397 }
1398 1398
1399 1399 //***********************************
1400 1400 // RESET THE MODE OF THE CALLING TASK
1401 1401 status = rtems_task_mode( initial_mode_set, RTEMS_PREEMPT_MASK, &current_mode_set );
1402 1402 }
General Comments 0
You need to be logged in to leave comments. Login now