##// 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

Added GPL header and documented some more functions
jeandet -
r390:1c936ff95146 No PWD scrub with... draft
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,2 +1,2
1 1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
2 7053a75ef0a55a40e950a8302065856f3bd06a26 header/lfr_common_headers
2 f5b83fb540b1cfd5d87c68621fb53f238eb623ae header/lfr_common_headers
Show file before | Show file after | Hide comments
@@ -1,8 +1,32
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 #ifndef GRSPW_H_INCLUDED
2 26 #define GRSPW_H_INCLUDED
3 27
4 28 int grspw_set_ie( unsigned char value, unsigned int *ctrlReg );
5 29 int grspw_set_tq( unsigned char value, unsigned int *ctrlReg );
6 30 int grspw_set_tr( unsigned char value, unsigned int *ctrlReg );
7 31
8 32 #endif // GRSPW_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,96 +1,119
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
1 24 /** Global variables of the LFR flight software.
2 25 *
3 26 * @file
4 27 * @author P. LEROY
5 28 *
6 29 * Among global variables, there are:
7 30 * - RTEMS names and id.
8 31 * - APB configuration registers.
9 32 * - waveforms global buffers, used by the waveform picker hardware module to store data.
10 33 * - spectral matrices buffesr, used by the hardware module to store data.
11 34 * - variable related to LFR modes parameters.
12 35 * - the global HK packet buffer.
13 36 * - the global dump parameter buffer.
14 37 *
15 38 */
16 39
17 40 #include <rtems.h>
18 41 #include <grspw.h>
19 42
20 43 #include "ccsds_types.h"
21 44 #include "grlib_regs.h"
22 45 #include "fsw_params.h"
23 46 #include "fsw_params_wf_handler.h"
24 47
25 48
26 49 #define NB_OF_MISC_NAMES 5
27 50
28 51 // RTEMS GLOBAL VARIABLES
29 52 rtems_name misc_name[NB_OF_MISC_NAMES] = {0};
30 53 rtems_name Task_name[CONFIGURE_MAXIMUM_TASKS-1] = {0}; /* array of task names */
31 54 rtems_id Task_id[CONFIGURE_MAXIMUM_TASKS-1] = {0}; /* array of task ids */
32 55 rtems_name timecode_timer_name = 0;
33 56 rtems_id timecode_timer_id = RTEMS_ID_NONE;
34 57 rtems_name name_hk_rate_monotonic = 0; // name of the HK rate monotonic
35 58 rtems_id HK_id = RTEMS_ID_NONE;// id of the HK rate monotonic period
36 59 rtems_name name_avgv_rate_monotonic = 0; // name of the AVGV rate monotonic
37 60 rtems_id AVGV_id = RTEMS_ID_NONE;// id of the AVGV rate monotonic period
38 61 int fdSPW = 0;
39 62 int fdUART = 0;
40 63 unsigned char lfrCurrentMode = 0;
41 64 unsigned char pa_bia_status_info = 0;
42 65 unsigned char thisIsAnASMRestart = 0;
43 66 unsigned char oneTcLfrUpdateTimeReceived = 0;
44 67
45 68 // WAVEFORMS GLOBAL VARIABLES // 2048 * 3 * 4 + 2 * 4 = 24576 + 8 bytes = 24584
46 69 // 97 * 256 = 24832 => delta = 248 bytes = 62 words
47 70 // WAVEFORMS GLOBAL VARIABLES // 2688 * 3 * 4 + 2 * 4 = 32256 + 8 bytes = 32264
48 71 // 127 * 256 = 32512 => delta = 248 bytes = 62 words
49 72 // F0 F1 F2 F3
50 73 volatile int wf_buffer_f0[ NB_RING_NODES_F0 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
51 74 volatile int wf_buffer_f1[ NB_RING_NODES_F1 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
52 75 volatile int wf_buffer_f2[ NB_RING_NODES_F2 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
53 76 volatile int wf_buffer_f3[ NB_RING_NODES_F3 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
54 77
55 78 //***********************************
56 79 // SPECTRAL MATRICES GLOBAL VARIABLES
57 80
58 81 // alignment constraints for the spectral matrices buffers => the first data after the time (8 bytes) shall be aligned on 0x00
59 82 volatile int sm_f0[ NB_RING_NODES_SM_F0 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0};
60 83 volatile int sm_f1[ NB_RING_NODES_SM_F1 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0};
61 84 volatile int sm_f2[ NB_RING_NODES_SM_F2 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0};
62 85
63 86 // APB CONFIGURATION REGISTERS
64 87 time_management_regs_t *time_management_regs = (time_management_regs_t*) REGS_ADDR_TIME_MANAGEMENT;
65 88 gptimer_regs_t *gptimer_regs = (gptimer_regs_t *) REGS_ADDR_GPTIMER;
66 89 waveform_picker_regs_0_1_18_t *waveform_picker_regs = (waveform_picker_regs_0_1_18_t*) REGS_ADDR_WAVEFORM_PICKER;
67 90 spectral_matrix_regs_t *spectral_matrix_regs = (spectral_matrix_regs_t*) REGS_ADDR_SPECTRAL_MATRIX;
68 91
69 92 // MODE PARAMETERS
70 93 Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet = {0};
71 94 struct param_local_str param_local = {0};
72 95 unsigned int lastValidEnterModeTime = {0};
73 96
74 97 // HK PACKETS
75 98 Packet_TM_LFR_HK_t housekeeping_packet = {0};
76 99 // message queues occupancy
77 100 unsigned char hk_lfr_q_sd_fifo_size_max = 0;
78 101 unsigned char hk_lfr_q_rv_fifo_size_max = 0;
79 102 unsigned char hk_lfr_q_p0_fifo_size_max = 0;
80 103 unsigned char hk_lfr_q_p1_fifo_size_max = 0;
81 104 unsigned char hk_lfr_q_p2_fifo_size_max = 0;
82 105 // sequence counters are incremented by APID (PID + CAT) and destination ID
83 106 unsigned short sequenceCounters_SCIENCE_NORMAL_BURST __attribute__((aligned(0x4))) = 0;
84 107 unsigned short sequenceCounters_SCIENCE_SBM1_SBM2 __attribute__((aligned(0x4))) = 0;
85 108 unsigned short sequenceCounters_TC_EXE[SEQ_CNT_NB_DEST_ID] __attribute__((aligned(0x4))) = {0};
86 109 unsigned short sequenceCounters_TM_DUMP[SEQ_CNT_NB_DEST_ID] __attribute__((aligned(0x4))) = {0};
87 110 unsigned short sequenceCounterHK __attribute__((aligned(0x4))) = {0};
88 111 spw_stats grspw_stats __attribute__((aligned(0x4))) = {0};
89 112
90 113 // TC_LFR_UPDATE_INFO
91 114 rw_f_t rw_f;
92 115
93 116 // TC_LFR_LOAD_FILTER_PAR
94 117 filterPar_t filterPar = {0};
95 118
96 119 fbins_masks_t fbins_masks = {0};
Show file before | Show file after | Hide comments
@@ -1,1009 +1,1029
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** This is the RTEMS initialization module.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * This module contains two very different information:
7 31 * - specific instructions to configure the compilation of the RTEMS executive
8 32 * - functions related to the fligth softwre initialization, especially the INIT RTEMS task
9 33 *
10 34 */
11 35
12 //*************************
13 // GPL reminder to be added
14 //*************************
15
16 36 #include <rtems.h>
17 37
18 38
19 39 /* configuration information */
20 40
21 41 #define CONFIGURE_INIT
22 42
23 43 #include <bsp.h> /* for device driver prototypes */
24 44
25 45 /* configuration information */
26 46
27 47 #include <fsw_params.h>
28 48
29 49 #include <rtems/confdefs.h>
30 50
31 51 /* If --drvmgr was enabled during the configuration of the RTEMS kernel */
32 52 #ifdef RTEMS_DRVMGR_STARTUP
33 53 #ifdef LEON3
34 54 /* Add Timer and UART Driver */
35 55
36 56 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
37 57 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
38 58 #endif
39 59
40 60 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
41 61 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
42 62 #endif
43 63
44 64 #endif
45 65 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
46 66 #include <drvmgr/drvmgr_confdefs.h>
47 67 #endif
48 68
49 69 #include "fsw_init.h"
50 70 #include "fsw_config.c"
51 71 #include "GscMemoryLPP.hpp"
52 72
53 73 void initCache()
54 74 {
55 75 // ASI 2 contains a few control registers that have not been assigned as ancillary state registers.
56 76 // These should only be read and written using 32-bit LDA/STA instructions.
57 77 // All cache registers are accessed through load/store operations to the alternate address space (LDA/STA), using ASI = 2.
58 78 // The table below shows the register addresses:
59 79 // 0x00 Cache control register
60 80 // 0x04 Reserved
61 81 // 0x08 Instruction cache configuration register
62 82 // 0x0C Data cache configuration register
63 83
64 84 // Cache Control Register Leon3 / Leon3FT
65 85 // 31..30 29 28 27..24 23 22 21 20..19 18 17 16
66 86 // RFT PS TB DS FD FI FT ST IB
67 87 // 15 14 13..12 11..10 9..8 7..6 5 4 3..2 1..0
68 88 // IP DP ITE IDE DTE DDE DF IF DCS ICS
69 89
70 90 unsigned int cacheControlRegister;
71 91
72 92 CCR_resetCacheControlRegister();
73 93 ASR16_resetRegisterProtectionControlRegister();
74 94
75 95 cacheControlRegister = CCR_getValue();
76 96 PRINTF1("(0) CCR - Cache Control Register = %x\n", cacheControlRegister);
77 97 PRINTF1("(0) ASR16 = %x\n", *asr16Ptr);
78 98
79 99 CCR_enableInstructionCache(); // ICS bits
80 100 CCR_enableDataCache(); // DCS bits
81 101 CCR_enableInstructionBurstFetch(); // IB bit
82 102
83 103 faultTolerantScheme();
84 104
85 105 cacheControlRegister = CCR_getValue();
86 106 PRINTF1("(1) CCR - Cache Control Register = %x\n", cacheControlRegister);
87 107 PRINTF1("(1) ASR16 Register protection control register = %x\n", *asr16Ptr);
88 108
89 109 PRINTF("\n");
90 110 }
91 111
92 112 rtems_task Init( rtems_task_argument ignored )
93 113 {
94 114 /** This is the RTEMS INIT taks, it is the first task launched by the system.
95 115 *
96 116 * @param unused is the starting argument of the RTEMS task
97 117 *
98 118 * The INIT task create and run all other RTEMS tasks.
99 119 *
100 120 */
101 121
102 122 //***********
103 123 // INIT CACHE
104 124
105 125 unsigned char *vhdlVersion;
106 126
107 127 reset_lfr();
108 128
109 129 reset_local_time();
110 130
111 131 rtems_cpu_usage_reset();
112 132
113 133 rtems_status_code status;
114 134 rtems_status_code status_spw;
115 135 rtems_isr_entry old_isr_handler;
116 136
117 137 old_isr_handler = NULL;
118 138
119 139 // UART settings
120 140 enable_apbuart_transmitter();
121 141 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
122 142
123 143 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
124 144
125 145
126 146 PRINTF("\n\n\n\n\n")
127 147
128 148 initCache();
129 149
130 150 PRINTF("*************************\n")
131 151 PRINTF("** LFR Flight Software **\n")
132 152
133 153 PRINTF1("** %d-", SW_VERSION_N1)
134 154 PRINTF1("%d-" , SW_VERSION_N2)
135 155 PRINTF1("%d-" , SW_VERSION_N3)
136 156 PRINTF1("%d **\n", SW_VERSION_N4)
137 157
138 158 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
139 159 PRINTF("** VHDL **\n")
140 160 PRINTF1("** %d-", vhdlVersion[1])
141 161 PRINTF1("%d-" , vhdlVersion[2])
142 162 PRINTF1("%d **\n", vhdlVersion[3])
143 163 PRINTF("*************************\n")
144 164 PRINTF("\n\n")
145 165
146 166 init_parameter_dump();
147 167 init_kcoefficients_dump();
148 168 init_local_mode_parameters();
149 169 init_housekeeping_parameters();
150 170 init_k_coefficients_prc0();
151 171 init_k_coefficients_prc1();
152 172 init_k_coefficients_prc2();
153 173 pa_bia_status_info = INIT_CHAR;
154 174
155 175 // initialize all reaction wheels frequencies to NaN
156 176 rw_f.cp_rpw_sc_rw1_f1 = NAN;
157 177 rw_f.cp_rpw_sc_rw1_f2 = NAN;
158 178 rw_f.cp_rpw_sc_rw1_f3 = NAN;
159 179 rw_f.cp_rpw_sc_rw1_f4 = NAN;
160 180 rw_f.cp_rpw_sc_rw2_f1 = NAN;
161 181 rw_f.cp_rpw_sc_rw2_f2 = NAN;
162 182 rw_f.cp_rpw_sc_rw2_f3 = NAN;
163 183 rw_f.cp_rpw_sc_rw2_f4 = NAN;
164 184 rw_f.cp_rpw_sc_rw3_f1 = NAN;
165 185 rw_f.cp_rpw_sc_rw3_f2 = NAN;
166 186 rw_f.cp_rpw_sc_rw3_f3 = NAN;
167 187 rw_f.cp_rpw_sc_rw3_f4 = NAN;
168 188 rw_f.cp_rpw_sc_rw4_f1 = NAN;
169 189 rw_f.cp_rpw_sc_rw4_f2 = NAN;
170 190 rw_f.cp_rpw_sc_rw4_f3 = NAN;
171 191 rw_f.cp_rpw_sc_rw4_f4 = NAN;
172 192
173 193 // initialize filtering parameters
174 194 filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED;
175 195 filterPar.sy_lfr_sc_rw_delta_f = DEFAULT_SY_LFR_SC_RW_DELTA_F;
176 196 filterPar.sy_lfr_pas_filter_tbad = DEFAULT_SY_LFR_PAS_FILTER_TBAD;
177 197 filterPar.sy_lfr_pas_filter_shift = DEFAULT_SY_LFR_PAS_FILTER_SHIFT;
178 198 filterPar.modulus_in_finetime = DEFAULT_MODULUS;
179 199 filterPar.tbad_in_finetime = DEFAULT_TBAD;
180 200 filterPar.offset_in_finetime = DEFAULT_OFFSET;
181 201 filterPar.shift_in_finetime = DEFAULT_SHIFT;
182 202 update_last_valid_transition_date( DEFAULT_LAST_VALID_TRANSITION_DATE );
183 203
184 204 // waveform picker initialization
185 205 WFP_init_rings();
186 206 LEON_Clear_interrupt( IRQ_SPARC_GPTIMER_WATCHDOG ); // initialize the waveform rings
187 207 WFP_reset_current_ring_nodes();
188 208 reset_waveform_picker_regs();
189 209
190 210 // spectral matrices initialization
191 211 SM_init_rings(); // initialize spectral matrices rings
192 212 SM_reset_current_ring_nodes();
193 213 reset_spectral_matrix_regs();
194 214
195 215 // configure calibration
196 216 configureCalibration( false ); // true means interleaved mode, false is for normal mode
197 217
198 218 updateLFRCurrentMode( LFR_MODE_STANDBY );
199 219
200 220 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
201 221
202 222 create_names(); // create all names
203 223
204 224 status = create_timecode_timer(); // create the timer used by timecode_irq_handler
205 225 if (status != RTEMS_SUCCESSFUL)
206 226 {
207 227 PRINTF1("in INIT *** ERR in create_timer_timecode, code %d", status)
208 228 }
209 229
210 230 status = create_message_queues(); // create message queues
211 231 if (status != RTEMS_SUCCESSFUL)
212 232 {
213 233 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
214 234 }
215 235
216 236 status = create_all_tasks(); // create all tasks
217 237 if (status != RTEMS_SUCCESSFUL)
218 238 {
219 239 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
220 240 }
221 241
222 242 // **************************
223 243 // <SPACEWIRE INITIALIZATION>
224 244 status_spw = spacewire_open_link(); // (1) open the link
225 245 if ( status_spw != RTEMS_SUCCESSFUL )
226 246 {
227 247 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
228 248 }
229 249
230 250 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
231 251 {
232 252 status_spw = spacewire_configure_link( fdSPW );
233 253 if ( status_spw != RTEMS_SUCCESSFUL )
234 254 {
235 255 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
236 256 }
237 257 }
238 258
239 259 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
240 260 {
241 261 status_spw = spacewire_start_link( fdSPW );
242 262 if ( status_spw != RTEMS_SUCCESSFUL )
243 263 {
244 264 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
245 265 }
246 266 }
247 267 // </SPACEWIRE INITIALIZATION>
248 268 // ***************************
249 269
250 270 status = start_all_tasks(); // start all tasks
251 271 if (status != RTEMS_SUCCESSFUL)
252 272 {
253 273 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
254 274 }
255 275
256 276 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
257 277 status = start_recv_send_tasks();
258 278 if ( status != RTEMS_SUCCESSFUL )
259 279 {
260 280 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
261 281 }
262 282
263 283 // suspend science tasks, they will be restarted later depending on the mode
264 284 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
265 285 if (status != RTEMS_SUCCESSFUL)
266 286 {
267 287 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
268 288 }
269 289
270 290 // configure IRQ handling for the waveform picker unit
271 291 status = rtems_interrupt_catch( waveforms_isr,
272 292 IRQ_SPARC_WAVEFORM_PICKER,
273 293 &old_isr_handler) ;
274 294 // configure IRQ handling for the spectral matrices unit
275 295 status = rtems_interrupt_catch( spectral_matrices_isr,
276 296 IRQ_SPARC_SPECTRAL_MATRIX,
277 297 &old_isr_handler) ;
278 298
279 299 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
280 300 if ( status_spw != RTEMS_SUCCESSFUL )
281 301 {
282 302 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
283 303 if ( status != RTEMS_SUCCESSFUL ) {
284 304 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
285 305 }
286 306 }
287 307
288 308 BOOT_PRINTF("delete INIT\n")
289 309
290 310 set_hk_lfr_sc_potential_flag( true );
291 311
292 312 // start the timer to detect a missing spacewire timecode
293 313 // the timeout is larger because the spw IP needs to receive several valid timecodes before generating a tickout
294 314 // if a tickout is generated, the timer is restarted
295 315 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT_INIT, timecode_timer_routine, NULL );
296 316
297 317 grspw_timecode_callback = &timecode_irq_handler;
298 318
299 319 status = rtems_task_delete(RTEMS_SELF);
300 320
301 321 }
302 322
303 323 void init_local_mode_parameters( void )
304 324 {
305 325 /** This function initialize the param_local global variable with default values.
306 326 *
307 327 */
308 328
309 329 unsigned int i;
310 330
311 331 // LOCAL PARAMETERS
312 332
313 333 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
314 334 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
315 335
316 336 // init sequence counters
317 337
318 338 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
319 339 {
320 340 sequenceCounters_TC_EXE[i] = INIT_CHAR;
321 341 sequenceCounters_TM_DUMP[i] = INIT_CHAR;
322 342 }
323 343 sequenceCounters_SCIENCE_NORMAL_BURST = INIT_CHAR;
324 344 sequenceCounters_SCIENCE_SBM1_SBM2 = INIT_CHAR;
325 345 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << TM_PACKET_SEQ_SHIFT;
326 346 }
327 347
328 348 void reset_local_time( void )
329 349 {
330 350 time_management_regs->ctrl = time_management_regs->ctrl | VAL_SOFTWARE_RESET; // [0010] software reset, coarse time = 0x80000000
331 351 }
332 352
333 353 void create_names( void ) // create all names for tasks and queues
334 354 {
335 355 /** This function creates all RTEMS names used in the software for tasks and queues.
336 356 *
337 357 * @return RTEMS directive status codes:
338 358 * - RTEMS_SUCCESSFUL - successful completion
339 359 *
340 360 */
341 361
342 362 // task names
343 363 Task_name[TASKID_AVGV] = rtems_build_name( 'A', 'V', 'G', 'V' );
344 364 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
345 365 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
346 366 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
347 367 Task_name[TASKID_LOAD] = rtems_build_name( 'L', 'O', 'A', 'D' );
348 368 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
349 369 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
350 370 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
351 371 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
352 372 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
353 373 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
354 374 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
355 375 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
356 376 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
357 377 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
358 378 Task_name[TASKID_LINK] = rtems_build_name( 'L', 'I', 'N', 'K' );
359 379 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
360 380 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
361 381 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
362 382 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
363 383 Task_name[TASKID_SCRB] = rtems_build_name( 'S', 'C', 'R', 'B' );
364 384 Task_name[TASKID_CALI] = rtems_build_name( 'C', 'A', 'L', 'I' );
365 385
366 386 // rate monotonic period names
367 387 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
368 388 name_avgv_rate_monotonic = rtems_build_name( 'A', 'V', 'G', 'V' );
369 389
370 390 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
371 391 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
372 392 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
373 393 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
374 394 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
375 395
376 396 timecode_timer_name = rtems_build_name( 'S', 'P', 'T', 'C' );
377 397 }
378 398
379 399 int create_all_tasks( void ) // create all tasks which run in the software
380 400 {
381 401 /** This function creates all RTEMS tasks used in the software.
382 402 *
383 403 * @return RTEMS directive status codes:
384 404 * - RTEMS_SUCCESSFUL - task created successfully
385 405 * - RTEMS_INVALID_ADDRESS - id is NULL
386 406 * - RTEMS_INVALID_NAME - invalid task name
387 407 * - RTEMS_INVALID_PRIORITY - invalid task priority
388 408 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
389 409 * - RTEMS_TOO_MANY - too many tasks created
390 410 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
391 411 * - RTEMS_TOO_MANY - too many global objects
392 412 *
393 413 */
394 414
395 415 rtems_status_code status;
396 416
397 417 //**********
398 418 // SPACEWIRE
399 419 // RECV
400 420 status = rtems_task_create(
401 421 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
402 422 RTEMS_DEFAULT_MODES,
403 423 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
404 424 );
405 425 if (status == RTEMS_SUCCESSFUL) // SEND
406 426 {
407 427 status = rtems_task_create(
408 428 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
409 429 RTEMS_DEFAULT_MODES,
410 430 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
411 431 );
412 432 }
413 433 if (status == RTEMS_SUCCESSFUL) // LINK
414 434 {
415 435 status = rtems_task_create(
416 436 Task_name[TASKID_LINK], TASK_PRIORITY_LINK, RTEMS_MINIMUM_STACK_SIZE,
417 437 RTEMS_DEFAULT_MODES,
418 438 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LINK]
419 439 );
420 440 }
421 441 if (status == RTEMS_SUCCESSFUL) // ACTN
422 442 {
423 443 status = rtems_task_create(
424 444 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
425 445 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
426 446 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
427 447 );
428 448 }
429 449 if (status == RTEMS_SUCCESSFUL) // SPIQ
430 450 {
431 451 status = rtems_task_create(
432 452 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
433 453 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
434 454 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
435 455 );
436 456 }
437 457
438 458 //******************
439 459 // SPECTRAL MATRICES
440 460 if (status == RTEMS_SUCCESSFUL) // AVF0
441 461 {
442 462 status = rtems_task_create(
443 463 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
444 464 RTEMS_DEFAULT_MODES,
445 465 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
446 466 );
447 467 }
448 468 if (status == RTEMS_SUCCESSFUL) // PRC0
449 469 {
450 470 status = rtems_task_create(
451 471 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
452 472 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
453 473 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
454 474 );
455 475 }
456 476 if (status == RTEMS_SUCCESSFUL) // AVF1
457 477 {
458 478 status = rtems_task_create(
459 479 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
460 480 RTEMS_DEFAULT_MODES,
461 481 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
462 482 );
463 483 }
464 484 if (status == RTEMS_SUCCESSFUL) // PRC1
465 485 {
466 486 status = rtems_task_create(
467 487 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
468 488 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
469 489 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
470 490 );
471 491 }
472 492 if (status == RTEMS_SUCCESSFUL) // AVF2
473 493 {
474 494 status = rtems_task_create(
475 495 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
476 496 RTEMS_DEFAULT_MODES,
477 497 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
478 498 );
479 499 }
480 500 if (status == RTEMS_SUCCESSFUL) // PRC2
481 501 {
482 502 status = rtems_task_create(
483 503 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
484 504 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
485 505 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
486 506 );
487 507 }
488 508
489 509 //****************
490 510 // WAVEFORM PICKER
491 511 if (status == RTEMS_SUCCESSFUL) // WFRM
492 512 {
493 513 status = rtems_task_create(
494 514 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
495 515 RTEMS_DEFAULT_MODES,
496 516 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
497 517 );
498 518 }
499 519 if (status == RTEMS_SUCCESSFUL) // CWF3
500 520 {
501 521 status = rtems_task_create(
502 522 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
503 523 RTEMS_DEFAULT_MODES,
504 524 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
505 525 );
506 526 }
507 527 if (status == RTEMS_SUCCESSFUL) // CWF2
508 528 {
509 529 status = rtems_task_create(
510 530 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
511 531 RTEMS_DEFAULT_MODES,
512 532 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
513 533 );
514 534 }
515 535 if (status == RTEMS_SUCCESSFUL) // CWF1
516 536 {
517 537 status = rtems_task_create(
518 538 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
519 539 RTEMS_DEFAULT_MODES,
520 540 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
521 541 );
522 542 }
523 543 if (status == RTEMS_SUCCESSFUL) // SWBD
524 544 {
525 545 status = rtems_task_create(
526 546 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
527 547 RTEMS_DEFAULT_MODES,
528 548 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
529 549 );
530 550 }
531 551
532 552 //*****
533 553 // MISC
534 554 if (status == RTEMS_SUCCESSFUL) // LOAD
535 555 {
536 556 status = rtems_task_create(
537 557 Task_name[TASKID_LOAD], TASK_PRIORITY_LOAD, RTEMS_MINIMUM_STACK_SIZE,
538 558 RTEMS_DEFAULT_MODES,
539 559 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LOAD]
540 560 );
541 561 }
542 562 if (status == RTEMS_SUCCESSFUL) // DUMB
543 563 {
544 564 status = rtems_task_create(
545 565 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
546 566 RTEMS_DEFAULT_MODES,
547 567 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
548 568 );
549 569 }
550 570 if (status == RTEMS_SUCCESSFUL) // SCRUBBING TASK
551 571 {
552 572 status = rtems_task_create(
553 573 Task_name[TASKID_SCRB], TASK_PRIORITY_SCRB, RTEMS_MINIMUM_STACK_SIZE,
554 574 RTEMS_DEFAULT_MODES,
555 575 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SCRB]
556 576 );
557 577 }
558 578 if (status == RTEMS_SUCCESSFUL) // HOUS
559 579 {
560 580 status = rtems_task_create(
561 581 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
562 582 RTEMS_DEFAULT_MODES,
563 583 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
564 584 );
565 585 }
566 586 if (status == RTEMS_SUCCESSFUL) // AVGV
567 587 {
568 588 status = rtems_task_create(
569 589 Task_name[TASKID_AVGV], TASK_PRIORITY_AVGV, RTEMS_MINIMUM_STACK_SIZE,
570 590 RTEMS_DEFAULT_MODES,
571 591 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVGV]
572 592 );
573 593 }
574 594 if (status == RTEMS_SUCCESSFUL) // CALI
575 595 {
576 596 status = rtems_task_create(
577 597 Task_name[TASKID_CALI], TASK_PRIORITY_CALI, RTEMS_MINIMUM_STACK_SIZE,
578 598 RTEMS_DEFAULT_MODES,
579 599 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CALI]
580 600 );
581 601 }
582 602
583 603 return status;
584 604 }
585 605
586 606 int start_recv_send_tasks( void )
587 607 {
588 608 rtems_status_code status;
589 609
590 610 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
591 611 if (status!=RTEMS_SUCCESSFUL) {
592 612 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
593 613 }
594 614
595 615 if (status == RTEMS_SUCCESSFUL) // SEND
596 616 {
597 617 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
598 618 if (status!=RTEMS_SUCCESSFUL) {
599 619 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
600 620 }
601 621 }
602 622
603 623 return status;
604 624 }
605 625
606 626 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
607 627 {
608 628 /** This function starts all RTEMS tasks used in the software.
609 629 *
610 630 * @return RTEMS directive status codes:
611 631 * - RTEMS_SUCCESSFUL - ask started successfully
612 632 * - RTEMS_INVALID_ADDRESS - invalid task entry point
613 633 * - RTEMS_INVALID_ID - invalid task id
614 634 * - RTEMS_INCORRECT_STATE - task not in the dormant state
615 635 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
616 636 *
617 637 */
618 638 // starts all the tasks fot eh flight software
619 639
620 640 rtems_status_code status;
621 641
622 642 //**********
623 643 // SPACEWIRE
624 644 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
625 645 if (status!=RTEMS_SUCCESSFUL) {
626 646 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
627 647 }
628 648
629 649 if (status == RTEMS_SUCCESSFUL) // LINK
630 650 {
631 651 status = rtems_task_start( Task_id[TASKID_LINK], link_task, 1 );
632 652 if (status!=RTEMS_SUCCESSFUL) {
633 653 BOOT_PRINTF("in INIT *** Error starting TASK_LINK\n")
634 654 }
635 655 }
636 656
637 657 if (status == RTEMS_SUCCESSFUL) // ACTN
638 658 {
639 659 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
640 660 if (status!=RTEMS_SUCCESSFUL) {
641 661 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
642 662 }
643 663 }
644 664
645 665 //******************
646 666 // SPECTRAL MATRICES
647 667 if (status == RTEMS_SUCCESSFUL) // AVF0
648 668 {
649 669 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
650 670 if (status!=RTEMS_SUCCESSFUL) {
651 671 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
652 672 }
653 673 }
654 674 if (status == RTEMS_SUCCESSFUL) // PRC0
655 675 {
656 676 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
657 677 if (status!=RTEMS_SUCCESSFUL) {
658 678 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
659 679 }
660 680 }
661 681 if (status == RTEMS_SUCCESSFUL) // AVF1
662 682 {
663 683 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
664 684 if (status!=RTEMS_SUCCESSFUL) {
665 685 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
666 686 }
667 687 }
668 688 if (status == RTEMS_SUCCESSFUL) // PRC1
669 689 {
670 690 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
671 691 if (status!=RTEMS_SUCCESSFUL) {
672 692 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
673 693 }
674 694 }
675 695 if (status == RTEMS_SUCCESSFUL) // AVF2
676 696 {
677 697 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
678 698 if (status!=RTEMS_SUCCESSFUL) {
679 699 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
680 700 }
681 701 }
682 702 if (status == RTEMS_SUCCESSFUL) // PRC2
683 703 {
684 704 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
685 705 if (status!=RTEMS_SUCCESSFUL) {
686 706 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
687 707 }
688 708 }
689 709
690 710 //****************
691 711 // WAVEFORM PICKER
692 712 if (status == RTEMS_SUCCESSFUL) // WFRM
693 713 {
694 714 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
695 715 if (status!=RTEMS_SUCCESSFUL) {
696 716 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
697 717 }
698 718 }
699 719 if (status == RTEMS_SUCCESSFUL) // CWF3
700 720 {
701 721 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
702 722 if (status!=RTEMS_SUCCESSFUL) {
703 723 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
704 724 }
705 725 }
706 726 if (status == RTEMS_SUCCESSFUL) // CWF2
707 727 {
708 728 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
709 729 if (status!=RTEMS_SUCCESSFUL) {
710 730 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
711 731 }
712 732 }
713 733 if (status == RTEMS_SUCCESSFUL) // CWF1
714 734 {
715 735 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
716 736 if (status!=RTEMS_SUCCESSFUL) {
717 737 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
718 738 }
719 739 }
720 740 if (status == RTEMS_SUCCESSFUL) // SWBD
721 741 {
722 742 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
723 743 if (status!=RTEMS_SUCCESSFUL) {
724 744 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
725 745 }
726 746 }
727 747
728 748 //*****
729 749 // MISC
730 750 if (status == RTEMS_SUCCESSFUL) // HOUS
731 751 {
732 752 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
733 753 if (status!=RTEMS_SUCCESSFUL) {
734 754 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
735 755 }
736 756 }
737 757 if (status == RTEMS_SUCCESSFUL) // AVGV
738 758 {
739 759 status = rtems_task_start( Task_id[TASKID_AVGV], avgv_task, 1 );
740 760 if (status!=RTEMS_SUCCESSFUL) {
741 761 BOOT_PRINTF("in INIT *** Error starting TASK_AVGV\n")
742 762 }
743 763 }
744 764 if (status == RTEMS_SUCCESSFUL) // DUMB
745 765 {
746 766 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
747 767 if (status!=RTEMS_SUCCESSFUL) {
748 768 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
749 769 }
750 770 }
751 771 if (status == RTEMS_SUCCESSFUL) // SCRUBBING
752 772 {
753 773 status = rtems_task_start( Task_id[TASKID_SCRB], scrubbing_task, 1 );
754 774 if (status!=RTEMS_SUCCESSFUL) {
755 775 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
756 776 }
757 777 }
758 778 if (status == RTEMS_SUCCESSFUL) // LOAD
759 779 {
760 780 status = rtems_task_start( Task_id[TASKID_LOAD], load_task, 1 );
761 781 if (status!=RTEMS_SUCCESSFUL) {
762 782 BOOT_PRINTF("in INIT *** Error starting TASK_LOAD\n")
763 783 }
764 784 }
765 785 if (status == RTEMS_SUCCESSFUL) // CALI
766 786 {
767 787 status = rtems_task_start( Task_id[TASKID_CALI], calibration_sweep_task, 1 );
768 788 if (status!=RTEMS_SUCCESSFUL) {
769 789 BOOT_PRINTF("in INIT *** Error starting TASK_LOAD\n")
770 790 }
771 791 }
772 792
773 793 return status;
774 794 }
775 795
776 796 rtems_status_code create_message_queues( void ) // create the five message queues used in the software
777 797 {
778 798 rtems_status_code status_recv;
779 799 rtems_status_code status_send;
780 800 rtems_status_code status_q_p0;
781 801 rtems_status_code status_q_p1;
782 802 rtems_status_code status_q_p2;
783 803 rtems_status_code ret;
784 804 rtems_id queue_id;
785 805
786 806 ret = RTEMS_SUCCESSFUL;
787 807 queue_id = RTEMS_ID_NONE;
788 808
789 809 //****************************************
790 810 // create the queue for handling valid TCs
791 811 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
792 812 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
793 813 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
794 814 if ( status_recv != RTEMS_SUCCESSFUL ) {
795 815 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
796 816 }
797 817
798 818 //************************************************
799 819 // create the queue for handling TM packet sending
800 820 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
801 821 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
802 822 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
803 823 if ( status_send != RTEMS_SUCCESSFUL ) {
804 824 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
805 825 }
806 826
807 827 //*****************************************************************************
808 828 // create the queue for handling averaged spectral matrices for processing @ f0
809 829 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
810 830 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
811 831 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
812 832 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
813 833 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
814 834 }
815 835
816 836 //*****************************************************************************
817 837 // create the queue for handling averaged spectral matrices for processing @ f1
818 838 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
819 839 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
820 840 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
821 841 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
822 842 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
823 843 }
824 844
825 845 //*****************************************************************************
826 846 // create the queue for handling averaged spectral matrices for processing @ f2
827 847 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
828 848 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
829 849 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
830 850 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
831 851 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
832 852 }
833 853
834 854 if ( status_recv != RTEMS_SUCCESSFUL )
835 855 {
836 856 ret = status_recv;
837 857 }
838 858 else if( status_send != RTEMS_SUCCESSFUL )
839 859 {
840 860 ret = status_send;
841 861 }
842 862 else if( status_q_p0 != RTEMS_SUCCESSFUL )
843 863 {
844 864 ret = status_q_p0;
845 865 }
846 866 else if( status_q_p1 != RTEMS_SUCCESSFUL )
847 867 {
848 868 ret = status_q_p1;
849 869 }
850 870 else
851 871 {
852 872 ret = status_q_p2;
853 873 }
854 874
855 875 return ret;
856 876 }
857 877
858 878 rtems_status_code create_timecode_timer( void )
859 879 {
860 880 rtems_status_code status;
861 881
862 882 status = rtems_timer_create( timecode_timer_name, &timecode_timer_id );
863 883
864 884 if ( status != RTEMS_SUCCESSFUL )
865 885 {
866 886 PRINTF1("in create_timer_timecode *** ERR creating SPTC timer, %d\n", status)
867 887 }
868 888 else
869 889 {
870 890 PRINTF("in create_timer_timecode *** OK creating SPTC timer\n")
871 891 }
872 892
873 893 return status;
874 894 }
875 895
876 896 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
877 897 {
878 898 rtems_status_code status;
879 899 rtems_name queue_name;
880 900
881 901 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
882 902
883 903 status = rtems_message_queue_ident( queue_name, 0, queue_id );
884 904
885 905 return status;
886 906 }
887 907
888 908 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
889 909 {
890 910 rtems_status_code status;
891 911 rtems_name queue_name;
892 912
893 913 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
894 914
895 915 status = rtems_message_queue_ident( queue_name, 0, queue_id );
896 916
897 917 return status;
898 918 }
899 919
900 920 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
901 921 {
902 922 rtems_status_code status;
903 923 rtems_name queue_name;
904 924
905 925 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
906 926
907 927 status = rtems_message_queue_ident( queue_name, 0, queue_id );
908 928
909 929 return status;
910 930 }
911 931
912 932 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
913 933 {
914 934 rtems_status_code status;
915 935 rtems_name queue_name;
916 936
917 937 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
918 938
919 939 status = rtems_message_queue_ident( queue_name, 0, queue_id );
920 940
921 941 return status;
922 942 }
923 943
924 944 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
925 945 {
926 946 rtems_status_code status;
927 947 rtems_name queue_name;
928 948
929 949 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
930 950
931 951 status = rtems_message_queue_ident( queue_name, 0, queue_id );
932 952
933 953 return status;
934 954 }
935 955
936 956 /**
937 957 * @brief update_queue_max_count returns max(fifo_size_max, pending_messages + 1)
938 958 * @param queue_id
939 959 * @param fifo_size_max
940 960 */
941 961 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max )
942 962 {
943 963 u_int32_t count;
944 964 rtems_status_code status;
945 965
946 966 count = 0;
947 967
948 968 status = rtems_message_queue_get_number_pending( queue_id, &count );
949 969
950 970 count = count + 1;
951 971
952 972 if (status != RTEMS_SUCCESSFUL)
953 973 {
954 974 PRINTF1("in update_queue_max_count *** ERR = %d\n", status)
955 975 }
956 976 else
957 977 {
958 978 if (count > *fifo_size_max)
959 979 {
960 980 *fifo_size_max = count;
961 981 }
962 982 }
963 983 }
964 984
965 985 /**
966 986 * @brief init_ring initializes given ring buffer
967 987 * @param ring array of nodes to initialize
968 988 * @param nbNodes number of node in the ring buffer
969 989 * @param buffer memory space given to the ring buffer
970 990 * @param bufferSize size of the whole ring buffer memory space
971 991 *
972 992 * @details This function creates a circular buffer from a given number of nodes and a given memory space. It first sets all nodes attributes to thier defaults values
973 993 * and associates a portion of the given memory space with each node. Then it connects each nodes to build a circular buffer.
974 994 *
975 995 * Each node capacity will be bufferSize/nbNodes.
976 996 *
977 997 * https://en.wikipedia.org/wiki/Circular_buffer
978 998 */
979 999 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
980 1000 {
981 1001 unsigned char i;
982 1002
983 1003 //***************
984 1004 // BUFFER ADDRESS
985 1005 for(i=0; i<nbNodes; i++)
986 1006 {
987 1007 ring[i].coarseTime = INT32_ALL_F;
988 1008 ring[i].fineTime = INT32_ALL_F;
989 1009 ring[i].sid = INIT_CHAR;
990 1010 ring[i].status = INIT_CHAR;
991 1011 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
992 1012 }
993 1013
994 1014 //*****
995 1015 // NEXT
996 1016 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
997 1017 for(i=0; i<nbNodes-1; i++)
998 1018 {
999 1019 ring[i].next = (ring_node*) &ring[ i + 1 ];
1000 1020 }
1001 1021
1002 1022 //*********
1003 1023 // PREVIOUS
1004 1024 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
1005 1025 for(i=1; i<nbNodes; i++)
1006 1026 {
1007 1027 ring[i].previous = (ring_node*) &ring[ i - 1 ];
1008 1028 }
1009 1029 }
Show file before | Show file after | Hide comments
@@ -1,1054 +1,1109
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** General usage functions and RTEMS tasks.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 */
7 31
8 32 #include "fsw_misc.h"
9 33
10 34 int16_t hk_lfr_sc_v_f3_as_int16 = 0;
11 35 int16_t hk_lfr_sc_e1_f3_as_int16 = 0;
12 36 int16_t hk_lfr_sc_e2_f3_as_int16 = 0;
13 37
14 38 void timer_configure(unsigned char timer, unsigned int clock_divider,
15 39 unsigned char interrupt_level, rtems_isr (*timer_isr)() )
16 40 {
17 41 /** This function configures a GPTIMER timer instantiated in the VHDL design.
18 42 *
19 43 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
20 44 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
21 45 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
22 46 * @param interrupt_level is the interrupt level that the timer drives.
23 47 * @param timer_isr is the interrupt subroutine that will be attached to the IRQ driven by the timer.
24 48 *
25 49 * Interrupt levels are described in the SPARC documentation sparcv8.pdf p.76
26 50 *
27 51 */
28 52
29 53 rtems_status_code status;
30 54 rtems_isr_entry old_isr_handler;
31 55
32 56 old_isr_handler = NULL;
33 57
34 58 gptimer_regs->timer[timer].ctrl = INIT_CHAR; // reset the control register
35 59
36 60 status = rtems_interrupt_catch( timer_isr, interrupt_level, &old_isr_handler) ; // see sparcv8.pdf p.76 for interrupt levels
37 61 if (status!=RTEMS_SUCCESSFUL)
38 62 {
39 63 PRINTF("in configure_timer *** ERR rtems_interrupt_catch\n")
40 64 }
41 65
42 66 timer_set_clock_divider( timer, clock_divider);
43 67 }
44 68
45 69 void timer_start(unsigned char timer)
46 70 {
47 71 /** This function starts a GPTIMER timer.
48 72 *
49 73 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
50 74 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
51 75 *
52 76 */
53 77
54 78 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_CLEAR_IRQ;
55 79 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_LD;
56 80 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_EN;
57 81 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_RS;
58 82 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_IE;
59 83 }
60 84
61 85 void timer_stop(unsigned char timer)
62 86 {
63 87 /** This function stops a GPTIMER timer.
64 88 *
65 89 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
66 90 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
67 91 *
68 92 */
69 93
70 94 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & GPTIMER_EN_MASK;
71 95 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & GPTIMER_IE_MASK;
72 96 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_CLEAR_IRQ;
73 97 }
74 98
75 99 void timer_set_clock_divider(unsigned char timer, unsigned int clock_divider)
76 100 {
77 101 /** This function sets the clock divider of a GPTIMER timer.
78 102 *
79 103 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
80 104 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
81 105 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
82 106 *
83 107 */
84 108
85 109 gptimer_regs->timer[timer].reload = clock_divider; // base clock frequency is 1 MHz
86 110 }
87 111
88 // WATCHDOG
112 // WATCHDOG, this ISR should never be triggered.
89 113
90 114 rtems_isr watchdog_isr( rtems_vector_number vector )
91 115 {
92 116 rtems_status_code status_code;
93 117
94 118 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_12 );
95 119
96 120 PRINTF("watchdog_isr *** this is the end, exit(0)\n");
97 121
98 122 exit(0);
99 123 }
100 124
101 125 void watchdog_configure(void)
102 126 {
103 127 /** This function configure the watchdog.
104 128 *
105 129 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
106 130 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
107 131 *
108 132 * The watchdog is a timer provided by the GPTIMER IP core of the GRLIB.
109 133 *
110 134 */
111 135
112 136 LEON_Mask_interrupt( IRQ_GPTIMER_WATCHDOG ); // mask gptimer/watchdog interrupt during configuration
113 137
114 138 timer_configure( TIMER_WATCHDOG, CLKDIV_WATCHDOG, IRQ_SPARC_GPTIMER_WATCHDOG, watchdog_isr );
115 139
116 140 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG ); // clear gptimer/watchdog interrupt
117 141 }
118 142
119 143 void watchdog_stop(void)
120 144 {
121 145 LEON_Mask_interrupt( IRQ_GPTIMER_WATCHDOG ); // mask gptimer/watchdog interrupt line
122 146 timer_stop( TIMER_WATCHDOG );
123 147 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG ); // clear gptimer/watchdog interrupt
124 148 }
125 149
126 150 void watchdog_reload(void)
127 151 {
128 152 /** This function reloads the watchdog timer counter with the timer reload value.
129 153 *
130 154 * @param void
131 155 *
132 156 * @return void
133 157 *
134 158 */
135 159
136 160 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_LD;
137 161 }
138 162
139 163 void watchdog_start(void)
140 164 {
141 165 /** This function starts the watchdog timer.
142 166 *
143 167 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
144 168 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
145 169 *
146 170 */
147 171
148 172 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG );
149 173
150 174 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_CLEAR_IRQ;
151 175 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_LD;
152 176 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_EN;
153 177 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_IE;
154 178
155 179 LEON_Unmask_interrupt( IRQ_GPTIMER_WATCHDOG );
156 180
157 181 }
158 182
159 183 int enable_apbuart_transmitter( void ) // set the bit 1, TE Transmitter Enable to 1 in the APBUART control register
160 184 {
161 185 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
162 186
163 187 apbuart_regs->ctrl = APBUART_CTRL_REG_MASK_TE;
164 188
165 189 return 0;
166 190 }
167 191
168 192 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value)
169 193 {
170 194 /** This function sets the scaler reload register of the apbuart module
171 195 *
172 196 * @param regs is the address of the apbuart registers in memory
173 197 * @param value is the value that will be stored in the scaler register
174 198 *
175 199 * The value shall be set by the software to get data on the serial interface.
176 200 *
177 201 */
178 202
179 203 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) regs;
180 204
181 205 apbuart_regs->scaler = value;
182 206
183 207 BOOT_PRINTF1("OK *** apbuart port scaler reload register set to 0x%x\n", value)
184 208 }
185 209
186 //************
187 // RTEMS TASKS
210 /**
211 * @brief load_task starts and keeps the watchdog alive.
212 * @param argument
213 * @return
214 */
188 215
189 216 rtems_task load_task(rtems_task_argument argument)
190 217 {
191 218 BOOT_PRINTF("in LOAD *** \n")
192 219
193 220 rtems_status_code status;
194 221 unsigned int i;
195 222 unsigned int j;
196 223 rtems_name name_watchdog_rate_monotonic; // name of the watchdog rate monotonic
197 224 rtems_id watchdog_period_id; // id of the watchdog rate monotonic period
198 225
199 226 watchdog_period_id = RTEMS_ID_NONE;
200 227
201 228 name_watchdog_rate_monotonic = rtems_build_name( 'L', 'O', 'A', 'D' );
202 229
203 230 status = rtems_rate_monotonic_create( name_watchdog_rate_monotonic, &watchdog_period_id );
204 231 if( status != RTEMS_SUCCESSFUL ) {
205 232 PRINTF1( "in LOAD *** rtems_rate_monotonic_create failed with status of %d\n", status )
206 233 }
207 234
208 235 i = 0;
209 236 j = 0;
210 237
211 238 watchdog_configure();
212 239
213 240 watchdog_start();
214 241
215 242 set_sy_lfr_watchdog_enabled( true );
216 243
217 244 while(1){
218 245 status = rtems_rate_monotonic_period( watchdog_period_id, WATCHDOG_PERIOD );
219 246 watchdog_reload();
220 247 i = i + 1;
221 248 if ( i == WATCHDOG_LOOP_PRINTF )
222 249 {
223 250 i = 0;
224 251 j = j + 1;
225 252 PRINTF1("%d\n", j)
226 253 }
227 254 #ifdef DEBUG_WATCHDOG
228 255 if (j == WATCHDOG_LOOP_DEBUG )
229 256 {
230 257 status = rtems_task_delete(RTEMS_SELF);
231 258 }
232 259 #endif
233 260 }
234 261 }
235 262
263 /**
264 * @brief hous_task produces and sends HK each seconds
265 * @param argument
266 * @return
267 */
236 268 rtems_task hous_task(rtems_task_argument argument)
237 269 {
238 270 rtems_status_code status;
239 271 rtems_status_code spare_status;
240 272 rtems_id queue_id;
241 273 rtems_rate_monotonic_period_status period_status;
242 274 bool isSynchronized;
243 275
244 276 queue_id = RTEMS_ID_NONE;
245 277 memset(&period_status, 0, sizeof(rtems_rate_monotonic_period_status));
246 278 isSynchronized = false;
247 279
248 280 status = get_message_queue_id_send( &queue_id );
249 281 if (status != RTEMS_SUCCESSFUL)
250 282 {
251 283 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
252 284 }
253 285
254 286 BOOT_PRINTF("in HOUS ***\n");
255 287
256 288 if (rtems_rate_monotonic_ident( name_hk_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
257 289 status = rtems_rate_monotonic_create( name_hk_rate_monotonic, &HK_id );
258 290 if( status != RTEMS_SUCCESSFUL ) {
259 291 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status );
260 292 }
261 293 }
262 294
263 295 status = rtems_rate_monotonic_cancel(HK_id);
264 296 if( status != RTEMS_SUCCESSFUL ) {
265 297 PRINTF1( "ERR *** in HOUS *** rtems_rate_monotonic_cancel(HK_id) ***code: %d\n", status );
266 298 }
267 299 else {
268 300 DEBUG_PRINTF("OK *** in HOUS *** rtems_rate_monotonic_cancel(HK_id)\n");
269 301 }
270 302
271 303 // startup phase
272 304 status = rtems_rate_monotonic_period( HK_id, SY_LFR_TIME_SYN_TIMEOUT_in_ticks );
273 305 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
274 306 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
275 307 while( (period_status.state != RATE_MONOTONIC_EXPIRED)
276 308 && (isSynchronized == false) ) // after SY_LFR_TIME_SYN_TIMEOUT ms, starts HK anyway
277 309 {
278 310 if ((time_management_regs->coarse_time & VAL_LFR_SYNCHRONIZED) == INT32_ALL_0) // check time synchronization
279 311 {
280 312 isSynchronized = true;
281 313 }
282 314 else
283 315 {
284 316 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
285 317
286 318 status = rtems_task_wake_after( HK_SYNC_WAIT ); // wait HK_SYNCH_WAIT 100 ms = 10 * 10 ms
287 319 }
288 320 }
289 321 status = rtems_rate_monotonic_cancel(HK_id);
290 322 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
291 323
292 324 set_hk_lfr_reset_cause( POWER_ON );
293 325
294 326 while(1){ // launch the rate monotonic task
295 327 status = rtems_rate_monotonic_period( HK_id, HK_PERIOD );
296 328 if ( status != RTEMS_SUCCESSFUL ) {
297 329 PRINTF1( "in HOUS *** ERR period: %d\n", status);
298 330 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_6 );
299 331 }
300 332 else {
301 333 housekeeping_packet.packetSequenceControl[BYTE_0] = (unsigned char) (sequenceCounterHK >> SHIFT_1_BYTE);
302 334 housekeeping_packet.packetSequenceControl[BYTE_1] = (unsigned char) (sequenceCounterHK );
303 335 increment_seq_counter( &sequenceCounterHK );
304 336
305 337 housekeeping_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
306 338 housekeeping_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
307 339 housekeeping_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
308 340 housekeeping_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
309 341 housekeeping_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
310 342 housekeeping_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
311 343
312 344 spacewire_update_hk_lfr_link_state( &housekeeping_packet.lfr_status_word[0] );
313 345
314 346 spacewire_read_statistics();
315 347
316 348 update_hk_with_grspw_stats();
317 349
318 350 set_hk_lfr_time_not_synchro();
319 351
320 352 housekeeping_packet.hk_lfr_q_sd_fifo_size_max = hk_lfr_q_sd_fifo_size_max;
321 353 housekeeping_packet.hk_lfr_q_rv_fifo_size_max = hk_lfr_q_rv_fifo_size_max;
322 354 housekeeping_packet.hk_lfr_q_p0_fifo_size_max = hk_lfr_q_p0_fifo_size_max;
323 355 housekeeping_packet.hk_lfr_q_p1_fifo_size_max = hk_lfr_q_p1_fifo_size_max;
324 356 housekeeping_packet.hk_lfr_q_p2_fifo_size_max = hk_lfr_q_p2_fifo_size_max;
325 357
326 358 housekeeping_packet.sy_lfr_common_parameters_spare = parameter_dump_packet.sy_lfr_common_parameters_spare;
327 359 housekeeping_packet.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
328 360 get_temperatures( housekeeping_packet.hk_lfr_temp_scm );
329 361 get_v_e1_e2_f3( housekeeping_packet.hk_lfr_sc_v_f3 );
330 362 get_cpu_load( (unsigned char *) &housekeeping_packet.hk_lfr_cpu_load );
331 363
332 364 hk_lfr_le_me_he_update();
333 365
334 366 // SEND PACKET
335 367 status = rtems_message_queue_send( queue_id, &housekeeping_packet,
336 368 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
337 369 if (status != RTEMS_SUCCESSFUL) {
338 370 PRINTF1("in HOUS *** ERR send: %d\n", status)
339 371 }
340 372 }
341 373 }
342 374
343 375 PRINTF("in HOUS *** deleting task\n")
344 376
345 377 status = rtems_task_delete( RTEMS_SELF ); // should not return
346 378
347 379 return;
348 380 }
349 381
382 /**
383 * @brief filter is a Direct-Form-II filter implementation, mostly used to filter electric field for HK
384 * @param x, new sample
385 * @param ctx, filter context, used to store previous input and output samples
386 * @return a new filtered sample
387 */
350 388 int filter( int x, filter_ctx* ctx )
351 389 {
352 390 static const int b[NB_COEFFS][NB_COEFFS]={ {B00, B01, B02}, {B10, B11, B12}, {B20, B21, B22} };
353 391 static const int a[NB_COEFFS][NB_COEFFS]={ {A00, A01, A02}, {A10, A11, A12}, {A20, A21, A22} };
354 392 static const int b_gain[NB_COEFFS]={GAIN_B0, GAIN_B1, GAIN_B2};
355 393 static const int a_gain[NB_COEFFS]={GAIN_A0, GAIN_A1, GAIN_A2};
356 394
357 395 int_fast32_t W;
358 396 int i;
359 397
360 398 W = INIT_INT;
361 399 i = INIT_INT;
362 400
363 401 //Direct-Form-II
364 402 for ( i = 0; i < NB_COEFFS; i++ )
365 403 {
366 404 x = x << a_gain[i];
367 405 W = (x - ( a[i][COEFF1] * ctx->W[i][COEFF0] )
368 406 - ( a[i][COEFF2] * ctx->W[i][COEFF1] ) ) >> a_gain[i];
369 407 x = ( b[i][COEFF0] * W )
370 408 + ( b[i][COEFF1] * ctx->W[i][COEFF0] )
371 409 + ( b[i][COEFF2] * ctx->W[i][COEFF1] );
372 410 x = x >> b_gain[i];
373 411 ctx->W[i][1] = ctx->W[i][0];
374 412 ctx->W[i][0] = W;
375 413 }
376 414 return x;
377 415 }
378 416
417 /**
418 * @brief avgv_task pruduces HK rate elctrical field from F3 data
419 * @param argument
420 * @return
421 */
379 422 rtems_task avgv_task(rtems_task_argument argument)
380 423 {
381 424 #define MOVING_AVERAGE 16
382 425 rtems_status_code status;
383 426 static int32_t v[MOVING_AVERAGE] = {0};
384 427 static int32_t e1[MOVING_AVERAGE] = {0};
385 428 static int32_t e2[MOVING_AVERAGE] = {0};
386 429 static int old_v = 0;
387 430 static int old_e1 = 0;
388 431 static int old_e2 = 0;
389 432 int32_t current_v;
390 433 int32_t current_e1;
391 434 int32_t current_e2;
392 435 int32_t average_v;
393 436 int32_t average_e1;
394 437 int32_t average_e2;
395 438 int32_t newValue_v;
396 439 int32_t newValue_e1;
397 440 int32_t newValue_e2;
398 441 unsigned char k;
399 442 unsigned char indexOfOldValue;
400 443
401 444 static filter_ctx ctx_v = { { {0,0,0}, {0,0,0}, {0,0,0} } };
402 445 static filter_ctx ctx_e1 = { { {0,0,0}, {0,0,0}, {0,0,0} } };
403 446 static filter_ctx ctx_e2 = { { {0,0,0}, {0,0,0}, {0,0,0} } };
404 447
405 448 BOOT_PRINTF("in AVGV ***\n");
406 449
407 450 if (rtems_rate_monotonic_ident( name_avgv_rate_monotonic, &AVGV_id) != RTEMS_SUCCESSFUL) {
408 451 status = rtems_rate_monotonic_create( name_avgv_rate_monotonic, &AVGV_id );
409 452 if( status != RTEMS_SUCCESSFUL ) {
410 453 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status );
411 454 }
412 455 }
413 456
414 457 status = rtems_rate_monotonic_cancel(AVGV_id);
415 458 if( status != RTEMS_SUCCESSFUL ) {
416 459 PRINTF1( "ERR *** in AVGV *** rtems_rate_monotonic_cancel(AVGV_id) ***code: %d\n", status );
417 460 }
418 461 else {
419 462 DEBUG_PRINTF("OK *** in AVGV *** rtems_rate_monotonic_cancel(AVGV_id)\n");
420 463 }
421 464
422 465 // initialize values
423 466 indexOfOldValue = MOVING_AVERAGE - 1;
424 467 current_v = 0;
425 468 current_e1 = 0;
426 469 current_e2 = 0;
427 470 average_v = 0;
428 471 average_e1 = 0;
429 472 average_e2 = 0;
430 473 newValue_v = 0;
431 474 newValue_e1 = 0;
432 475 newValue_e2 = 0;
433 476
434 477 k = INIT_CHAR;
435 478
436 479 while(1)
437 480 { // launch the rate monotonic task
438 481 status = rtems_rate_monotonic_period( AVGV_id, AVGV_PERIOD );
439 482 if ( status != RTEMS_SUCCESSFUL )
440 483 {
441 484 PRINTF1( "in AVGV *** ERR period: %d\n", status);
442 485 }
443 486 else
444 487 {
445 488 current_v = waveform_picker_regs->v;
446 489 current_e1 = waveform_picker_regs->e1;
447 490 current_e2 = waveform_picker_regs->e2;
448 491 if ( (current_v != old_v)
449 492 || (current_e1 != old_e1)
450 493 || (current_e2 != old_e2))
451 494 {
452 495 average_v = filter( current_v, &ctx_v );
453 496 average_e1 = filter( current_e1, &ctx_e1 );
454 497 average_e2 = filter( current_e2, &ctx_e2 );
455 498
456 499 //update int16 values
457 500 hk_lfr_sc_v_f3_as_int16 = (int16_t) average_v;
458 501 hk_lfr_sc_e1_f3_as_int16 = (int16_t) average_e1;
459 502 hk_lfr_sc_e2_f3_as_int16 = (int16_t) average_e2;
460 503 }
461 504 old_v = current_v;
462 505 old_e1 = current_e1;
463 506 old_e2 = current_e2;
464 507 }
465 508 }
466 509
467 510 PRINTF("in AVGV *** deleting task\n");
468 511
469 512 status = rtems_task_delete( RTEMS_SELF ); // should not return
470 513
471 514 return;
472 515 }
473 516
474 517 rtems_task dumb_task( rtems_task_argument unused )
475 518 {
476 519 /** This RTEMS taks is used to print messages without affecting the general behaviour of the software.
477 520 *
478 521 * @param unused is the starting argument of the RTEMS task
479 522 *
480 523 * The DUMB taks waits for RTEMS events and print messages depending on the incoming events.
481 524 *
482 525 */
483 526
484 527 unsigned int i;
485 528 unsigned int intEventOut;
486 529 unsigned int coarse_time = 0;
487 530 unsigned int fine_time = 0;
488 531 rtems_event_set event_out;
489 532
490 533 event_out = EVENT_SETS_NONE_PENDING;
491 534
492 535 BOOT_PRINTF("in DUMB *** \n")
493 536
494 537 while(1){
495 538 rtems_event_receive(RTEMS_EVENT_0 | RTEMS_EVENT_1 | RTEMS_EVENT_2 | RTEMS_EVENT_3
496 539 | RTEMS_EVENT_4 | RTEMS_EVENT_5 | RTEMS_EVENT_6 | RTEMS_EVENT_7
497 540 | RTEMS_EVENT_8 | RTEMS_EVENT_9 | RTEMS_EVENT_12 | RTEMS_EVENT_13
498 541 | RTEMS_EVENT_14,
499 542 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT
500 543 intEventOut = (unsigned int) event_out;
501 544 for ( i=0; i<NB_RTEMS_EVENTS; i++)
502 545 {
503 546 if ( ((intEventOut >> i) & 1) != 0)
504 547 {
505 548 coarse_time = time_management_regs->coarse_time;
506 549 fine_time = time_management_regs->fine_time;
507 550 if (i==EVENT_12)
508 551 {
509 552 PRINTF1("%s\n", DUMB_MESSAGE_12)
510 553 }
511 554 if (i==EVENT_13)
512 555 {
513 556 PRINTF1("%s\n", DUMB_MESSAGE_13)
514 557 }
515 558 if (i==EVENT_14)
516 559 {
517 560 PRINTF1("%s\n", DUMB_MESSAGE_1)
518 561 }
519 562 }
520 563 }
521 564 }
522 565 }
523 566
524 567 rtems_task scrubbing_task( rtems_task_argument unused )
525 568 {
526 569 /** This RTEMS taks is used to avoid entering IDLE task and also scrub memory to increase scubbing frequency.
527 570 *
528 571 * @param unused is the starting argument of the RTEMS task
529 572 *
530 573 * The scrubbing reads continuously memory when no other tasks are ready.
531 574 *
532 575 */
533 576
534 577 BOOT_PRINTF("in SCRUBBING *** \n");
535 578 volatile int i=0;
536 579 volatile float valuef = 1.;
537 580 volatile uint32_t* RAM=(uint32_t*)0x40000000;
538 581 volatile uint32_t value;
539 582 #ifdef ENABLE_SCRUBBING_COUNTER
540 583 housekeeping_packet.lfr_fpga_version[BYTE_0] = 0;
541 584 #endif
542 585 while(1){
543 586 i=(i+1)%(1024*1024);
544 587 valuef += 10.f*(float)RAM[i];
545 588 #ifdef ENABLE_SCRUBBING_COUNTER
546 589 if(i==0)
547 590 {
548 591 housekeeping_packet.lfr_fpga_version[BYTE_0] += 1;
549 592 }
550 593 #endif
551 594 }
552 595 }
553 596
554 597 rtems_task calibration_sweep_task( rtems_task_argument unused )
555 598 {
556 599 /** This RTEMS taks is used to change calibration signal smapling frequency between snapshots.
557 600 *
558 601 * @param unused is the starting argument of the RTEMS task
559 602 *
560 603 * If calibration is enabled, this task will divide by two the calibration signal smapling frequency between snapshots.
561 604 * When minimum sampling frequency is reach it will jump to maximum sampling frequency to loop indefinitely.
562 605 *
563 606 */
564 607 rtems_event_set event_out;
565 608 BOOT_PRINTF("in calibration sweep *** \n");
566 609 rtems_interval ticks_per_seconds = rtems_clock_get_ticks_per_second();
567 610 while(1){
568 611 // Waiting for next F0 snapshot
569 612 rtems_event_receive(RTEMS_EVENT_CAL_SWEEP_WAKE, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out);
570 613 if(time_management_regs->calDACCtrl & BIT_CAL_ENABLE)
571 614 {
572 615 unsigned int delta_snapshot;
573 616 delta_snapshot = (parameter_dump_packet.sy_lfr_n_swf_p[0] * CONST_256)
574 617 + parameter_dump_packet.sy_lfr_n_swf_p[1];
575 618 // We are woken almost in the center of a snapshot -> let's wait for sy_lfr_n_swf_p / 2
576 619 rtems_task_wake_after( ticks_per_seconds * delta_snapshot / 2);
577 620 if(time_management_regs->calDivisor >= CAL_F_DIVISOR_MAX){
578 621 time_management_regs->calDivisor = CAL_F_DIVISOR_MIN;
579 622 }
580 623 else{
581 624 time_management_regs->calDivisor *= 2;
582 625 }
583 626 }
584 627
585 628
586 629
587 630 }
588 631
589 632 }
590 633
591 634
592 635 //*****************************
593 636 // init housekeeping parameters
594 637
595 638 void init_housekeeping_parameters( void )
596 639 {
597 640 /** This function initialize the housekeeping_packet global variable with default values.
598 641 *
599 642 */
600 643
601 644 unsigned int i = 0;
602 645 unsigned char *parameters;
603 646 unsigned char sizeOfHK;
604 647
605 648 sizeOfHK = sizeof( Packet_TM_LFR_HK_t );
606 649
607 650 parameters = (unsigned char*) &housekeeping_packet;
608 651
609 652 for(i = 0; i< sizeOfHK; i++)
610 653 {
611 654 parameters[i] = INIT_CHAR;
612 655 }
613 656
614 657 housekeeping_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
615 658 housekeeping_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
616 659 housekeeping_packet.reserved = DEFAULT_RESERVED;
617 660 housekeeping_packet.userApplication = CCSDS_USER_APP;
618 661 housekeeping_packet.packetID[0] = (unsigned char) (APID_TM_HK >> SHIFT_1_BYTE);
619 662 housekeeping_packet.packetID[1] = (unsigned char) (APID_TM_HK);
620 663 housekeeping_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
621 664 housekeeping_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
622 665 housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> SHIFT_1_BYTE);
623 666 housekeeping_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
624 667 housekeeping_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
625 668 housekeeping_packet.serviceType = TM_TYPE_HK;
626 669 housekeeping_packet.serviceSubType = TM_SUBTYPE_HK;
627 670 housekeeping_packet.destinationID = TM_DESTINATION_ID_GROUND;
628 671 housekeeping_packet.sid = SID_HK;
629 672
630 673 // init status word
631 674 housekeeping_packet.lfr_status_word[0] = DEFAULT_STATUS_WORD_BYTE0;
632 675 housekeeping_packet.lfr_status_word[1] = DEFAULT_STATUS_WORD_BYTE1;
633 676 // init software version
634 677 housekeeping_packet.lfr_sw_version[0] = SW_VERSION_N1;
635 678 housekeeping_packet.lfr_sw_version[1] = SW_VERSION_N2;
636 679 housekeeping_packet.lfr_sw_version[BYTE_2] = SW_VERSION_N3;
637 680 housekeeping_packet.lfr_sw_version[BYTE_3] = SW_VERSION_N4;
638 681 // init fpga version
639 682 parameters = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
640 683 housekeeping_packet.lfr_fpga_version[BYTE_0] = parameters[BYTE_1]; // n1
641 684 housekeeping_packet.lfr_fpga_version[BYTE_1] = parameters[BYTE_2]; // n2
642 685 housekeeping_packet.lfr_fpga_version[BYTE_2] = parameters[BYTE_3]; // n3
643 686
644 687 housekeeping_packet.hk_lfr_q_sd_fifo_size = MSG_QUEUE_COUNT_SEND;
645 688 housekeeping_packet.hk_lfr_q_rv_fifo_size = MSG_QUEUE_COUNT_RECV;
646 689 housekeeping_packet.hk_lfr_q_p0_fifo_size = MSG_QUEUE_COUNT_PRC0;
647 690 housekeeping_packet.hk_lfr_q_p1_fifo_size = MSG_QUEUE_COUNT_PRC1;
648 691 housekeeping_packet.hk_lfr_q_p2_fifo_size = MSG_QUEUE_COUNT_PRC2;
649 692 }
650 693
651 694 void increment_seq_counter( unsigned short *packetSequenceControl )
652 695 {
653 696 /** This function increment the sequence counter passes in argument.
654 697 *
655 698 * The increment does not affect the grouping flag. In case of an overflow, the counter is reset to 0.
656 699 *
657 700 */
658 701
659 702 unsigned short segmentation_grouping_flag;
660 703 unsigned short sequence_cnt;
661 704
662 705 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE; // keep bits 7 downto 6
663 706 sequence_cnt = (*packetSequenceControl) & SEQ_CNT_MASK; // [0011 1111 1111 1111]
664 707
665 708 if ( sequence_cnt < SEQ_CNT_MAX)
666 709 {
667 710 sequence_cnt = sequence_cnt + 1;
668 711 }
669 712 else
670 713 {
671 714 sequence_cnt = 0;
672 715 }
673 716
674 717 *packetSequenceControl = segmentation_grouping_flag | sequence_cnt ;
675 718 }
676 719
677 720 void getTime( unsigned char *time)
678 721 {
679 722 /** This function write the current local time in the time buffer passed in argument.
680 723 *
681 724 */
682 725
683 726 time[0] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_3_BYTES);
684 727 time[1] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_2_BYTES);
685 728 time[2] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_1_BYTE);
686 729 time[3] = (unsigned char) (time_management_regs->coarse_time);
687 730 time[4] = (unsigned char) (time_management_regs->fine_time>>SHIFT_1_BYTE);
688 731 time[5] = (unsigned char) (time_management_regs->fine_time);
689 732 }
690 733
691 734 unsigned long long int getTimeAsUnsignedLongLongInt( )
692 735 {
693 736 /** This function write the current local time in the time buffer passed in argument.
694 737 *
695 738 */
696 739 unsigned long long int time;
697 740
698 741 time = ( (unsigned long long int) (time_management_regs->coarse_time & COARSE_TIME_MASK) << SHIFT_2_BYTES )
699 742 + time_management_regs->fine_time;
700 743
701 744 return time;
702 745 }
703 746
704 747 void get_temperatures( unsigned char *temperatures )
705 748 {
706 749 unsigned char* temp_scm_ptr;
707 750 unsigned char* temp_pcb_ptr;
708 751 unsigned char* temp_fpga_ptr;
709 752
710 753 // SEL1 SEL0
711 754 // 0 0 => PCB
712 755 // 0 1 => FPGA
713 756 // 1 0 => SCM
714 757
715 758 temp_scm_ptr = (unsigned char *) &time_management_regs->temp_scm;
716 759 temp_pcb_ptr = (unsigned char *) &time_management_regs->temp_pcb;
717 760 temp_fpga_ptr = (unsigned char *) &time_management_regs->temp_fpga;
718 761
719 762 temperatures[ BYTE_0 ] = temp_scm_ptr[ BYTE_2 ];
720 763 temperatures[ BYTE_1 ] = temp_scm_ptr[ BYTE_3 ];
721 764 temperatures[ BYTE_2 ] = temp_pcb_ptr[ BYTE_2 ];
722 765 temperatures[ BYTE_3 ] = temp_pcb_ptr[ BYTE_3 ];
723 766 temperatures[ BYTE_4 ] = temp_fpga_ptr[ BYTE_2 ];
724 767 temperatures[ BYTE_5 ] = temp_fpga_ptr[ BYTE_3 ];
725 768 }
726 769
727 770 void get_v_e1_e2_f3( unsigned char *spacecraft_potential )
728 771 {
729 772 unsigned char* v_ptr;
730 773 unsigned char* e1_ptr;
731 774 unsigned char* e2_ptr;
732 775
733 776 v_ptr = (unsigned char *) &hk_lfr_sc_v_f3_as_int16;
734 777 e1_ptr = (unsigned char *) &hk_lfr_sc_e1_f3_as_int16;
735 778 e2_ptr = (unsigned char *) &hk_lfr_sc_e2_f3_as_int16;
736 779
737 780 spacecraft_potential[BYTE_0] = v_ptr[0];
738 781 spacecraft_potential[BYTE_1] = v_ptr[1];
739 782 spacecraft_potential[BYTE_2] = e1_ptr[0];
740 783 spacecraft_potential[BYTE_3] = e1_ptr[1];
741 784 spacecraft_potential[BYTE_4] = e2_ptr[0];
742 785 spacecraft_potential[BYTE_5] = e2_ptr[1];
743 786 }
744 787
788 /**
789 * @brief get_cpu_load, computes CPU load, CPU load average and CPU load max
790 * @param resource_statistics stores:
791 * - CPU load at index 0
792 * - CPU load max at index 1
793 * - CPU load average at index 2
794 *
795 * The CPU load average is computed on the last 60 values with a simple moving average.
796 */
745 797 void get_cpu_load( unsigned char *resource_statistics )
746 798 {
747 799 #define LOAD_AVG_SIZE 60
748 800 static unsigned char cpu_load_hist[LOAD_AVG_SIZE]={0};
749 801 static char old_avg_pos=0;
750 802 static unsigned int cpu_load_avg;
751 803 unsigned char cpu_load;
752 804
753 805 cpu_load = lfr_rtems_cpu_usage_report();
754 806
755 807 // HK_LFR_CPU_LOAD
756 resource_statistics[0] = cpu_load;
808 resource_statistics[BYTE_0] = cpu_load;
757 809
758 810 // HK_LFR_CPU_LOAD_MAX
759 if (cpu_load > resource_statistics[1])
811 if (cpu_load > resource_statistics[BYTE_1])
760 812 {
761 resource_statistics[1] = cpu_load;
813 resource_statistics[BYTE_1] = cpu_load;
762 814 }
763 815
764 816 cpu_load_avg = cpu_load_avg - (unsigned int)cpu_load_hist[(int)old_avg_pos] + (unsigned int)cpu_load;
765 817 cpu_load_hist[(int)old_avg_pos] = cpu_load;
766 818 old_avg_pos += 1;
767 819 old_avg_pos %= LOAD_AVG_SIZE;
768 820 // CPU_LOAD_AVE
769 821 resource_statistics[BYTE_2] = (unsigned char)(cpu_load_avg / LOAD_AVG_SIZE);
770 822 // this will change the way LFR compute usage
771 823 #ifndef PRINT_TASK_STATISTICS
772 824 rtems_cpu_usage_reset();
773 825 #endif
774 826
775 827 }
776 828
777 829 void set_hk_lfr_sc_potential_flag( bool state )
778 830 {
779 831 if (state == true)
780 832 {
781 833 housekeeping_packet.lfr_status_word[1] =
782 834 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_SC_POTENTIAL_FLAG_BIT; // [0100 0000]
783 835 }
784 836 else
785 837 {
786 838 housekeeping_packet.lfr_status_word[1] =
787 839 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_SC_POTENTIAL_FLAG_MASK; // [1011 1111]
788 840 }
789 841 }
790 842
791 843 void set_sy_lfr_pas_filter_enabled( bool state )
792 844 {
793 845 if (state == true)
794 846 {
795 847 housekeeping_packet.lfr_status_word[1] =
796 848 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_PAS_FILTER_ENABLED_BIT; // [0010 0000]
797 849 }
798 850 else
799 851 {
800 852 housekeeping_packet.lfr_status_word[1] =
801 853 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_PAS_FILTER_ENABLED_MASK; // [1101 1111]
802 854 }
803 855 }
804 856
805 857 void set_sy_lfr_watchdog_enabled( bool state )
806 858 {
807 859 if (state == true)
808 860 {
809 861 housekeeping_packet.lfr_status_word[1] =
810 862 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_WATCHDOG_BIT; // [0001 0000]
811 863 }
812 864 else
813 865 {
814 866 housekeeping_packet.lfr_status_word[1] =
815 867 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_WATCHDOG_MASK; // [1110 1111]
816 868 }
817 869 }
818 870
819 871 void set_hk_lfr_calib_enable( bool state )
820 872 {
821 873 if (state == true)
822 874 {
823 875 housekeeping_packet.lfr_status_word[1] =
824 876 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_CALIB_BIT; // [0000 1000]
825 877 }
826 878 else
827 879 {
828 880 housekeeping_packet.lfr_status_word[1] =
829 881 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_CALIB_MASK; // [1111 0111]
830 882 }
831 883 }
832 884
833 885 void set_hk_lfr_reset_cause( enum lfr_reset_cause_t lfr_reset_cause )
834 886 {
835 887 housekeeping_packet.lfr_status_word[1] =
836 888 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_RESET_CAUSE_MASK; // [1111 1000]
837 889
838 890 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1]
839 891 | (lfr_reset_cause & STATUS_WORD_RESET_CAUSE_BITS ); // [0000 0111]
840 892
841 893 }
842 894
843 895 void increment_hk_counter( unsigned char newValue, unsigned char oldValue, unsigned int *counter )
844 896 {
845 897 int delta;
846 898
847 899 delta = 0;
848 900
849 901 if (newValue >= oldValue)
850 902 {
851 903 delta = newValue - oldValue;
852 904 }
853 905 else
854 906 {
855 907 delta = (CONST_256 - oldValue) + newValue;
856 908 }
857 909
858 910 *counter = *counter + delta;
859 911 }
860 912
913 // Low severity error counters update
861 914 void hk_lfr_le_update( void )
862 915 {
863 916 static hk_lfr_le_t old_hk_lfr_le = {0};
864 917 hk_lfr_le_t new_hk_lfr_le;
865 918 unsigned int counter;
866 919
867 920 counter = (((unsigned int) housekeeping_packet.hk_lfr_le_cnt[0]) * CONST_256) + housekeeping_packet.hk_lfr_le_cnt[1];
868 921
869 922 // DPU
870 923 new_hk_lfr_le.dpu_spw_parity = housekeeping_packet.hk_lfr_dpu_spw_parity;
871 924 new_hk_lfr_le.dpu_spw_disconnect= housekeeping_packet.hk_lfr_dpu_spw_disconnect;
872 925 new_hk_lfr_le.dpu_spw_escape = housekeeping_packet.hk_lfr_dpu_spw_escape;
873 926 new_hk_lfr_le.dpu_spw_credit = housekeeping_packet.hk_lfr_dpu_spw_credit;
874 927 new_hk_lfr_le.dpu_spw_write_sync= housekeeping_packet.hk_lfr_dpu_spw_write_sync;
875 928 // TIMECODE
876 929 new_hk_lfr_le.timecode_erroneous= housekeeping_packet.hk_lfr_timecode_erroneous;
877 930 new_hk_lfr_le.timecode_missing = housekeeping_packet.hk_lfr_timecode_missing;
878 931 new_hk_lfr_le.timecode_invalid = housekeeping_packet.hk_lfr_timecode_invalid;
879 932 // TIME
880 933 new_hk_lfr_le.time_timecode_it = housekeeping_packet.hk_lfr_time_timecode_it;
881 934 new_hk_lfr_le.time_not_synchro = housekeeping_packet.hk_lfr_time_not_synchro;
882 935 new_hk_lfr_le.time_timecode_ctr = housekeeping_packet.hk_lfr_time_timecode_ctr;
883 936 //AHB
884 937 new_hk_lfr_le.ahb_correctable = housekeeping_packet.hk_lfr_ahb_correctable;
885 938 // housekeeping_packet.hk_lfr_dpu_spw_rx_ahb => not handled by the grspw driver
886 939 // housekeeping_packet.hk_lfr_dpu_spw_tx_ahb => not handled by the grspw driver
887 940
888 941 // update the le counter
889 942 // DPU
890 943 increment_hk_counter( new_hk_lfr_le.dpu_spw_parity, old_hk_lfr_le.dpu_spw_parity, &counter );
891 944 increment_hk_counter( new_hk_lfr_le.dpu_spw_disconnect,old_hk_lfr_le.dpu_spw_disconnect, &counter );
892 945 increment_hk_counter( new_hk_lfr_le.dpu_spw_escape, old_hk_lfr_le.dpu_spw_escape, &counter );
893 946 increment_hk_counter( new_hk_lfr_le.dpu_spw_credit, old_hk_lfr_le.dpu_spw_credit, &counter );
894 947 increment_hk_counter( new_hk_lfr_le.dpu_spw_write_sync,old_hk_lfr_le.dpu_spw_write_sync, &counter );
895 948 // TIMECODE
896 949 increment_hk_counter( new_hk_lfr_le.timecode_erroneous,old_hk_lfr_le.timecode_erroneous, &counter );
897 950 increment_hk_counter( new_hk_lfr_le.timecode_missing, old_hk_lfr_le.timecode_missing, &counter );
898 951 increment_hk_counter( new_hk_lfr_le.timecode_invalid, old_hk_lfr_le.timecode_invalid, &counter );
899 952 // TIME
900 953 increment_hk_counter( new_hk_lfr_le.time_timecode_it, old_hk_lfr_le.time_timecode_it, &counter );
901 954 increment_hk_counter( new_hk_lfr_le.time_not_synchro, old_hk_lfr_le.time_not_synchro, &counter );
902 955 increment_hk_counter( new_hk_lfr_le.time_timecode_ctr, old_hk_lfr_le.time_timecode_ctr, &counter );
903 956 // AHB
904 957 increment_hk_counter( new_hk_lfr_le.ahb_correctable, old_hk_lfr_le.ahb_correctable, &counter );
905 958
906 959 // DPU
907 960 old_hk_lfr_le.dpu_spw_parity = new_hk_lfr_le.dpu_spw_parity;
908 961 old_hk_lfr_le.dpu_spw_disconnect= new_hk_lfr_le.dpu_spw_disconnect;
909 962 old_hk_lfr_le.dpu_spw_escape = new_hk_lfr_le.dpu_spw_escape;
910 963 old_hk_lfr_le.dpu_spw_credit = new_hk_lfr_le.dpu_spw_credit;
911 964 old_hk_lfr_le.dpu_spw_write_sync= new_hk_lfr_le.dpu_spw_write_sync;
912 965 // TIMECODE
913 966 old_hk_lfr_le.timecode_erroneous= new_hk_lfr_le.timecode_erroneous;
914 967 old_hk_lfr_le.timecode_missing = new_hk_lfr_le.timecode_missing;
915 968 old_hk_lfr_le.timecode_invalid = new_hk_lfr_le.timecode_invalid;
916 969 // TIME
917 970 old_hk_lfr_le.time_timecode_it = new_hk_lfr_le.time_timecode_it;
918 971 old_hk_lfr_le.time_not_synchro = new_hk_lfr_le.time_not_synchro;
919 972 old_hk_lfr_le.time_timecode_ctr = new_hk_lfr_le.time_timecode_ctr;
920 973 //AHB
921 974 old_hk_lfr_le.ahb_correctable = new_hk_lfr_le.ahb_correctable;
922 975 // housekeeping_packet.hk_lfr_dpu_spw_rx_ahb => not handled by the grspw driver
923 976 // housekeeping_packet.hk_lfr_dpu_spw_tx_ahb => not handled by the grspw driver
924 977
925 978 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
926 979 // LE
927 980 housekeeping_packet.hk_lfr_le_cnt[0] = (unsigned char) ((counter & BYTE0_MASK) >> SHIFT_1_BYTE);
928 981 housekeeping_packet.hk_lfr_le_cnt[1] = (unsigned char) (counter & BYTE1_MASK);
929 982 }
930 983
984 // Medium severity error counters update
931 985 void hk_lfr_me_update( void )
932 986 {
933 987 static hk_lfr_me_t old_hk_lfr_me = {0};
934 988 hk_lfr_me_t new_hk_lfr_me;
935 989 unsigned int counter;
936 990
937 991 counter = (((unsigned int) housekeeping_packet.hk_lfr_me_cnt[0]) * CONST_256) + housekeeping_packet.hk_lfr_me_cnt[1];
938 992
939 993 // get the current values
940 994 new_hk_lfr_me.dpu_spw_early_eop = housekeeping_packet.hk_lfr_dpu_spw_early_eop;
941 995 new_hk_lfr_me.dpu_spw_invalid_addr = housekeeping_packet.hk_lfr_dpu_spw_invalid_addr;
942 996 new_hk_lfr_me.dpu_spw_eep = housekeeping_packet.hk_lfr_dpu_spw_eep;
943 997 new_hk_lfr_me.dpu_spw_rx_too_big = housekeeping_packet.hk_lfr_dpu_spw_rx_too_big;
944 998
945 999 // update the me counter
946 1000 increment_hk_counter( new_hk_lfr_me.dpu_spw_early_eop, old_hk_lfr_me.dpu_spw_early_eop, &counter );
947 1001 increment_hk_counter( new_hk_lfr_me.dpu_spw_invalid_addr, old_hk_lfr_me.dpu_spw_invalid_addr, &counter );
948 1002 increment_hk_counter( new_hk_lfr_me.dpu_spw_eep, old_hk_lfr_me.dpu_spw_eep, &counter );
949 1003 increment_hk_counter( new_hk_lfr_me.dpu_spw_rx_too_big, old_hk_lfr_me.dpu_spw_rx_too_big, &counter );
950 1004
951 1005 // store the counters for the next time
952 1006 old_hk_lfr_me.dpu_spw_early_eop = new_hk_lfr_me.dpu_spw_early_eop;
953 1007 old_hk_lfr_me.dpu_spw_invalid_addr = new_hk_lfr_me.dpu_spw_invalid_addr;
954 1008 old_hk_lfr_me.dpu_spw_eep = new_hk_lfr_me.dpu_spw_eep;
955 1009 old_hk_lfr_me.dpu_spw_rx_too_big = new_hk_lfr_me.dpu_spw_rx_too_big;
956 1010
957 1011 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
958 1012 // ME
959 1013 housekeeping_packet.hk_lfr_me_cnt[0] = (unsigned char) ((counter & BYTE0_MASK) >> SHIFT_1_BYTE);
960 1014 housekeeping_packet.hk_lfr_me_cnt[1] = (unsigned char) (counter & BYTE1_MASK);
961 1015 }
962 1016
1017 // High severity error counters update
963 1018 void hk_lfr_le_me_he_update()
964 1019 {
965 1020
966 1021 unsigned int hk_lfr_he_cnt;
967 1022
968 1023 hk_lfr_he_cnt = (((unsigned int) housekeeping_packet.hk_lfr_he_cnt[0]) * 256) + housekeeping_packet.hk_lfr_he_cnt[1];
969 1024
970 1025 //update the low severity error counter
971 1026 hk_lfr_le_update( );
972 1027
973 1028 //update the medium severity error counter
974 1029 hk_lfr_me_update();
975 1030
976 1031 //update the high severity error counter
977 1032 hk_lfr_he_cnt = 0;
978 1033
979 1034 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
980 1035 // HE
981 1036 housekeeping_packet.hk_lfr_he_cnt[0] = (unsigned char) ((hk_lfr_he_cnt & BYTE0_MASK) >> SHIFT_1_BYTE);
982 1037 housekeeping_packet.hk_lfr_he_cnt[1] = (unsigned char) (hk_lfr_he_cnt & BYTE1_MASK);
983 1038
984 1039 }
985 1040
986 1041 void set_hk_lfr_time_not_synchro()
987 1042 {
988 1043 static unsigned char synchroLost = 1;
989 1044 int synchronizationBit;
990 1045
991 1046 // get the synchronization bit
992 1047 synchronizationBit =
993 1048 (time_management_regs->coarse_time & VAL_LFR_SYNCHRONIZED) >> BIT_SYNCHRONIZATION; // 1000 0000 0000 0000
994 1049
995 1050 switch (synchronizationBit)
996 1051 {
997 1052 case 0:
998 1053 if (synchroLost == 1)
999 1054 {
1000 1055 synchroLost = 0;
1001 1056 }
1002 1057 break;
1003 1058 case 1:
1004 1059 if (synchroLost == 0 )
1005 1060 {
1006 1061 synchroLost = 1;
1007 1062 increase_unsigned_char_counter(&housekeeping_packet.hk_lfr_time_not_synchro);
1008 1063 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_NOT_SYNCHRO );
1009 1064 }
1010 1065 break;
1011 1066 default:
1012 1067 PRINTF1("in hk_lfr_time_not_synchro *** unexpected value for synchronizationBit = %d\n", synchronizationBit);
1013 1068 break;
1014 1069 }
1015 1070
1016 1071 }
1017 1072
1018 1073 void set_hk_lfr_ahb_correctable() // CRITICITY L
1019 1074 {
1020 1075 /** This function builds the error counter hk_lfr_ahb_correctable using the statistics provided
1021 1076 * by the Cache Control Register (ASI 2, offset 0) and in the Register Protection Control Register (ASR16) on the
1022 1077 * detected errors in the cache, in the integer unit and in the floating point unit.
1023 1078 *
1024 1079 * @param void
1025 1080 *
1026 1081 * @return void
1027 1082 *
1028 1083 * All errors are summed to set the value of the hk_lfr_ahb_correctable counter.
1029 1084 *
1030 1085 */
1031 1086
1032 1087 unsigned int ahb_correctable;
1033 1088 unsigned int instructionErrorCounter;
1034 1089 unsigned int dataErrorCounter;
1035 1090 unsigned int fprfErrorCounter;
1036 1091 unsigned int iurfErrorCounter;
1037 1092
1038 1093 instructionErrorCounter = 0;
1039 1094 dataErrorCounter = 0;
1040 1095 fprfErrorCounter = 0;
1041 1096 iurfErrorCounter = 0;
1042 1097
1043 1098 CCR_getInstructionAndDataErrorCounters( &instructionErrorCounter, &dataErrorCounter);
1044 1099 ASR16_get_FPRF_IURF_ErrorCounters( &fprfErrorCounter, &iurfErrorCounter);
1045 1100
1046 1101 ahb_correctable = instructionErrorCounter
1047 1102 + dataErrorCounter
1048 1103 + fprfErrorCounter
1049 1104 + iurfErrorCounter
1050 1105 + housekeeping_packet.hk_lfr_ahb_correctable;
1051 1106
1052 1107 housekeeping_packet.hk_lfr_ahb_correctable = (unsigned char) (ahb_correctable & INT8_ALL_F); // [1111 1111]
1053 1108
1054 1109 }
Show file before | Show file after | Hide comments
@@ -1,1608 +1,1632
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** Functions related to the SpaceWire interface.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * A group of functions to handle SpaceWire transmissions:
7 31 * - configuration of the SpaceWire link
8 32 * - SpaceWire related interruption requests processing
9 33 * - transmission of TeleMetry packets by a dedicated RTEMS task
10 34 * - reception of TeleCommands by a dedicated RTEMS task
11 35 *
12 36 */
13 37
14 38 #include "fsw_spacewire.h"
15 39
16 40 rtems_name semq_name = 0;
17 41 rtems_id semq_id = RTEMS_ID_NONE;
18 42
19 43 //*****************
20 44 // waveform headers
21 45 Header_TM_LFR_SCIENCE_CWF_t headerCWF = {0};
22 46 Header_TM_LFR_SCIENCE_SWF_t headerSWF = {0};
23 47 Header_TM_LFR_SCIENCE_ASM_t headerASM = {0};
24 48
25 49 unsigned char previousTimecodeCtr = 0;
26 50 unsigned int *grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER);
27 51
28 52 //***********
29 53 // RTEMS TASK
30 54 rtems_task spiq_task(rtems_task_argument unused)
31 55 {
32 56 /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver.
33 57 *
34 58 * @param unused is the starting argument of the RTEMS task
35 59 *
36 60 */
37 61
38 62 rtems_event_set event_out;
39 63 rtems_status_code status;
40 64 int linkStatus;
41 65
42 66 event_out = EVENT_SETS_NONE_PENDING;
43 67 linkStatus = 0;
44 68
45 69 BOOT_PRINTF("in SPIQ *** \n")
46 70
47 71 while(true){
48 72 rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT
49 73 PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n")
50 74
51 75 // [0] SUSPEND RECV AND SEND TASKS
52 76 status = rtems_task_suspend( Task_id[ TASKID_RECV ] );
53 77 if ( status != RTEMS_SUCCESSFUL ) {
54 78 PRINTF("in SPIQ *** ERR suspending RECV Task\n")
55 79 }
56 80 status = rtems_task_suspend( Task_id[ TASKID_SEND ] );
57 81 if ( status != RTEMS_SUCCESSFUL ) {
58 82 PRINTF("in SPIQ *** ERR suspending SEND Task\n")
59 83 }
60 84
61 85 // [1] CHECK THE LINK
62 86 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1)
63 87 if ( linkStatus != SPW_LINK_OK) {
64 88 PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus)
65 89 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
66 90 }
67 91
68 92 // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT
69 93 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2)
70 94 if ( linkStatus != SPW_LINK_OK ) // [2.a] not in run state, reset the link
71 95 {
72 96 spacewire_read_statistics();
73 97 status = spacewire_several_connect_attemps( );
74 98 }
75 99 else // [2.b] in run state, start the link
76 100 {
77 101 status = spacewire_stop_and_start_link( fdSPW ); // start the link
78 102 if ( status != RTEMS_SUCCESSFUL)
79 103 {
80 104 PRINTF1("in SPIQ *** ERR spacewire_stop_and_start_link %d\n", status)
81 105 }
82 106 }
83 107
84 108 // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS
85 109 if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully
86 110 {
87 111 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
88 112 if ( status != RTEMS_SUCCESSFUL ) {
89 113 PRINTF("in SPIQ *** ERR resuming SEND Task\n")
90 114 }
91 115 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
92 116 if ( status != RTEMS_SUCCESSFUL ) {
93 117 PRINTF("in SPIQ *** ERR resuming RECV Task\n")
94 118 }
95 119 }
96 120 else // [3.b] the link is not in run state, go in STANDBY mode
97 121 {
98 122 status = enter_mode_standby();
99 123 if ( status != RTEMS_SUCCESSFUL )
100 124 {
101 125 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
102 126 }
103 127 {
104 128 updateLFRCurrentMode( LFR_MODE_STANDBY );
105 129 }
106 130 // wake the LINK task up to wait for the link recovery
107 131 status = rtems_event_send ( Task_id[TASKID_LINK], RTEMS_EVENT_0 );
108 132 status = rtems_task_suspend( RTEMS_SELF );
109 133 }
110 134 }
111 135 }
112 136
113 137 rtems_task recv_task( rtems_task_argument unused )
114 138 {
115 139 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
116 140 *
117 141 * @param unused is the starting argument of the RTEMS task
118 142 *
119 143 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
120 144 * 1. It reads the incoming data.
121 145 * 2. Launches the acceptance procedure.
122 146 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
123 147 *
124 148 */
125 149
126 150 int len;
127 151 ccsdsTelecommandPacket_t __attribute__((aligned(4))) currentTC;
128 152 unsigned char computed_CRC[ BYTES_PER_CRC ];
129 153 unsigned char currentTC_LEN_RCV[ BYTES_PER_PKT_LEN ];
130 154 unsigned char destinationID;
131 155 unsigned int estimatedPacketLength;
132 156 unsigned int parserCode;
133 157 rtems_status_code status;
134 158 rtems_id queue_recv_id;
135 159 rtems_id queue_send_id;
136 160
137 161 memset( &currentTC, 0, sizeof(ccsdsTelecommandPacket_t) );
138 162 destinationID = 0;
139 163 queue_recv_id = RTEMS_ID_NONE;
140 164 queue_send_id = RTEMS_ID_NONE;
141 165
142 166 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
143 167
144 168 status = get_message_queue_id_recv( &queue_recv_id );
145 169 if (status != RTEMS_SUCCESSFUL)
146 170 {
147 171 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
148 172 }
149 173
150 174 status = get_message_queue_id_send( &queue_send_id );
151 175 if (status != RTEMS_SUCCESSFUL)
152 176 {
153 177 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
154 178 }
155 179
156 180 BOOT_PRINTF("in RECV *** \n")
157 181
158 182 while(1)
159 183 {
160 184 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
161 185 if (len == -1){ // error during the read call
162 186 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
163 187 }
164 188 else {
165 189 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
166 190 PRINTF("in RECV *** packet lenght too short\n")
167 191 }
168 192 else {
169 193 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - PROTID_RES_APP); // => -3 is for Prot ID, Reserved and User App bytes
170 194 PRINTF1("incoming TC with Length (byte): %d\n", len - 3);
171 195 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> SHIFT_1_BYTE);
172 196 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
173 197 // CHECK THE TC
174 198 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
175 199 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
176 200 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
177 201 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
178 202 || (parserCode == WRONG_SRC_ID) )
179 203 { // send TM_LFR_TC_EXE_CORRUPTED
180 204 PRINTF1("TC corrupted received, with code: %d\n", parserCode);
181 205 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
182 206 &&
183 207 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
184 208 )
185 209 {
186 210 if ( parserCode == WRONG_SRC_ID )
187 211 {
188 212 destinationID = SID_TC_GROUND;
189 213 }
190 214 else
191 215 {
192 216 destinationID = currentTC.sourceID;
193 217 }
194 218 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
195 219 computed_CRC, currentTC_LEN_RCV,
196 220 destinationID );
197 221 }
198 222 }
199 223 else
200 224 { // send valid TC to the action launcher
201 225 status = rtems_message_queue_send( queue_recv_id, &currentTC,
202 226 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + PROTID_RES_APP);
203 227 }
204 228 }
205 229 }
206 230
207 231 update_queue_max_count( queue_recv_id, &hk_lfr_q_rv_fifo_size_max );
208 232
209 233 }
210 234 }
211 235
212 236 rtems_task send_task( rtems_task_argument argument)
213 237 {
214 238 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
215 239 *
216 240 * @param unused is the starting argument of the RTEMS task
217 241 *
218 242 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
219 243 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
220 244 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
221 245 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
222 246 * data it contains.
223 247 *
224 248 */
225 249
226 250 rtems_status_code status; // RTEMS status code
227 251 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
228 252 ring_node *incomingRingNodePtr;
229 253 int ring_node_address;
230 254 char *charPtr;
231 255 spw_ioctl_pkt_send *spw_ioctl_send;
232 256 size_t size; // size of the incoming TC packet
233 257 rtems_id queue_send_id;
234 258 unsigned int sid;
235 259 unsigned char sidAsUnsignedChar;
236 260 unsigned char type;
237 261
238 262 incomingRingNodePtr = NULL;
239 263 ring_node_address = 0;
240 264 charPtr = (char *) &ring_node_address;
241 265 size = 0;
242 266 queue_send_id = RTEMS_ID_NONE;
243 267 sid = 0;
244 268 sidAsUnsignedChar = 0;
245 269
246 270 init_header_cwf( &headerCWF );
247 271 init_header_swf( &headerSWF );
248 272 init_header_asm( &headerASM );
249 273
250 274 status = get_message_queue_id_send( &queue_send_id );
251 275 if (status != RTEMS_SUCCESSFUL)
252 276 {
253 277 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
254 278 }
255 279
256 280 BOOT_PRINTF("in SEND *** \n")
257 281
258 282 while(1)
259 283 {
260 284 status = rtems_message_queue_receive( queue_send_id, incomingData, &size,
261 285 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
262 286
263 287 if (status!=RTEMS_SUCCESSFUL)
264 288 {
265 289 PRINTF1("in SEND *** (1) ERR = %d\n", status)
266 290 }
267 291 else
268 292 {
269 293 if ( size == sizeof(ring_node*) )
270 294 {
271 295 charPtr[0] = incomingData[0];
272 296 charPtr[1] = incomingData[1];
273 297 charPtr[BYTE_2] = incomingData[BYTE_2];
274 298 charPtr[BYTE_3] = incomingData[BYTE_3];
275 299 incomingRingNodePtr = (ring_node*) ring_node_address;
276 300 sid = incomingRingNodePtr->sid;
277 301 if ( (sid==SID_NORM_CWF_LONG_F3)
278 302 || (sid==SID_BURST_CWF_F2 )
279 303 || (sid==SID_SBM1_CWF_F1 )
280 304 || (sid==SID_SBM2_CWF_F2 ))
281 305 {
282 306 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
283 307 }
284 308 else if ( (sid==SID_NORM_SWF_F0) || (sid==SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
285 309 {
286 310 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
287 311 }
288 312 else if (sid==SID_NORM_CWF_F3)
289 313 {
290 314 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
291 315 }
292 316 else if (sid==SID_NORM_ASM_F0)
293 317 {
294 318 spw_send_asm_f0( incomingRingNodePtr, &headerASM );
295 319 }
296 320 else if (sid==SID_NORM_ASM_F1)
297 321 {
298 322 spw_send_asm_f1( incomingRingNodePtr, &headerASM );
299 323 }
300 324 else if (sid==SID_NORM_ASM_F2)
301 325 {
302 326 spw_send_asm_f2( incomingRingNodePtr, &headerASM );
303 327 }
304 328 else if (sid==TM_CODE_K_DUMP)
305 329 {
306 330 spw_send_k_dump( incomingRingNodePtr );
307 331 }
308 332 else
309 333 {
310 334 PRINTF1("unexpected sid = %d\n", sid);
311 335 }
312 336 }
313 337 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
314 338 {
315 339 sidAsUnsignedChar = (unsigned char) incomingData[ PACKET_POS_PA_LFR_SID_PKT ];
316 340 sid = sidAsUnsignedChar;
317 341 type = (unsigned char) incomingData[ PACKET_POS_SERVICE_TYPE ];
318 342 if (type == TM_TYPE_LFR_SCIENCE) // this is a BP packet, all other types are handled differently
319 343 // SET THE SEQUENCE_CNT PARAMETER IN CASE OF BP0 OR BP1 PACKETS
320 344 {
321 345 increment_seq_counter_source_id( (unsigned char*) &incomingData[ PACKET_POS_SEQUENCE_CNT ], sid );
322 346 }
323 347
324 348 status = write( fdSPW, incomingData, size );
325 349 if (status == -1){
326 350 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
327 351 }
328 352 }
329 353 else // the incoming message is a spw_ioctl_pkt_send structure
330 354 {
331 355 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
332 356 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
333 357 if (status == -1){
334 358 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
335 359 }
336 360 }
337 361 }
338 362
339 363 update_queue_max_count( queue_send_id, &hk_lfr_q_sd_fifo_size_max );
340 364
341 365 }
342 366 }
343 367
344 368 rtems_task link_task( rtems_task_argument argument )
345 369 {
346 370 rtems_event_set event_out;
347 371 rtems_status_code status;
348 372 int linkStatus;
349 373
350 374 event_out = EVENT_SETS_NONE_PENDING;
351 375 linkStatus = 0;
352 376
353 377 BOOT_PRINTF("in LINK ***\n")
354 378
355 379 while(1)
356 380 {
357 381 // wait for an RTEMS_EVENT
358 382 rtems_event_receive( RTEMS_EVENT_0,
359 383 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
360 384 PRINTF("in LINK *** wait for the link\n")
361 385 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
362 386 while( linkStatus != SPW_LINK_OK) // wait for the link
363 387 {
364 388 status = rtems_task_wake_after( SPW_LINK_WAIT ); // monitor the link each 100ms
365 389 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
366 390 watchdog_reload();
367 391 }
368 392
369 393 spacewire_read_statistics();
370 394 status = spacewire_stop_and_start_link( fdSPW );
371 395
372 396 if (status != RTEMS_SUCCESSFUL)
373 397 {
374 398 PRINTF1("in LINK *** ERR link not started %d\n", status)
375 399 }
376 400 else
377 401 {
378 402 PRINTF("in LINK *** OK link started\n")
379 403 }
380 404
381 405 // restart the SPIQ task
382 406 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
383 407 if ( status != RTEMS_SUCCESSFUL ) {
384 408 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
385 409 }
386 410
387 411 // restart RECV and SEND
388 412 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
389 413 if ( status != RTEMS_SUCCESSFUL ) {
390 414 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
391 415 }
392 416 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
393 417 if ( status != RTEMS_SUCCESSFUL ) {
394 418 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
395 419 }
396 420 }
397 421 }
398 422
399 423 //****************
400 424 // OTHER FUNCTIONS
401 425 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
402 426 {
403 427 /** This function opens the SpaceWire link.
404 428 *
405 429 * @return a valid file descriptor in case of success, -1 in case of a failure
406 430 *
407 431 */
408 432 rtems_status_code status;
409 433
410 434 status = RTEMS_SUCCESSFUL;
411 435
412 436 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
413 437 if ( fdSPW < 0 ) {
414 438 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
415 439 }
416 440 else
417 441 {
418 442 status = RTEMS_SUCCESSFUL;
419 443 }
420 444
421 445 return status;
422 446 }
423 447
424 448 int spacewire_start_link( int fd )
425 449 {
426 450 rtems_status_code status;
427 451
428 452 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
429 453 // -1 default hardcoded driver timeout
430 454
431 455 return status;
432 456 }
433 457
434 458 int spacewire_stop_and_start_link( int fd )
435 459 {
436 460 rtems_status_code status;
437 461
438 462 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
439 463 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
440 464 // -1 default hardcoded driver timeout
441 465
442 466 return status;
443 467 }
444 468
445 469 int spacewire_configure_link( int fd )
446 470 {
447 471 /** This function configures the SpaceWire link.
448 472 *
449 473 * @return GR-RTEMS-DRIVER directive status codes:
450 474 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
451 475 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
452 476 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
453 477 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
454 478 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
455 479 * - 5 EIO - Error when writing to grswp hardware registers.
456 480 * - 2 ENOENT - No such file or directory
457 481 */
458 482
459 483 rtems_status_code status;
460 484
461 485 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
462 486 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
463 487 spw_ioctl_packetsize packetsize;
464 488
465 489 packetsize.rxsize = SPW_RXSIZE;
466 490 packetsize.txdsize = SPW_TXDSIZE;
467 491 packetsize.txhsize = SPW_TXHSIZE;
468 492
469 493 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
470 494 if (status!=RTEMS_SUCCESSFUL) {
471 495 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
472 496 }
473 497 //
474 498 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
475 499 if (status!=RTEMS_SUCCESSFUL) {
476 500 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
477 501 }
478 502 //
479 503 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
480 504 if (status!=RTEMS_SUCCESSFUL) {
481 505 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
482 506 }
483 507 //
484 508 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
485 509 if (status!=RTEMS_SUCCESSFUL) {
486 510 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
487 511 }
488 512 //
489 513 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
490 514 if (status!=RTEMS_SUCCESSFUL) {
491 515 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
492 516 }
493 517 //
494 518 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
495 519 if (status!=RTEMS_SUCCESSFUL) {
496 520 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
497 521 }
498 522 //
499 523 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, CONF_TCODE_CTRL); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
500 524 if (status!=RTEMS_SUCCESSFUL) {
501 525 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
502 526 }
503 527 //
504 528 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_PACKETSIZE, packetsize); // set rxsize, txdsize and txhsize
505 529 if (status!=RTEMS_SUCCESSFUL) {
506 530 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_PACKETSIZE,\n")
507 531 }
508 532
509 533 return status;
510 534 }
511 535
512 536 int spacewire_several_connect_attemps( void )
513 537 {
514 538 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
515 539 *
516 540 * @return RTEMS directive status code:
517 541 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
518 542 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
519 543 *
520 544 */
521 545
522 546 rtems_status_code status_spw;
523 547 rtems_status_code status;
524 548 int i;
525 549
526 550 status_spw = RTEMS_SUCCESSFUL;
527 551
528 552 i = 0;
529 553 while (i < SY_LFR_DPU_CONNECT_ATTEMPT)
530 554 {
531 555 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
532 556
533 557 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
534 558
535 559 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
536 560
537 561 status_spw = spacewire_stop_and_start_link( fdSPW );
538 562
539 563 if ( status_spw != RTEMS_SUCCESSFUL )
540 564 {
541 565 i = i + 1;
542 566 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw);
543 567 }
544 568 else
545 569 {
546 570 i = SY_LFR_DPU_CONNECT_ATTEMPT;
547 571 }
548 572 }
549 573
550 574 return status_spw;
551 575 }
552 576
553 577 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
554 578 {
555 579 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
556 580 *
557 581 * @param val is the value, 0 or 1, used to set the value of the NP bit.
558 582 * @param regAddr is the address of the GRSPW control register.
559 583 *
560 584 * NP is the bit 20 of the GRSPW control register.
561 585 *
562 586 */
563 587
564 588 unsigned int *spwptr = (unsigned int*) regAddr;
565 589
566 590 if (val == 1) {
567 591 *spwptr = *spwptr | SPW_BIT_NP; // [NP] set the No port force bit
568 592 }
569 593 if (val== 0) {
570 594 *spwptr = *spwptr & SPW_BIT_NP_MASK;
571 595 }
572 596 }
573 597
574 598 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
575 599 {
576 600 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
577 601 *
578 602 * @param val is the value, 0 or 1, used to set the value of the RE bit.
579 603 * @param regAddr is the address of the GRSPW control register.
580 604 *
581 605 * RE is the bit 16 of the GRSPW control register.
582 606 *
583 607 */
584 608
585 609 unsigned int *spwptr = (unsigned int*) regAddr;
586 610
587 611 if (val == 1)
588 612 {
589 613 *spwptr = *spwptr | SPW_BIT_RE; // [RE] set the RMAP Enable bit
590 614 }
591 615 if (val== 0)
592 616 {
593 617 *spwptr = *spwptr & SPW_BIT_RE_MASK;
594 618 }
595 619 }
596 620
597 621 void spacewire_read_statistics( void )
598 622 {
599 623 /** This function reads the SpaceWire statistics from the grspw RTEMS driver.
600 624 *
601 625 * @param void
602 626 *
603 627 * @return void
604 628 *
605 629 * Once they are read, the counters are stored in a global variable used during the building of the
606 630 * HK packets.
607 631 *
608 632 */
609 633
610 634 rtems_status_code status;
611 635 spw_stats current;
612 636
613 637 memset(&current, 0, sizeof(spw_stats));
614 638
615 639 spacewire_get_last_error();
616 640
617 641 // read the current statistics
618 642 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &current );
619 643
620 644 // clear the counters
621 645 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_CLR_STATISTICS );
622 646
623 647 // rx_eep_err
624 648 grspw_stats.rx_eep_err = grspw_stats.rx_eep_err + current.rx_eep_err;
625 649 // rx_truncated
626 650 grspw_stats.rx_truncated = grspw_stats.rx_truncated + current.rx_truncated;
627 651 // parity_err
628 652 grspw_stats.parity_err = grspw_stats.parity_err + current.parity_err;
629 653 // escape_err
630 654 grspw_stats.escape_err = grspw_stats.escape_err + current.escape_err;
631 655 // credit_err
632 656 grspw_stats.credit_err = grspw_stats.credit_err + current.credit_err;
633 657 // write_sync_err
634 658 grspw_stats.write_sync_err = grspw_stats.write_sync_err + current.write_sync_err;
635 659 // disconnect_err
636 660 grspw_stats.disconnect_err = grspw_stats.disconnect_err + current.disconnect_err;
637 661 // early_ep
638 662 grspw_stats.early_ep = grspw_stats.early_ep + current.early_ep;
639 663 // invalid_address
640 664 grspw_stats.invalid_address = grspw_stats.invalid_address + current.invalid_address;
641 665 // packets_sent
642 666 grspw_stats.packets_sent = grspw_stats.packets_sent + current.packets_sent;
643 667 // packets_received
644 668 grspw_stats.packets_received= grspw_stats.packets_received + current.packets_received;
645 669
646 670 }
647 671
648 672 void spacewire_get_last_error( void )
649 673 {
650 674 static spw_stats previous = {0};
651 675 spw_stats current;
652 676 rtems_status_code status;
653 677
654 678 unsigned int hk_lfr_last_er_rid;
655 679 unsigned char hk_lfr_last_er_code;
656 680 int coarseTime;
657 681 int fineTime;
658 682 unsigned char update_hk_lfr_last_er;
659 683
660 684 memset(&current, 0, sizeof(spw_stats));
661 685 hk_lfr_last_er_rid = INIT_CHAR;
662 686 hk_lfr_last_er_code = INIT_CHAR;
663 687 update_hk_lfr_last_er = INIT_CHAR;
664 688
665 689 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &current );
666 690
667 691 // get current time
668 692 coarseTime = time_management_regs->coarse_time;
669 693 fineTime = time_management_regs->fine_time;
670 694
671 695 // tx_link_err *** no code associated to this field
672 696 // rx_rmap_header_crc_err *** LE *** in HK
673 697 if (previous.rx_rmap_header_crc_err != current.rx_rmap_header_crc_err)
674 698 {
675 699 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
676 700 hk_lfr_last_er_code = CODE_HEADER_CRC;
677 701 update_hk_lfr_last_er = 1;
678 702 }
679 703 // rx_rmap_data_crc_err *** LE *** NOT IN HK
680 704 if (previous.rx_rmap_data_crc_err != current.rx_rmap_data_crc_err)
681 705 {
682 706 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
683 707 hk_lfr_last_er_code = CODE_DATA_CRC;
684 708 update_hk_lfr_last_er = 1;
685 709 }
686 710 // rx_eep_err
687 711 if (previous.rx_eep_err != current.rx_eep_err)
688 712 {
689 713 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
690 714 hk_lfr_last_er_code = CODE_EEP;
691 715 update_hk_lfr_last_er = 1;
692 716 }
693 717 // rx_truncated
694 718 if (previous.rx_truncated != current.rx_truncated)
695 719 {
696 720 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
697 721 hk_lfr_last_er_code = CODE_RX_TOO_BIG;
698 722 update_hk_lfr_last_er = 1;
699 723 }
700 724 // parity_err
701 725 if (previous.parity_err != current.parity_err)
702 726 {
703 727 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
704 728 hk_lfr_last_er_code = CODE_PARITY;
705 729 update_hk_lfr_last_er = 1;
706 730 }
707 731 // escape_err
708 732 if (previous.parity_err != current.parity_err)
709 733 {
710 734 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
711 735 hk_lfr_last_er_code = CODE_ESCAPE;
712 736 update_hk_lfr_last_er = 1;
713 737 }
714 738 // credit_err
715 739 if (previous.credit_err != current.credit_err)
716 740 {
717 741 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
718 742 hk_lfr_last_er_code = CODE_CREDIT;
719 743 update_hk_lfr_last_er = 1;
720 744 }
721 745 // write_sync_err
722 746 if (previous.write_sync_err != current.write_sync_err)
723 747 {
724 748 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
725 749 hk_lfr_last_er_code = CODE_WRITE_SYNC;
726 750 update_hk_lfr_last_er = 1;
727 751 }
728 752 // disconnect_err
729 753 if (previous.disconnect_err != current.disconnect_err)
730 754 {
731 755 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
732 756 hk_lfr_last_er_code = CODE_DISCONNECT;
733 757 update_hk_lfr_last_er = 1;
734 758 }
735 759 // early_ep
736 760 if (previous.early_ep != current.early_ep)
737 761 {
738 762 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
739 763 hk_lfr_last_er_code = CODE_EARLY_EOP_EEP;
740 764 update_hk_lfr_last_er = 1;
741 765 }
742 766 // invalid_address
743 767 if (previous.invalid_address != current.invalid_address)
744 768 {
745 769 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
746 770 hk_lfr_last_er_code = CODE_INVALID_ADDRESS;
747 771 update_hk_lfr_last_er = 1;
748 772 }
749 773
750 774 // if a field has changed, update the hk_last_er fields
751 775 if (update_hk_lfr_last_er == 1)
752 776 {
753 777 update_hk_lfr_last_er_fields( hk_lfr_last_er_rid, hk_lfr_last_er_code );
754 778 }
755 779
756 780 previous = current;
757 781 }
758 782
759 783 void update_hk_lfr_last_er_fields(unsigned int rid, unsigned char code)
760 784 {
761 785 unsigned char *coarseTimePtr;
762 786 unsigned char *fineTimePtr;
763 787
764 788 coarseTimePtr = (unsigned char*) &time_management_regs->coarse_time;
765 789 fineTimePtr = (unsigned char*) &time_management_regs->fine_time;
766 790
767 791 housekeeping_packet.hk_lfr_last_er_rid[0] = (unsigned char) ((rid & BYTE0_MASK) >> SHIFT_1_BYTE );
768 792 housekeeping_packet.hk_lfr_last_er_rid[1] = (unsigned char) (rid & BYTE1_MASK);
769 793 housekeeping_packet.hk_lfr_last_er_code = code;
770 794 housekeeping_packet.hk_lfr_last_er_time[0] = coarseTimePtr[0];
771 795 housekeeping_packet.hk_lfr_last_er_time[1] = coarseTimePtr[1];
772 796 housekeeping_packet.hk_lfr_last_er_time[BYTE_2] = coarseTimePtr[BYTE_2];
773 797 housekeeping_packet.hk_lfr_last_er_time[BYTE_3] = coarseTimePtr[BYTE_3];
774 798 housekeeping_packet.hk_lfr_last_er_time[BYTE_4] = fineTimePtr[BYTE_2];
775 799 housekeeping_packet.hk_lfr_last_er_time[BYTE_5] = fineTimePtr[BYTE_3];
776 800 }
777 801
778 802 void update_hk_with_grspw_stats( void )
779 803 {
780 804 //****************************
781 805 // DPU_SPACEWIRE_IF_STATISTICS
782 806 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (grspw_stats.packets_received >> SHIFT_1_BYTE);
783 807 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (grspw_stats.packets_received);
784 808 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (grspw_stats.packets_sent >> SHIFT_1_BYTE);
785 809 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (grspw_stats.packets_sent);
786 810
787 811 //******************************************
788 812 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
789 813 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) grspw_stats.parity_err;
790 814 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) grspw_stats.disconnect_err;
791 815 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) grspw_stats.escape_err;
792 816 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) grspw_stats.credit_err;
793 817 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) grspw_stats.write_sync_err;
794 818
795 819 //*********************************************
796 820 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
797 821 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) grspw_stats.early_ep;
798 822 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) grspw_stats.invalid_address;
799 823 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) grspw_stats.rx_eep_err;
800 824 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) grspw_stats.rx_truncated;
801 825 }
802 826
803 827 void spacewire_update_hk_lfr_link_state( unsigned char *hk_lfr_status_word_0 )
804 828 {
805 829 unsigned int *statusRegisterPtr;
806 830 unsigned char linkState;
807 831
808 832 statusRegisterPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_STATUS_REGISTER);
809 833 linkState =
810 834 (unsigned char) ( ( (*statusRegisterPtr) >> SPW_LINK_STAT_POS) & STATUS_WORD_LINK_STATE_BITS); // [0000 0111]
811 835
812 836 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 & STATUS_WORD_LINK_STATE_MASK; // [1111 1000] set link state to 0
813 837
814 838 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 | linkState; // update hk_lfr_dpu_spw_link_state
815 839 }
816 840
817 841 void increase_unsigned_char_counter( unsigned char *counter )
818 842 {
819 843 // update the number of valid timecodes that have been received
820 844 if (*counter == UINT8_MAX)
821 845 {
822 846 *counter = 0;
823 847 }
824 848 else
825 849 {
826 850 *counter = *counter + 1;
827 851 }
828 852 }
829 853
830 854 unsigned int check_timecode_and_previous_timecode_coherency(unsigned char currentTimecodeCtr)
831 855 {
832 856 /** This function checks the coherency between the incoming timecode and the last valid timecode.
833 857 *
834 858 * @param currentTimecodeCtr is the incoming timecode
835 859 *
836 860 * @return returned codes::
837 861 * - LFR_DEFAULT
838 862 * - LFR_SUCCESSFUL
839 863 *
840 864 */
841 865
842 866 static unsigned char firstTickout = 1;
843 867 unsigned char ret;
844 868
845 869 ret = LFR_DEFAULT;
846 870
847 871 if (firstTickout == 0)
848 872 {
849 873 if (currentTimecodeCtr == 0)
850 874 {
851 875 if (previousTimecodeCtr == SPW_TIMECODE_MAX)
852 876 {
853 877 ret = LFR_SUCCESSFUL;
854 878 }
855 879 else
856 880 {
857 881 ret = LFR_DEFAULT;
858 882 }
859 883 }
860 884 else
861 885 {
862 886 if (currentTimecodeCtr == (previousTimecodeCtr +1))
863 887 {
864 888 ret = LFR_SUCCESSFUL;
865 889 }
866 890 else
867 891 {
868 892 ret = LFR_DEFAULT;
869 893 }
870 894 }
871 895 }
872 896 else
873 897 {
874 898 firstTickout = 0;
875 899 ret = LFR_SUCCESSFUL;
876 900 }
877 901
878 902 return ret;
879 903 }
880 904
881 905 unsigned int check_timecode_and_internal_time_coherency(unsigned char timecode, unsigned char internalTime)
882 906 {
883 907 unsigned int ret;
884 908
885 909 ret = LFR_DEFAULT;
886 910
887 911 if (timecode == internalTime)
888 912 {
889 913 ret = LFR_SUCCESSFUL;
890 914 }
891 915 else
892 916 {
893 917 ret = LFR_DEFAULT;
894 918 }
895 919
896 920 return ret;
897 921 }
898 922
899 923 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
900 924 {
901 925 // a tickout has been emitted, perform actions on the incoming timecode
902 926
903 927 unsigned char incomingTimecode;
904 928 unsigned char updateTime;
905 929 unsigned char internalTime;
906 930 rtems_status_code status;
907 931
908 932 incomingTimecode = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
909 933 updateTime = time_management_regs->coarse_time_load & TIMECODE_MASK;
910 934 internalTime = time_management_regs->coarse_time & TIMECODE_MASK;
911 935
912 936 housekeeping_packet.hk_lfr_dpu_spw_last_timc = incomingTimecode;
913 937
914 938 // update the number of tickout that have been generated
915 939 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt );
916 940
917 941 //**************************
918 942 // HK_LFR_TIMECODE_ERRONEOUS
919 943 // MISSING and INVALID are handled by the timecode_timer_routine service routine
920 944 if (check_timecode_and_previous_timecode_coherency( incomingTimecode ) == LFR_DEFAULT)
921 945 {
922 946 // this is unexpected but a tickout could have been raised despite of the timecode being erroneous
923 947 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_erroneous );
924 948 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_ERRONEOUS );
925 949 }
926 950
927 951 //************************
928 952 // HK_LFR_TIME_TIMECODE_IT
929 953 // check the coherency between the SpaceWire timecode and the Internal Time
930 954 if (check_timecode_and_internal_time_coherency( incomingTimecode, internalTime ) == LFR_DEFAULT)
931 955 {
932 956 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_it );
933 957 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_IT );
934 958 }
935 959
936 960 //********************
937 961 // HK_LFR_TIMECODE_CTR
938 962 // check the value of the timecode with respect to the last TC_LFR_UPDATE_TIME => SSS-CP-FS-370
939 963 if (oneTcLfrUpdateTimeReceived == 1)
940 964 {
941 965 if ( incomingTimecode != updateTime )
942 966 {
943 967 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_ctr );
944 968 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_CTR );
945 969 }
946 970 }
947 971
948 972 // launch the timecode timer to detect missing or invalid timecodes
949 973 previousTimecodeCtr = incomingTimecode; // update the previousTimecodeCtr value
950 974 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT, timecode_timer_routine, NULL );
951 975 if (status != RTEMS_SUCCESSFUL)
952 976 {
953 977 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_14 );
954 978 }
955 979 }
956 980
957 981 rtems_timer_service_routine timecode_timer_routine( rtems_id timer_id, void *user_data )
958 982 {
959 983 static unsigned char initStep = 1;
960 984
961 985 unsigned char currentTimecodeCtr;
962 986
963 987 currentTimecodeCtr = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
964 988
965 989 if (initStep == 1)
966 990 {
967 991 if (currentTimecodeCtr == previousTimecodeCtr)
968 992 {
969 993 //************************
970 994 // HK_LFR_TIMECODE_MISSING
971 995 // the timecode value has not changed, no valid timecode has been received, the timecode is MISSING
972 996 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
973 997 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING );
974 998 }
975 999 else if (currentTimecodeCtr == (previousTimecodeCtr+1))
976 1000 {
977 1001 // the timecode value has changed and the value is valid, this is unexpected because
978 1002 // the timer should not have fired, the timecode_irq_handler should have been raised
979 1003 }
980 1004 else
981 1005 {
982 1006 //************************
983 1007 // HK_LFR_TIMECODE_INVALID
984 1008 // the timecode value has changed and the value is not valid, no tickout has been generated
985 1009 // this is why the timer has fired
986 1010 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_invalid );
987 1011 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_INVALID );
988 1012 }
989 1013 }
990 1014 else
991 1015 {
992 1016 initStep = 1;
993 1017 //************************
994 1018 // HK_LFR_TIMECODE_MISSING
995 1019 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
996 1020 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING );
997 1021 }
998 1022
999 1023 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_13 );
1000 1024 }
1001 1025
1002 1026 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
1003 1027 {
1004 1028 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1005 1029 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1006 1030 header->reserved = DEFAULT_RESERVED;
1007 1031 header->userApplication = CCSDS_USER_APP;
1008 1032 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
1009 1033 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
1010 1034 header->packetLength[0] = INIT_CHAR;
1011 1035 header->packetLength[1] = INIT_CHAR;
1012 1036 // DATA FIELD HEADER
1013 1037 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1014 1038 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1015 1039 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
1016 1040 header->destinationID = TM_DESTINATION_ID_GROUND;
1017 1041 header->time[BYTE_0] = INIT_CHAR;
1018 1042 header->time[BYTE_1] = INIT_CHAR;
1019 1043 header->time[BYTE_2] = INIT_CHAR;
1020 1044 header->time[BYTE_3] = INIT_CHAR;
1021 1045 header->time[BYTE_4] = INIT_CHAR;
1022 1046 header->time[BYTE_5] = INIT_CHAR;
1023 1047 // AUXILIARY DATA HEADER
1024 1048 header->sid = INIT_CHAR;
1025 1049 header->pa_bia_status_info = DEFAULT_HKBIA;
1026 1050 header->blkNr[0] = INIT_CHAR;
1027 1051 header->blkNr[1] = INIT_CHAR;
1028 1052 }
1029 1053
1030 1054 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
1031 1055 {
1032 1056 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1033 1057 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1034 1058 header->reserved = DEFAULT_RESERVED;
1035 1059 header->userApplication = CCSDS_USER_APP;
1036 1060 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1037 1061 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1038 1062 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1039 1063 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1040 1064 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> SHIFT_1_BYTE);
1041 1065 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
1042 1066 // DATA FIELD HEADER
1043 1067 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1044 1068 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1045 1069 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
1046 1070 header->destinationID = TM_DESTINATION_ID_GROUND;
1047 1071 header->time[BYTE_0] = INIT_CHAR;
1048 1072 header->time[BYTE_1] = INIT_CHAR;
1049 1073 header->time[BYTE_2] = INIT_CHAR;
1050 1074 header->time[BYTE_3] = INIT_CHAR;
1051 1075 header->time[BYTE_4] = INIT_CHAR;
1052 1076 header->time[BYTE_5] = INIT_CHAR;
1053 1077 // AUXILIARY DATA HEADER
1054 1078 header->sid = INIT_CHAR;
1055 1079 header->pa_bia_status_info = DEFAULT_HKBIA;
1056 1080 header->pktCnt = PKTCNT_SWF; // PKT_CNT
1057 1081 header->pktNr = INIT_CHAR;
1058 1082 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> SHIFT_1_BYTE);
1059 1083 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
1060 1084 }
1061 1085
1062 1086 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
1063 1087 {
1064 1088 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1065 1089 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1066 1090 header->reserved = DEFAULT_RESERVED;
1067 1091 header->userApplication = CCSDS_USER_APP;
1068 1092 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1069 1093 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1070 1094 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1071 1095 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1072 1096 header->packetLength[0] = INIT_CHAR;
1073 1097 header->packetLength[1] = INIT_CHAR;
1074 1098 // DATA FIELD HEADER
1075 1099 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1076 1100 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1077 1101 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
1078 1102 header->destinationID = TM_DESTINATION_ID_GROUND;
1079 1103 header->time[BYTE_0] = INIT_CHAR;
1080 1104 header->time[BYTE_1] = INIT_CHAR;
1081 1105 header->time[BYTE_2] = INIT_CHAR;
1082 1106 header->time[BYTE_3] = INIT_CHAR;
1083 1107 header->time[BYTE_4] = INIT_CHAR;
1084 1108 header->time[BYTE_5] = INIT_CHAR;
1085 1109 // AUXILIARY DATA HEADER
1086 1110 header->sid = INIT_CHAR;
1087 1111 header->pa_bia_status_info = INIT_CHAR;
1088 1112 header->pa_lfr_pkt_cnt_asm = INIT_CHAR;
1089 1113 header->pa_lfr_pkt_nr_asm = INIT_CHAR;
1090 1114 header->pa_lfr_asm_blk_nr[0] = INIT_CHAR;
1091 1115 header->pa_lfr_asm_blk_nr[1] = INIT_CHAR;
1092 1116 }
1093 1117
1094 1118 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
1095 1119 Header_TM_LFR_SCIENCE_CWF_t *header )
1096 1120 {
1097 1121 /** This function sends CWF CCSDS packets (F2, F1 or F0).
1098 1122 *
1099 1123 * @param waveform points to the buffer containing the data that will be send.
1100 1124 * @param sid is the source identifier of the data that will be sent.
1101 1125 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1102 1126 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1103 1127 * contain information to setup the transmission of the data packets.
1104 1128 *
1105 1129 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1106 1130 *
1107 1131 */
1108 1132
1109 1133 unsigned int i;
1110 1134 int ret;
1111 1135 unsigned int coarseTime;
1112 1136 unsigned int fineTime;
1113 1137 rtems_status_code status;
1114 1138 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1115 1139 int *dataPtr;
1116 1140 unsigned char sid;
1117 1141
1118 1142 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1119 1143 spw_ioctl_send_CWF.options = 0;
1120 1144
1121 1145 ret = LFR_DEFAULT;
1122 1146 sid = (unsigned char) ring_node_to_send->sid;
1123 1147
1124 1148 coarseTime = ring_node_to_send->coarseTime;
1125 1149 fineTime = ring_node_to_send->fineTime;
1126 1150 dataPtr = (int*) ring_node_to_send->buffer_address;
1127 1151
1128 1152 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> SHIFT_1_BYTE);
1129 1153 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
1130 1154 header->pa_bia_status_info = pa_bia_status_info;
1131 1155 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1132 1156 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> SHIFT_1_BYTE);
1133 1157 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
1134 1158
1135 1159 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
1136 1160 {
1137 1161 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
1138 1162 spw_ioctl_send_CWF.hdr = (char*) header;
1139 1163 // BUILD THE DATA
1140 1164 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
1141 1165
1142 1166 // SET PACKET SEQUENCE CONTROL
1143 1167 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1144 1168
1145 1169 // SET SID
1146 1170 header->sid = sid;
1147 1171
1148 1172 // SET PACKET TIME
1149 1173 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
1150 1174 //
1151 1175 header->time[0] = header->acquisitionTime[0];
1152 1176 header->time[1] = header->acquisitionTime[1];
1153 1177 header->time[BYTE_2] = header->acquisitionTime[BYTE_2];
1154 1178 header->time[BYTE_3] = header->acquisitionTime[BYTE_3];
1155 1179 header->time[BYTE_4] = header->acquisitionTime[BYTE_4];
1156 1180 header->time[BYTE_5] = header->acquisitionTime[BYTE_5];
1157 1181
1158 1182 // SET PACKET ID
1159 1183 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
1160 1184 {
1161 1185 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> SHIFT_1_BYTE);
1162 1186 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
1163 1187 }
1164 1188 else
1165 1189 {
1166 1190 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1167 1191 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1168 1192 }
1169 1193
1170 1194 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1171 1195 if (status != RTEMS_SUCCESSFUL) {
1172 1196 ret = LFR_DEFAULT;
1173 1197 }
1174 1198 }
1175 1199
1176 1200 return ret;
1177 1201 }
1178 1202
1179 1203 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
1180 1204 Header_TM_LFR_SCIENCE_SWF_t *header )
1181 1205 {
1182 1206 /** This function sends SWF CCSDS packets (F2, F1 or F0).
1183 1207 *
1184 1208 * @param waveform points to the buffer containing the data that will be send.
1185 1209 * @param sid is the source identifier of the data that will be sent.
1186 1210 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
1187 1211 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1188 1212 * contain information to setup the transmission of the data packets.
1189 1213 *
1190 1214 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1191 1215 *
1192 1216 */
1193 1217
1194 1218 unsigned int i;
1195 1219 int ret;
1196 1220 unsigned int coarseTime;
1197 1221 unsigned int fineTime;
1198 1222 rtems_status_code status;
1199 1223 spw_ioctl_pkt_send spw_ioctl_send_SWF;
1200 1224 int *dataPtr;
1201 1225 unsigned char sid;
1202 1226
1203 1227 spw_ioctl_send_SWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_SWF;
1204 1228 spw_ioctl_send_SWF.options = 0;
1205 1229
1206 1230 ret = LFR_DEFAULT;
1207 1231
1208 1232 coarseTime = ring_node_to_send->coarseTime;
1209 1233 fineTime = ring_node_to_send->fineTime;
1210 1234 dataPtr = (int*) ring_node_to_send->buffer_address;
1211 1235 sid = ring_node_to_send->sid;
1212 1236
1213 1237 header->pa_bia_status_info = pa_bia_status_info;
1214 1238 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1215 1239
1216 1240 for (i=0; i<PKTCNT_SWF; i++) // send waveform
1217 1241 {
1218 1242 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
1219 1243 spw_ioctl_send_SWF.hdr = (char*) header;
1220 1244
1221 1245 // SET PACKET SEQUENCE CONTROL
1222 1246 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1223 1247
1224 1248 // SET PACKET LENGTH AND BLKNR
1225 1249 if (i == (PKTCNT_SWF-1))
1226 1250 {
1227 1251 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
1228 1252 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> SHIFT_1_BYTE);
1229 1253 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
1230 1254 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> SHIFT_1_BYTE);
1231 1255 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
1232 1256 }
1233 1257 else
1234 1258 {
1235 1259 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
1236 1260 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> SHIFT_1_BYTE);
1237 1261 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
1238 1262 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> SHIFT_1_BYTE);
1239 1263 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
1240 1264 }
1241 1265
1242 1266 // SET PACKET TIME
1243 1267 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
1244 1268 //
1245 1269 header->time[BYTE_0] = header->acquisitionTime[BYTE_0];
1246 1270 header->time[BYTE_1] = header->acquisitionTime[BYTE_1];
1247 1271 header->time[BYTE_2] = header->acquisitionTime[BYTE_2];
1248 1272 header->time[BYTE_3] = header->acquisitionTime[BYTE_3];
1249 1273 header->time[BYTE_4] = header->acquisitionTime[BYTE_4];
1250 1274 header->time[BYTE_5] = header->acquisitionTime[BYTE_5];
1251 1275
1252 1276 // SET SID
1253 1277 header->sid = sid;
1254 1278
1255 1279 // SET PKTNR
1256 1280 header->pktNr = i+1; // PKT_NR
1257 1281
1258 1282 // SEND PACKET
1259 1283 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
1260 1284 if (status != RTEMS_SUCCESSFUL) {
1261 1285 ret = LFR_DEFAULT;
1262 1286 }
1263 1287 }
1264 1288
1265 1289 return ret;
1266 1290 }
1267 1291
1268 1292 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
1269 1293 Header_TM_LFR_SCIENCE_CWF_t *header )
1270 1294 {
1271 1295 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
1272 1296 *
1273 1297 * @param waveform points to the buffer containing the data that will be send.
1274 1298 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1275 1299 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1276 1300 * contain information to setup the transmission of the data packets.
1277 1301 *
1278 1302 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
1279 1303 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
1280 1304 *
1281 1305 */
1282 1306
1283 1307 unsigned int i;
1284 1308 int ret;
1285 1309 unsigned int coarseTime;
1286 1310 unsigned int fineTime;
1287 1311 rtems_status_code status;
1288 1312 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1289 1313 char *dataPtr;
1290 1314 unsigned char sid;
1291 1315
1292 1316 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1293 1317 spw_ioctl_send_CWF.options = 0;
1294 1318
1295 1319 ret = LFR_DEFAULT;
1296 1320 sid = ring_node_to_send->sid;
1297 1321
1298 1322 coarseTime = ring_node_to_send->coarseTime;
1299 1323 fineTime = ring_node_to_send->fineTime;
1300 1324 dataPtr = (char*) ring_node_to_send->buffer_address;
1301 1325
1302 1326 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> SHIFT_1_BYTE);
1303 1327 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
1304 1328 header->pa_bia_status_info = pa_bia_status_info;
1305 1329 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1306 1330 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> SHIFT_1_BYTE);
1307 1331 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
1308 1332
1309 1333 //*********************
1310 1334 // SEND CWF3_light DATA
1311 1335 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
1312 1336 {
1313 1337 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
1314 1338 spw_ioctl_send_CWF.hdr = (char*) header;
1315 1339 // BUILD THE DATA
1316 1340 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
1317 1341
1318 1342 // SET PACKET SEQUENCE COUNTER
1319 1343 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1320 1344
1321 1345 // SET SID
1322 1346 header->sid = sid;
1323 1347
1324 1348 // SET PACKET TIME
1325 1349 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1326 1350 //
1327 1351 header->time[BYTE_0] = header->acquisitionTime[BYTE_0];
1328 1352 header->time[BYTE_1] = header->acquisitionTime[BYTE_1];
1329 1353 header->time[BYTE_2] = header->acquisitionTime[BYTE_2];
1330 1354 header->time[BYTE_3] = header->acquisitionTime[BYTE_3];
1331 1355 header->time[BYTE_4] = header->acquisitionTime[BYTE_4];
1332 1356 header->time[BYTE_5] = header->acquisitionTime[BYTE_5];
1333 1357
1334 1358 // SET PACKET ID
1335 1359 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1336 1360 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1337 1361
1338 1362 // SEND PACKET
1339 1363 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1340 1364 if (status != RTEMS_SUCCESSFUL) {
1341 1365 ret = LFR_DEFAULT;
1342 1366 }
1343 1367 }
1344 1368
1345 1369 return ret;
1346 1370 }
1347 1371
1348 1372 void spw_send_asm_f0( ring_node *ring_node_to_send,
1349 1373 Header_TM_LFR_SCIENCE_ASM_t *header )
1350 1374 {
1351 1375 unsigned int i;
1352 1376 unsigned int length = 0;
1353 1377 rtems_status_code status;
1354 1378 unsigned int sid;
1355 1379 float *spectral_matrix;
1356 1380 int coarseTime;
1357 1381 int fineTime;
1358 1382 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1359 1383
1360 1384 sid = ring_node_to_send->sid;
1361 1385 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1362 1386 coarseTime = ring_node_to_send->coarseTime;
1363 1387 fineTime = ring_node_to_send->fineTime;
1364 1388
1365 1389 header->pa_bia_status_info = pa_bia_status_info;
1366 1390 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1367 1391
1368 1392 for (i=0; i<PKTCNT_ASM; i++)
1369 1393 {
1370 1394 if ((i==0) || (i==1))
1371 1395 {
1372 1396 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_1;
1373 1397 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1374 1398 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1375 1399 ];
1376 1400 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_1;
1377 1401 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1378 1402 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> SHIFT_1_BYTE ); // BLK_NR MSB
1379 1403 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_1); // BLK_NR LSB
1380 1404 }
1381 1405 else
1382 1406 {
1383 1407 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_2;
1384 1408 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1385 1409 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1386 1410 ];
1387 1411 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_2;
1388 1412 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1389 1413 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> SHIFT_1_BYTE ); // BLK_NR MSB
1390 1414 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_2); // BLK_NR LSB
1391 1415 }
1392 1416
1393 1417 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1394 1418 spw_ioctl_send_ASM.hdr = (char *) header;
1395 1419 spw_ioctl_send_ASM.options = 0;
1396 1420
1397 1421 // (2) BUILD THE HEADER
1398 1422 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1399 1423 header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE);
1400 1424 header->packetLength[1] = (unsigned char) (length);
1401 1425 header->sid = (unsigned char) sid; // SID
1402 1426 header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM;
1403 1427 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1404 1428
1405 1429 // (3) SET PACKET TIME
1406 1430 header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES);
1407 1431 header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES);
1408 1432 header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE);
1409 1433 header->time[BYTE_3] = (unsigned char) (coarseTime);
1410 1434 header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE);
1411 1435 header->time[BYTE_5] = (unsigned char) (fineTime);
1412 1436 //
1413 1437 header->acquisitionTime[BYTE_0] = header->time[BYTE_0];
1414 1438 header->acquisitionTime[BYTE_1] = header->time[BYTE_1];
1415 1439 header->acquisitionTime[BYTE_2] = header->time[BYTE_2];
1416 1440 header->acquisitionTime[BYTE_3] = header->time[BYTE_3];
1417 1441 header->acquisitionTime[BYTE_4] = header->time[BYTE_4];
1418 1442 header->acquisitionTime[BYTE_5] = header->time[BYTE_5];
1419 1443
1420 1444 // (4) SEND PACKET
1421 1445 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1422 1446 if (status != RTEMS_SUCCESSFUL) {
1423 1447 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1424 1448 }
1425 1449 }
1426 1450 }
1427 1451
1428 1452 void spw_send_asm_f1( ring_node *ring_node_to_send,
1429 1453 Header_TM_LFR_SCIENCE_ASM_t *header )
1430 1454 {
1431 1455 unsigned int i;
1432 1456 unsigned int length = 0;
1433 1457 rtems_status_code status;
1434 1458 unsigned int sid;
1435 1459 float *spectral_matrix;
1436 1460 int coarseTime;
1437 1461 int fineTime;
1438 1462 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1439 1463
1440 1464 sid = ring_node_to_send->sid;
1441 1465 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1442 1466 coarseTime = ring_node_to_send->coarseTime;
1443 1467 fineTime = ring_node_to_send->fineTime;
1444 1468
1445 1469 header->pa_bia_status_info = pa_bia_status_info;
1446 1470 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1447 1471
1448 1472 for (i=0; i<PKTCNT_ASM; i++)
1449 1473 {
1450 1474 if ((i==0) || (i==1))
1451 1475 {
1452 1476 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_1;
1453 1477 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1454 1478 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1455 1479 ];
1456 1480 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_1;
1457 1481 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1458 1482 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> SHIFT_1_BYTE ); // BLK_NR MSB
1459 1483 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_1); // BLK_NR LSB
1460 1484 }
1461 1485 else
1462 1486 {
1463 1487 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_2;
1464 1488 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1465 1489 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1466 1490 ];
1467 1491 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_2;
1468 1492 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1469 1493 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> SHIFT_1_BYTE ); // BLK_NR MSB
1470 1494 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_2); // BLK_NR LSB
1471 1495 }
1472 1496
1473 1497 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1474 1498 spw_ioctl_send_ASM.hdr = (char *) header;
1475 1499 spw_ioctl_send_ASM.options = 0;
1476 1500
1477 1501 // (2) BUILD THE HEADER
1478 1502 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1479 1503 header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE);
1480 1504 header->packetLength[1] = (unsigned char) (length);
1481 1505 header->sid = (unsigned char) sid; // SID
1482 1506 header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM;
1483 1507 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1484 1508
1485 1509 // (3) SET PACKET TIME
1486 1510 header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES);
1487 1511 header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES);
1488 1512 header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE);
1489 1513 header->time[BYTE_3] = (unsigned char) (coarseTime);
1490 1514 header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE);
1491 1515 header->time[BYTE_5] = (unsigned char) (fineTime);
1492 1516 //
1493 1517 header->acquisitionTime[BYTE_0] = header->time[BYTE_0];
1494 1518 header->acquisitionTime[BYTE_1] = header->time[BYTE_1];
1495 1519 header->acquisitionTime[BYTE_2] = header->time[BYTE_2];
1496 1520 header->acquisitionTime[BYTE_3] = header->time[BYTE_3];
1497 1521 header->acquisitionTime[BYTE_4] = header->time[BYTE_4];
1498 1522 header->acquisitionTime[BYTE_5] = header->time[BYTE_5];
1499 1523
1500 1524 // (4) SEND PACKET
1501 1525 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1502 1526 if (status != RTEMS_SUCCESSFUL) {
1503 1527 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1504 1528 }
1505 1529 }
1506 1530 }
1507 1531
1508 1532 /**
1509 1533 * @brief spw_send_asm_f2 Sends an ASM packet at F2 over spacewire
1510 1534 * @param ring_node_to_send node pointing to the actual buffer to send
1511 1535 * @param header
1512 1536 */
1513 1537 void spw_send_asm_f2( ring_node *ring_node_to_send,
1514 1538 Header_TM_LFR_SCIENCE_ASM_t *header )
1515 1539 {
1516 1540 unsigned int i;
1517 1541 unsigned int length = 0;
1518 1542 rtems_status_code status;
1519 1543 unsigned int sid;
1520 1544 float *spectral_matrix;
1521 1545 int coarseTime;
1522 1546 int fineTime;
1523 1547 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1524 1548
1525 1549 sid = ring_node_to_send->sid;
1526 1550 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1527 1551 coarseTime = ring_node_to_send->coarseTime;
1528 1552 fineTime = ring_node_to_send->fineTime;
1529 1553
1530 1554 header->pa_bia_status_info = pa_bia_status_info;
1531 1555 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1532 1556
1533 1557 for (i=0; i<PKTCNT_ASM; i++)
1534 1558 {
1535 1559
1536 1560 spw_ioctl_send_ASM.dlen = DLEN_ASM_F2_PKT;
1537 1561 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1538 1562 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM )
1539 1563 ];
1540 1564 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1541 1565 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1542 1566 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> SHIFT_1_BYTE ); // BLK_NR MSB
1543 1567 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1544 1568
1545 1569 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1546 1570 spw_ioctl_send_ASM.hdr = (char *) header;
1547 1571 spw_ioctl_send_ASM.options = 0;
1548 1572
1549 1573 // (2) BUILD THE HEADER
1550 1574 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1551 1575 header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE);
1552 1576 header->packetLength[1] = (unsigned char) (length);
1553 1577 header->sid = (unsigned char) sid; // SID
1554 1578 header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM;
1555 1579 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1556 1580
1557 1581 // (3) SET PACKET TIME
1558 1582 header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES);
1559 1583 header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES);
1560 1584 header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE);
1561 1585 header->time[BYTE_3] = (unsigned char) (coarseTime);
1562 1586 header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE);
1563 1587 header->time[BYTE_5] = (unsigned char) (fineTime);
1564 1588 //
1565 1589 header->acquisitionTime[BYTE_0] = header->time[BYTE_0];
1566 1590 header->acquisitionTime[BYTE_1] = header->time[BYTE_1];
1567 1591 header->acquisitionTime[BYTE_2] = header->time[BYTE_2];
1568 1592 header->acquisitionTime[BYTE_3] = header->time[BYTE_3];
1569 1593 header->acquisitionTime[BYTE_4] = header->time[BYTE_4];
1570 1594 header->acquisitionTime[BYTE_5] = header->time[BYTE_5];
1571 1595
1572 1596 // (4) SEND PACKET
1573 1597 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1574 1598 if (status != RTEMS_SUCCESSFUL) {
1575 1599 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1576 1600 }
1577 1601 }
1578 1602 }
1579 1603
1580 1604 /**
1581 1605 * @brief spw_send_k_dump Sends k coefficients dump packet over spacewire
1582 1606 * @param ring_node_to_send node pointing to the actual buffer to send
1583 1607 */
1584 1608 void spw_send_k_dump( ring_node *ring_node_to_send )
1585 1609 {
1586 1610 rtems_status_code status;
1587 1611 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump;
1588 1612 unsigned int packetLength;
1589 1613 unsigned int size;
1590 1614
1591 1615 PRINTF("spw_send_k_dump\n")
1592 1616
1593 1617 kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address;
1594 1618
1595 1619 packetLength = (kcoefficients_dump->packetLength[0] * CONST_256) + kcoefficients_dump->packetLength[1];
1596 1620
1597 1621 size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
1598 1622
1599 1623 PRINTF2("packetLength %d, size %d\n", packetLength, size )
1600 1624
1601 1625 status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size );
1602 1626
1603 1627 if (status == -1){
1604 1628 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
1605 1629 }
1606 1630
1607 1631 ring_node_to_send->status = INIT_CHAR;
1608 1632 }
Show file before | Show file after | Hide comments
@@ -1,423 +1,447
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** Functions related to data processing.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 31 *
8 32 */
9 33
10 34 #include "avf0_prc0.h"
11 35
12 36 nb_sm_before_bp_asm_f0 nb_sm_before_f0 = {0};
13 37
14 38 //***
15 39 // F0
16 40 ring_node_asm asm_ring_norm_f0 [ NB_RING_NODES_ASM_NORM_F0 ] = {0};
17 41 ring_node_asm asm_ring_burst_sbm_f0 [ NB_RING_NODES_ASM_BURST_SBM_F0 ] = {0};
18 42
19 43 ring_node ring_to_send_asm_f0 [ NB_RING_NODES_ASM_F0 ] = {0};
20 44 int buffer_asm_f0 [ NB_RING_NODES_ASM_F0 * TOTAL_SIZE_SM ] = {0};
21 45
22 46 float asm_f0_patched_norm [ TOTAL_SIZE_SM ] = {0};
23 47 float asm_f0_patched_burst_sbm [ TOTAL_SIZE_SM ] = {0};
24 48 float asm_f0_reorganized [ TOTAL_SIZE_SM ] = {0};
25 49
26 50 float compressed_sm_norm_f0[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F0] = {0};
27 51 float compressed_sm_sbm_f0 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F0 ] = {0};
28 52
29 53 float k_coeff_intercalib_f0_norm[ NB_BINS_COMPRESSED_SM_F0 * NB_K_COEFF_PER_BIN ] = {0}; // 11 * 32 = 352
30 54 float k_coeff_intercalib_f0_sbm[ NB_BINS_COMPRESSED_SM_SBM_F0 * NB_K_COEFF_PER_BIN ] = {0}; // 22 * 32 = 704
31 55
32 56 //************
33 57 // RTEMS TASKS
34 58
35 59 rtems_task avf0_task( rtems_task_argument lfrRequestedMode )
36 60 {
37 61 int i;
38 62
39 63 rtems_event_set event_out;
40 64 rtems_status_code status;
41 65 rtems_id queue_id_prc0;
42 66 asm_msg msgForPRC;
43 67 ring_node *nodeForAveraging;
44 68 ring_node *ring_node_tab[NB_SM_BEFORE_AVF0_F1];
45 69 ring_node_asm *current_ring_node_asm_burst_sbm_f0;
46 70 ring_node_asm *current_ring_node_asm_norm_f0;
47 71
48 72 unsigned int nb_norm_bp1;
49 73 unsigned int nb_norm_bp2;
50 74 unsigned int nb_norm_asm;
51 75 unsigned int nb_sbm_bp1;
52 76 unsigned int nb_sbm_bp2;
53 77
54 78 nb_norm_bp1 = 0;
55 79 nb_norm_bp2 = 0;
56 80 nb_norm_asm = 0;
57 81 nb_sbm_bp1 = 0;
58 82 nb_sbm_bp2 = 0;
59 83 event_out = EVENT_SETS_NONE_PENDING;
60 84 queue_id_prc0 = RTEMS_ID_NONE;
61 85
62 86 reset_nb_sm_f0( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
63 87 ASM_generic_init_ring( asm_ring_norm_f0, NB_RING_NODES_ASM_NORM_F0 );
64 88 ASM_generic_init_ring( asm_ring_burst_sbm_f0, NB_RING_NODES_ASM_BURST_SBM_F0 );
65 89 current_ring_node_asm_norm_f0 = asm_ring_norm_f0;
66 90 current_ring_node_asm_burst_sbm_f0 = asm_ring_burst_sbm_f0;
67 91
68 92 BOOT_PRINTF1("in AVFO *** lfrRequestedMode = %d\n", (int) lfrRequestedMode);
69 93
70 94 status = get_message_queue_id_prc0( &queue_id_prc0 );
71 95 if (status != RTEMS_SUCCESSFUL)
72 96 {
73 97 PRINTF1("in MATR *** ERR get_message_queue_id_prc0 %d\n", status)
74 98 }
75 99
76 100 while(1){
77 101 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
78 102
79 103 //****************************************
80 104 // initialize the mesage for the MATR task
81 105 msgForPRC.norm = current_ring_node_asm_norm_f0;
82 106 msgForPRC.burst_sbm = current_ring_node_asm_burst_sbm_f0;
83 107 msgForPRC.event = EVENT_SETS_NONE_PENDING; // this composite event will be sent to the PRC0 task
84 108 //
85 109 //****************************************
86 110
87 111 nodeForAveraging = getRingNodeForAveraging( 0 );
88 112
89 113 ring_node_tab[NB_SM_BEFORE_AVF0_F1-1] = nodeForAveraging;
90 114 for ( i = 1; i < (NB_SM_BEFORE_AVF0_F1); i++ )
91 115 {
92 116 nodeForAveraging = nodeForAveraging->previous;
93 117 ring_node_tab[NB_SM_BEFORE_AVF0_F1 - i - 1] = nodeForAveraging;
94 118 }
95 119
96 120 // compute the average and store it in the averaged_sm_f1 buffer
97 121 SM_average( current_ring_node_asm_norm_f0->matrix,
98 122 current_ring_node_asm_burst_sbm_f0->matrix,
99 123 ring_node_tab,
100 124 nb_norm_bp1, nb_sbm_bp1,
101 125 &msgForPRC, 0 ); // 0 => frequency channel 0
102 126
103 127 // update nb_average
104 128 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0_F1;
105 129 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0_F1;
106 130 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0_F1;
107 131 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0_F1;
108 132 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0_F1;
109 133
110 134 if (nb_sbm_bp1 == nb_sm_before_f0.burst_sbm_bp1)
111 135 {
112 136 nb_sbm_bp1 = 0;
113 137 // set another ring for the ASM storage
114 138 current_ring_node_asm_burst_sbm_f0 = current_ring_node_asm_burst_sbm_f0->next;
115 139 if ( lfrCurrentMode == LFR_MODE_BURST )
116 140 {
117 141 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP1_F0;
118 142 }
119 143 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
120 144 {
121 145 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_SBM_BP1_F0;
122 146 }
123 147 }
124 148
125 149 if (nb_sbm_bp2 == nb_sm_before_f0.burst_sbm_bp2)
126 150 {
127 151 nb_sbm_bp2 = 0;
128 152 if ( lfrCurrentMode == LFR_MODE_BURST )
129 153 {
130 154 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP2_F0;
131 155 }
132 156 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
133 157 {
134 158 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_SBM_BP2_F0;
135 159 }
136 160 }
137 161
138 162 if (nb_norm_bp1 == nb_sm_before_f0.norm_bp1)
139 163 {
140 164 nb_norm_bp1 = 0;
141 165 // set another ring for the ASM storage
142 166 current_ring_node_asm_norm_f0 = current_ring_node_asm_norm_f0->next;
143 167 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
144 168 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
145 169 {
146 170 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP1_F0;
147 171 }
148 172 }
149 173
150 174 if (nb_norm_bp2 == nb_sm_before_f0.norm_bp2)
151 175 {
152 176 nb_norm_bp2 = 0;
153 177 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
154 178 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
155 179 {
156 180 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP2_F0;
157 181 }
158 182 }
159 183
160 184 if (nb_norm_asm == nb_sm_before_f0.norm_asm)
161 185 {
162 186 nb_norm_asm = 0;
163 187 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
164 188 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
165 189 {
166 190 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_ASM_F0;
167 191 }
168 192 }
169 193
170 194 //*************************
171 195 // send the message to PRC
172 196 if (msgForPRC.event != EVENT_SETS_NONE_PENDING)
173 197 {
174 198 status = rtems_message_queue_send( queue_id_prc0, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC0);
175 199 }
176 200
177 201 if (status != RTEMS_SUCCESSFUL) {
178 202 PRINTF1("in AVF0 *** Error sending message to PRC, code %d\n", status)
179 203 }
180 204 }
181 205 }
182 206
183 207 rtems_task prc0_task( rtems_task_argument lfrRequestedMode )
184 208 {
185 209 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
186 210 size_t size; // size of the incoming TC packet
187 211 asm_msg *incomingMsg;
188 212 //
189 213 unsigned char sid;
190 214 rtems_status_code status;
191 215 rtems_id queue_id;
192 216 rtems_id queue_id_q_p0;
193 217 bp_packet_with_spare __attribute__((aligned(4))) packet_norm_bp1;
194 218 bp_packet __attribute__((aligned(4))) packet_norm_bp2;
195 219 bp_packet __attribute__((aligned(4))) packet_sbm_bp1;
196 220 bp_packet __attribute__((aligned(4))) packet_sbm_bp2;
197 221 ring_node *current_ring_node_to_send_asm_f0;
198 222 float nbSMInASMNORM;
199 223 float nbSMInASMSBM;
200 224
201 225 size = 0;
202 226 queue_id = RTEMS_ID_NONE;
203 227 queue_id_q_p0 = RTEMS_ID_NONE;
204 228 memset( &packet_norm_bp1, 0, sizeof(bp_packet_with_spare) );
205 229 memset( &packet_norm_bp2, 0, sizeof(bp_packet) );
206 230 memset( &packet_sbm_bp1, 0, sizeof(bp_packet) );
207 231 memset( &packet_sbm_bp2, 0, sizeof(bp_packet) );
208 232
209 233 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
210 234 init_ring( ring_to_send_asm_f0, NB_RING_NODES_ASM_F0, (volatile int*) buffer_asm_f0, TOTAL_SIZE_SM );
211 235 current_ring_node_to_send_asm_f0 = ring_to_send_asm_f0;
212 236
213 237 //*************
214 238 // NORM headers
215 239 BP_init_header_with_spare( &packet_norm_bp1,
216 240 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F0,
217 241 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0, NB_BINS_COMPRESSED_SM_F0 );
218 242 BP_init_header( &packet_norm_bp2,
219 243 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F0,
220 244 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0, NB_BINS_COMPRESSED_SM_F0);
221 245
222 246 //****************************
223 247 // BURST SBM1 and SBM2 headers
224 248 if ( lfrRequestedMode == LFR_MODE_BURST )
225 249 {
226 250 BP_init_header( &packet_sbm_bp1,
227 251 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F0,
228 252 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
229 253 BP_init_header( &packet_sbm_bp2,
230 254 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F0,
231 255 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
232 256 }
233 257 else if ( lfrRequestedMode == LFR_MODE_SBM1 )
234 258 {
235 259 BP_init_header( &packet_sbm_bp1,
236 260 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP1_F0,
237 261 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
238 262 BP_init_header( &packet_sbm_bp2,
239 263 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP2_F0,
240 264 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
241 265 }
242 266 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
243 267 {
244 268 BP_init_header( &packet_sbm_bp1,
245 269 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F0,
246 270 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
247 271 BP_init_header( &packet_sbm_bp2,
248 272 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F0,
249 273 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
250 274 }
251 275 else
252 276 {
253 277 PRINTF1("in PRC0 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
254 278 }
255 279
256 280 status = get_message_queue_id_send( &queue_id );
257 281 if (status != RTEMS_SUCCESSFUL)
258 282 {
259 283 PRINTF1("in PRC0 *** ERR get_message_queue_id_send %d\n", status)
260 284 }
261 285 status = get_message_queue_id_prc0( &queue_id_q_p0);
262 286 if (status != RTEMS_SUCCESSFUL)
263 287 {
264 288 PRINTF1("in PRC0 *** ERR get_message_queue_id_prc0 %d\n", status)
265 289 }
266 290
267 291 BOOT_PRINTF1("in PRC0 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
268 292
269 293 while(1){
270 294 status = rtems_message_queue_receive( queue_id_q_p0, incomingData, &size, //************************************
271 295 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
272 296
273 297 incomingMsg = (asm_msg*) incomingData;
274 298
275 299 ASM_patch( incomingMsg->norm->matrix, asm_f0_patched_norm );
276 300 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f0_patched_burst_sbm );
277 301
278 302 nbSMInASMNORM = incomingMsg->numberOfSMInASMNORM;
279 303 nbSMInASMSBM = incomingMsg->numberOfSMInASMSBM;
280 304
281 305 //****************
282 306 //****************
283 307 // BURST SBM1 SBM2
284 308 //****************
285 309 //****************
286 310 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F0 ) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F0 ) )
287 311 {
288 312 sid = getSID( incomingMsg->event );
289 313 // 1) compress the matrix for Basic Parameters calculation
290 314 ASM_compress_reorganize_and_divide_mask( asm_f0_patched_burst_sbm, compressed_sm_sbm_f0,
291 315 nbSMInASMSBM,
292 316 NB_BINS_COMPRESSED_SM_SBM_F0, NB_BINS_TO_AVERAGE_ASM_SBM_F0,
293 317 ASM_F0_INDICE_START, CHANNELF0);
294 318 // 2) compute the BP1 set
295 319 BP1_set( compressed_sm_sbm_f0, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp1.data );
296 320 // 3) send the BP1 set
297 321 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
298 322 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
299 323 packet_sbm_bp1.pa_bia_status_info = pa_bia_status_info;
300 324 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
301 325 BP_send_s1_s2( (char *) &packet_sbm_bp1, queue_id,
302 326 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0 + PACKET_LENGTH_DELTA,
303 327 sid);
304 328 // 4) compute the BP2 set if needed
305 329 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F0) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F0) )
306 330 {
307 331 // 1) compute the BP2 set
308 332 BP2_set( compressed_sm_sbm_f0, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp2.data );
309 333 // 2) send the BP2 set
310 334 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
311 335 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
312 336 packet_sbm_bp2.pa_bia_status_info = pa_bia_status_info;
313 337 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
314 338 BP_send_s1_s2( (char *) &packet_sbm_bp2, queue_id,
315 339 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0 + PACKET_LENGTH_DELTA,
316 340 sid);
317 341 }
318 342 }
319 343
320 344 //*****
321 345 //*****
322 346 // NORM
323 347 //*****
324 348 //*****
325 349 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F0)
326 350 {
327 351 // 1) compress the matrix for Basic Parameters calculation
328 352 ASM_compress_reorganize_and_divide_mask( asm_f0_patched_norm, compressed_sm_norm_f0,
329 353 nbSMInASMNORM,
330 354 NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0,
331 355 ASM_F0_INDICE_START, CHANNELF0 );
332 356 // 2) compute the BP1 set
333 357 BP1_set( compressed_sm_norm_f0, k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp1.data );
334 358 // 3) send the BP1 set
335 359 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
336 360 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
337 361 packet_norm_bp1.pa_bia_status_info = pa_bia_status_info;
338 362 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
339 363 BP_send( (char *) &packet_norm_bp1, queue_id,
340 364 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0 + PACKET_LENGTH_DELTA,
341 365 SID_NORM_BP1_F0 );
342 366 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F0)
343 367 {
344 368 // 1) compute the BP2 set using the same ASM as the one used for BP1
345 369 BP2_set( compressed_sm_norm_f0, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp2.data );
346 370 // 2) send the BP2 set
347 371 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
348 372 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
349 373 packet_norm_bp2.pa_bia_status_info = pa_bia_status_info;
350 374 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
351 375 BP_send( (char *) &packet_norm_bp2, queue_id,
352 376 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0 + PACKET_LENGTH_DELTA,
353 377 SID_NORM_BP2_F0);
354 378 }
355 379 }
356 380
357 381 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F0)
358 382 {
359 383 // 1) reorganize the ASM and divide
360 384 ASM_reorganize_and_divide( asm_f0_patched_norm,
361 385 (float*) current_ring_node_to_send_asm_f0->buffer_address,
362 386 nbSMInASMNORM );
363 387 current_ring_node_to_send_asm_f0->coarseTime = incomingMsg->coarseTimeNORM;
364 388 current_ring_node_to_send_asm_f0->fineTime = incomingMsg->fineTimeNORM;
365 389 current_ring_node_to_send_asm_f0->sid = SID_NORM_ASM_F0;
366 390
367 391 // 3) send the spectral matrix packets
368 392 status = rtems_message_queue_send( queue_id, &current_ring_node_to_send_asm_f0, sizeof( ring_node* ) );
369 393
370 394 // change asm ring node
371 395 current_ring_node_to_send_asm_f0 = current_ring_node_to_send_asm_f0->next;
372 396 }
373 397
374 398 update_queue_max_count( queue_id_q_p0, &hk_lfr_q_p0_fifo_size_max );
375 399
376 400 }
377 401 }
378 402
379 403 //**********
380 404 // FUNCTIONS
381 405
382 406 void reset_nb_sm_f0( unsigned char lfrMode )
383 407 {
384 408 nb_sm_before_f0.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * NB_SM_PER_S_F0;
385 409 nb_sm_before_f0.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * NB_SM_PER_S_F0;
386 410 nb_sm_before_f0.norm_asm =
387 411 ( (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256) + parameter_dump_packet.sy_lfr_n_asm_p[1]) * NB_SM_PER_S_F0;
388 412 nb_sm_before_f0.sbm1_bp1 = parameter_dump_packet.sy_lfr_s1_bp_p0 * NB_SM_PER_S1_BP_P0; // 0.25 s per digit
389 413 nb_sm_before_f0.sbm1_bp2 = parameter_dump_packet.sy_lfr_s1_bp_p1 * NB_SM_PER_S_F0;
390 414 nb_sm_before_f0.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * NB_SM_PER_S_F0;
391 415 nb_sm_before_f0.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * NB_SM_PER_S_F0;
392 416 nb_sm_before_f0.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * NB_SM_PER_S_F0;
393 417 nb_sm_before_f0.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * NB_SM_PER_S_F0;
394 418
395 419 if (lfrMode == LFR_MODE_SBM1)
396 420 {
397 421 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm1_bp1;
398 422 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm1_bp2;
399 423 }
400 424 else if (lfrMode == LFR_MODE_SBM2)
401 425 {
402 426 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm2_bp1;
403 427 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm2_bp2;
404 428 }
405 429 else if (lfrMode == LFR_MODE_BURST)
406 430 {
407 431 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
408 432 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
409 433 }
410 434 else
411 435 {
412 436 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
413 437 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
414 438 }
415 439 }
416 440
417 441 void init_k_coefficients_prc0( void )
418 442 {
419 443 init_k_coefficients( k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0 );
420 444
421 445 init_kcoeff_sbm_from_kcoeff_norm( k_coeff_intercalib_f0_norm, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_F0);
422 446 }
423 447
Show file before | Show file after | Hide comments
@@ -1,407 +1,431
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** Functions related to data processing.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 31 *
8 32 */
9 33
10 34 #include "avf1_prc1.h"
11 35
12 36 nb_sm_before_bp_asm_f1 nb_sm_before_f1 = {0};
13 37
14 38 //***
15 39 // F1
16 40 ring_node_asm asm_ring_norm_f1 [ NB_RING_NODES_ASM_NORM_F1 ] = {0};
17 41 ring_node_asm asm_ring_burst_sbm_f1 [ NB_RING_NODES_ASM_BURST_SBM_F1 ] = {0};
18 42
19 43 ring_node ring_to_send_asm_f1 [ NB_RING_NODES_ASM_F1 ] = {0};
20 44 int buffer_asm_f1 [ NB_RING_NODES_ASM_F1 * TOTAL_SIZE_SM ] = {0};
21 45
22 46 float asm_f1_patched_norm [ TOTAL_SIZE_SM ] = {0};
23 47 float asm_f1_patched_burst_sbm [ TOTAL_SIZE_SM ] = {0};
24 48 float asm_f1_reorganized [ TOTAL_SIZE_SM ] = {0};
25 49
26 50 float compressed_sm_norm_f1[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F1] = {0};
27 51 float compressed_sm_sbm_f1 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 ] = {0};
28 52
29 53 float k_coeff_intercalib_f1_norm[ NB_BINS_COMPRESSED_SM_F1 * NB_K_COEFF_PER_BIN ] = {0}; // 13 * 32 = 416
30 54 float k_coeff_intercalib_f1_sbm[ NB_BINS_COMPRESSED_SM_SBM_F1 * NB_K_COEFF_PER_BIN ] = {0}; // 26 * 32 = 832
31 55
32 56 //************
33 57 // RTEMS TASKS
34 58
35 59 rtems_task avf1_task( rtems_task_argument lfrRequestedMode )
36 60 {
37 61 int i;
38 62
39 63 rtems_event_set event_out;
40 64 rtems_status_code status;
41 65 rtems_id queue_id_prc1;
42 66 asm_msg msgForPRC;
43 67 ring_node *nodeForAveraging;
44 68 ring_node *ring_node_tab[NB_SM_BEFORE_AVF0_F1];
45 69 ring_node_asm *current_ring_node_asm_burst_sbm_f1;
46 70 ring_node_asm *current_ring_node_asm_norm_f1;
47 71
48 72 unsigned int nb_norm_bp1;
49 73 unsigned int nb_norm_bp2;
50 74 unsigned int nb_norm_asm;
51 75 unsigned int nb_sbm_bp1;
52 76 unsigned int nb_sbm_bp2;
53 77
54 78 event_out = EVENT_SETS_NONE_PENDING;
55 79 queue_id_prc1 = RTEMS_ID_NONE;
56 80
57 81 nb_norm_bp1 = 0;
58 82 nb_norm_bp2 = 0;
59 83 nb_norm_asm = 0;
60 84 nb_sbm_bp1 = 0;
61 85 nb_sbm_bp2 = 0;
62 86
63 87 reset_nb_sm_f1( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
64 88 ASM_generic_init_ring( asm_ring_norm_f1, NB_RING_NODES_ASM_NORM_F1 );
65 89 ASM_generic_init_ring( asm_ring_burst_sbm_f1, NB_RING_NODES_ASM_BURST_SBM_F1 );
66 90 current_ring_node_asm_norm_f1 = asm_ring_norm_f1;
67 91 current_ring_node_asm_burst_sbm_f1 = asm_ring_burst_sbm_f1;
68 92
69 93 BOOT_PRINTF1("in AVF1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
70 94
71 95 status = get_message_queue_id_prc1( &queue_id_prc1 );
72 96 if (status != RTEMS_SUCCESSFUL)
73 97 {
74 98 PRINTF1("in AVF1 *** ERR get_message_queue_id_prc1 %d\n", status)
75 99 }
76 100
77 101 while(1){
78 102 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
79 103
80 104 //****************************************
81 105 // initialize the mesage for the MATR task
82 106 msgForPRC.norm = current_ring_node_asm_norm_f1;
83 107 msgForPRC.burst_sbm = current_ring_node_asm_burst_sbm_f1;
84 108 msgForPRC.event = EVENT_SETS_NONE_PENDING; // this composite event will be sent to the PRC1 task
85 109 //
86 110 //****************************************
87 111
88 112 nodeForAveraging = getRingNodeForAveraging( 1 );
89 113
90 114 ring_node_tab[NB_SM_BEFORE_AVF0_F1-1] = nodeForAveraging;
91 115 for ( i = 1; i < (NB_SM_BEFORE_AVF0_F1); i++ )
92 116 {
93 117 nodeForAveraging = nodeForAveraging->previous;
94 118 ring_node_tab[NB_SM_BEFORE_AVF0_F1 - i - 1] = nodeForAveraging;
95 119 }
96 120
97 121 // compute the average and store it in the averaged_sm_f1 buffer
98 122 SM_average( current_ring_node_asm_norm_f1->matrix,
99 123 current_ring_node_asm_burst_sbm_f1->matrix,
100 124 ring_node_tab,
101 125 nb_norm_bp1, nb_sbm_bp1,
102 126 &msgForPRC, 1 ); // 1 => frequency channel 1
103 127
104 128 // update nb_average
105 129 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0_F1;
106 130 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0_F1;
107 131 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0_F1;
108 132 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0_F1;
109 133 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0_F1;
110 134
111 135 if (nb_sbm_bp1 == nb_sm_before_f1.burst_sbm_bp1)
112 136 {
113 137 nb_sbm_bp1 = 0;
114 138 // set another ring for the ASM storage
115 139 current_ring_node_asm_burst_sbm_f1 = current_ring_node_asm_burst_sbm_f1->next;
116 140 if ( lfrCurrentMode == LFR_MODE_BURST )
117 141 {
118 142 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP1_F1;
119 143 }
120 144 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
121 145 {
122 146 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_SBM_BP1_F1;
123 147 }
124 148 }
125 149
126 150 if (nb_sbm_bp2 == nb_sm_before_f1.burst_sbm_bp2)
127 151 {
128 152 nb_sbm_bp2 = 0;
129 153 if ( lfrCurrentMode == LFR_MODE_BURST )
130 154 {
131 155 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP2_F1;
132 156 }
133 157 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
134 158 {
135 159 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_SBM_BP2_F1;
136 160 }
137 161 }
138 162
139 163 if (nb_norm_bp1 == nb_sm_before_f1.norm_bp1)
140 164 {
141 165 nb_norm_bp1 = 0;
142 166 // set another ring for the ASM storage
143 167 current_ring_node_asm_norm_f1 = current_ring_node_asm_norm_f1->next;
144 168 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
145 169 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
146 170 {
147 171 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP1_F1;
148 172 }
149 173 }
150 174
151 175 if (nb_norm_bp2 == nb_sm_before_f1.norm_bp2)
152 176 {
153 177 nb_norm_bp2 = 0;
154 178 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
155 179 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
156 180 {
157 181 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP2_F1;
158 182 }
159 183 }
160 184
161 185 if (nb_norm_asm == nb_sm_before_f1.norm_asm)
162 186 {
163 187 nb_norm_asm = 0;
164 188 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
165 189 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
166 190 {
167 191 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_ASM_F1;
168 192 }
169 193 }
170 194
171 195 //*************************
172 196 // send the message to PRC
173 197 if (msgForPRC.event != EVENT_SETS_NONE_PENDING)
174 198 {
175 199 status = rtems_message_queue_send( queue_id_prc1, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC1);
176 200 }
177 201
178 202 if (status != RTEMS_SUCCESSFUL) {
179 203 PRINTF1("in AVF1 *** Error sending message to PRC1, code %d\n", status)
180 204 }
181 205 }
182 206 }
183 207
184 208 rtems_task prc1_task( rtems_task_argument lfrRequestedMode )
185 209 {
186 210 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
187 211 size_t size; // size of the incoming TC packet
188 212 asm_msg *incomingMsg;
189 213 //
190 214 unsigned char sid;
191 215 rtems_status_code status;
192 216 rtems_id queue_id_send;
193 217 rtems_id queue_id_q_p1;
194 218 bp_packet_with_spare __attribute__((aligned(4))) packet_norm_bp1;
195 219 bp_packet __attribute__((aligned(4))) packet_norm_bp2;
196 220 bp_packet __attribute__((aligned(4))) packet_sbm_bp1;
197 221 bp_packet __attribute__((aligned(4))) packet_sbm_bp2;
198 222 ring_node *current_ring_node_to_send_asm_f1;
199 223 float nbSMInASMNORM;
200 224 float nbSMInASMSBM;
201 225
202 226 size = 0;
203 227 queue_id_send = RTEMS_ID_NONE;
204 228 queue_id_q_p1 = RTEMS_ID_NONE;
205 229 memset( &packet_norm_bp1, 0, sizeof(bp_packet_with_spare) );
206 230 memset( &packet_norm_bp2, 0, sizeof(bp_packet) );
207 231 memset( &packet_sbm_bp1, 0, sizeof(bp_packet) );
208 232 memset( &packet_sbm_bp2, 0, sizeof(bp_packet) );
209 233
210 234 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
211 235 init_ring( ring_to_send_asm_f1, NB_RING_NODES_ASM_F1, (volatile int*) buffer_asm_f1, TOTAL_SIZE_SM );
212 236 current_ring_node_to_send_asm_f1 = ring_to_send_asm_f1;
213 237
214 238 //*************
215 239 // NORM headers
216 240 BP_init_header_with_spare( &packet_norm_bp1,
217 241 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F1,
218 242 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1, NB_BINS_COMPRESSED_SM_F1 );
219 243 BP_init_header( &packet_norm_bp2,
220 244 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F1,
221 245 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1, NB_BINS_COMPRESSED_SM_F1);
222 246
223 247 //***********************
224 248 // BURST and SBM2 headers
225 249 if ( lfrRequestedMode == LFR_MODE_BURST )
226 250 {
227 251 BP_init_header( &packet_sbm_bp1,
228 252 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F1,
229 253 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
230 254 BP_init_header( &packet_sbm_bp2,
231 255 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F1,
232 256 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
233 257 }
234 258 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
235 259 {
236 260 BP_init_header( &packet_sbm_bp1,
237 261 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F1,
238 262 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
239 263 BP_init_header( &packet_sbm_bp2,
240 264 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F1,
241 265 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
242 266 }
243 267 else
244 268 {
245 269 PRINTF1("in PRC1 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
246 270 }
247 271
248 272 status = get_message_queue_id_send( &queue_id_send );
249 273 if (status != RTEMS_SUCCESSFUL)
250 274 {
251 275 PRINTF1("in PRC1 *** ERR get_message_queue_id_send %d\n", status)
252 276 }
253 277 status = get_message_queue_id_prc1( &queue_id_q_p1);
254 278 if (status != RTEMS_SUCCESSFUL)
255 279 {
256 280 PRINTF1("in PRC1 *** ERR get_message_queue_id_prc1 %d\n", status)
257 281 }
258 282
259 283 BOOT_PRINTF1("in PRC1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
260 284
261 285 while(1){
262 286 status = rtems_message_queue_receive( queue_id_q_p1, incomingData, &size, //************************************
263 287 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
264 288
265 289 incomingMsg = (asm_msg*) incomingData;
266 290
267 291 ASM_patch( incomingMsg->norm->matrix, asm_f1_patched_norm );
268 292 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f1_patched_burst_sbm );
269 293
270 294 nbSMInASMNORM = incomingMsg->numberOfSMInASMNORM;
271 295 nbSMInASMSBM = incomingMsg->numberOfSMInASMSBM;
272 296
273 297 //***********
274 298 //***********
275 299 // BURST SBM2
276 300 //***********
277 301 //***********
278 302 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F1) )
279 303 {
280 304 sid = getSID( incomingMsg->event );
281 305 // 1) compress the matrix for Basic Parameters calculation
282 306 ASM_compress_reorganize_and_divide_mask( asm_f1_patched_burst_sbm, compressed_sm_sbm_f1,
283 307 nbSMInASMSBM,
284 308 NB_BINS_COMPRESSED_SM_SBM_F1, NB_BINS_TO_AVERAGE_ASM_SBM_F1,
285 309 ASM_F1_INDICE_START, CHANNELF1);
286 310 // 2) compute the BP1 set
287 311 BP1_set( compressed_sm_sbm_f1, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp1.data );
288 312 // 3) send the BP1 set
289 313 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
290 314 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
291 315 packet_sbm_bp1.pa_bia_status_info = pa_bia_status_info;
292 316 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
293 317 BP_send_s1_s2( (char *) &packet_sbm_bp1, queue_id_send,
294 318 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 + PACKET_LENGTH_DELTA,
295 319 sid );
296 320 // 4) compute the BP2 set if needed
297 321 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F1) )
298 322 {
299 323 // 1) compute the BP2 set
300 324 BP2_set( compressed_sm_sbm_f1, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp2.data );
301 325 // 2) send the BP2 set
302 326 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
303 327 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
304 328 packet_sbm_bp2.pa_bia_status_info = pa_bia_status_info;
305 329 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
306 330 BP_send_s1_s2( (char *) &packet_sbm_bp2, queue_id_send,
307 331 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1 + PACKET_LENGTH_DELTA,
308 332 sid );
309 333 }
310 334 }
311 335
312 336 //*****
313 337 //*****
314 338 // NORM
315 339 //*****
316 340 //*****
317 341 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F1)
318 342 {
319 343 // 1) compress the matrix for Basic Parameters calculation
320 344 ASM_compress_reorganize_and_divide_mask( asm_f1_patched_norm, compressed_sm_norm_f1,
321 345 nbSMInASMNORM,
322 346 NB_BINS_COMPRESSED_SM_F1, NB_BINS_TO_AVERAGE_ASM_F1,
323 347 ASM_F1_INDICE_START, CHANNELF1 );
324 348 // 2) compute the BP1 set
325 349 BP1_set( compressed_sm_norm_f1, k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp1.data );
326 350 // 3) send the BP1 set
327 351 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
328 352 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
329 353 packet_norm_bp1.pa_bia_status_info = pa_bia_status_info;
330 354 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
331 355 BP_send( (char *) &packet_norm_bp1, queue_id_send,
332 356 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 + PACKET_LENGTH_DELTA,
333 357 SID_NORM_BP1_F1 );
334 358 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F1)
335 359 {
336 360 // 1) compute the BP2 set
337 361 BP2_set( compressed_sm_norm_f1, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp2.data );
338 362 // 2) send the BP2 set
339 363 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
340 364 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
341 365 packet_norm_bp2.pa_bia_status_info = pa_bia_status_info;
342 366 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
343 367 BP_send( (char *) &packet_norm_bp2, queue_id_send,
344 368 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 + PACKET_LENGTH_DELTA,
345 369 SID_NORM_BP2_F1 );
346 370 }
347 371 }
348 372
349 373 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F1)
350 374 {
351 375 // 1) reorganize the ASM and divide
352 376 ASM_reorganize_and_divide( asm_f1_patched_norm,
353 377 (float*) current_ring_node_to_send_asm_f1->buffer_address,
354 378 nbSMInASMNORM );
355 379 current_ring_node_to_send_asm_f1->coarseTime = incomingMsg->coarseTimeNORM;
356 380 current_ring_node_to_send_asm_f1->fineTime = incomingMsg->fineTimeNORM;
357 381 current_ring_node_to_send_asm_f1->sid = SID_NORM_ASM_F1;
358 382
359 383 // 3) send the spectral matrix packets
360 384 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f1, sizeof( ring_node* ) );
361 385
362 386 // change asm ring node
363 387 current_ring_node_to_send_asm_f1 = current_ring_node_to_send_asm_f1->next;
364 388 }
365 389
366 390 update_queue_max_count( queue_id_q_p1, &hk_lfr_q_p1_fifo_size_max );
367 391
368 392 }
369 393 }
370 394
371 395 //**********
372 396 // FUNCTIONS
373 397
374 398 void reset_nb_sm_f1( unsigned char lfrMode )
375 399 {
376 400 nb_sm_before_f1.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * NB_SM_PER_S_F1;
377 401 nb_sm_before_f1.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * NB_SM_PER_S_F1;
378 402 nb_sm_before_f1.norm_asm =
379 403 ( (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256) + parameter_dump_packet.sy_lfr_n_asm_p[1]) * NB_SM_PER_S_F1;
380 404 nb_sm_before_f1.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * NB_SM_PER_S_F1;
381 405 nb_sm_before_f1.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * NB_SM_PER_S_F1;
382 406 nb_sm_before_f1.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * NB_SM_PER_S_F1;
383 407 nb_sm_before_f1.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * NB_SM_PER_S_F1;
384 408
385 409 if (lfrMode == LFR_MODE_SBM2)
386 410 {
387 411 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.sbm2_bp1;
388 412 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.sbm2_bp2;
389 413 }
390 414 else if (lfrMode == LFR_MODE_BURST)
391 415 {
392 416 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
393 417 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
394 418 }
395 419 else
396 420 {
397 421 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
398 422 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
399 423 }
400 424 }
401 425
402 426 void init_k_coefficients_prc1( void )
403 427 {
404 428 init_k_coefficients( k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1 );
405 429
406 430 init_kcoeff_sbm_from_kcoeff_norm( k_coeff_intercalib_f1_norm, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_F1);
407 431 }
Show file before | Show file after | Hide comments
@@ -1,332 +1,356
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** Functions related to data processing.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 31 *
8 32 */
9 33
10 34 #include "avf2_prc2.h"
11 35
12 36 nb_sm_before_bp_asm_f2 nb_sm_before_f2 = {0};
13 37
14 38 //***
15 39 // F2
16 40 ring_node_asm asm_ring_norm_f2 [ NB_RING_NODES_ASM_NORM_F2 ] = {0};
17 41
18 42 ring_node ring_to_send_asm_f2 [ NB_RING_NODES_ASM_F2 ] = {0};
19 43 int buffer_asm_f2 [ NB_RING_NODES_ASM_F2 * TOTAL_SIZE_SM ] = {0};
20 44
21 45 float asm_f2_patched_norm [ TOTAL_SIZE_SM ] = {0};
22 46 float asm_f2_reorganized [ TOTAL_SIZE_SM ] = {0};
23 47
24 48 float compressed_sm_norm_f2[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F2] = {0};
25 49
26 50 float k_coeff_intercalib_f2[ NB_BINS_COMPRESSED_SM_F2 * NB_K_COEFF_PER_BIN ] = {0}; // 12 * 32 = 384
27 51
28 52 //************
29 53 // RTEMS TASKS
30 54
31 55 //***
32 56 // F2
33 57 rtems_task avf2_task( rtems_task_argument argument )
34 58 {
35 59 rtems_event_set event_out;
36 60 rtems_status_code status;
37 61 rtems_id queue_id_prc2;
38 62 asm_msg msgForPRC;
39 63 ring_node *nodeForAveraging;
40 64 ring_node_asm *current_ring_node_asm_norm_f2;
41 65
42 66 unsigned int nb_norm_bp1;
43 67 unsigned int nb_norm_bp2;
44 68 unsigned int nb_norm_asm;
45 69
46 70 event_out = EVENT_SETS_NONE_PENDING;
47 71 queue_id_prc2 = RTEMS_ID_NONE;
48 72 nb_norm_bp1 = 0;
49 73 nb_norm_bp2 = 0;
50 74 nb_norm_asm = 0;
51 75
52 76 reset_nb_sm_f2( ); // reset the sm counters that drive the BP and ASM computations / transmissions
53 77 ASM_generic_init_ring( asm_ring_norm_f2, NB_RING_NODES_ASM_NORM_F2 );
54 78 current_ring_node_asm_norm_f2 = asm_ring_norm_f2;
55 79
56 80 BOOT_PRINTF("in AVF2 ***\n")
57 81
58 82 status = get_message_queue_id_prc2( &queue_id_prc2 );
59 83 if (status != RTEMS_SUCCESSFUL)
60 84 {
61 85 PRINTF1("in AVF2 *** ERR get_message_queue_id_prc2 %d\n", status)
62 86 }
63 87
64 88 while(1){
65 89 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
66 90
67 91 //****************************************
68 92 // initialize the mesage for the MATR task
69 93 msgForPRC.norm = current_ring_node_asm_norm_f2;
70 94 msgForPRC.burst_sbm = NULL;
71 95 msgForPRC.event = EVENT_SETS_NONE_PENDING; // this composite event will be sent to the PRC2 task
72 96 //
73 97 //****************************************
74 98
75 99 nodeForAveraging = getRingNodeForAveraging( CHANNELF2 );
76 100
77 101 // compute the average and store it in the averaged_sm_f2 buffer
78 102 SM_average_f2( current_ring_node_asm_norm_f2->matrix,
79 103 nodeForAveraging,
80 104 nb_norm_bp1,
81 105 &msgForPRC );
82 106
83 107 // update nb_average
84 108 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF2;
85 109 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF2;
86 110 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF2;
87 111
88 112 if (nb_norm_bp1 == nb_sm_before_f2.norm_bp1)
89 113 {
90 114 nb_norm_bp1 = 0;
91 115 // set another ring for the ASM storage
92 116 current_ring_node_asm_norm_f2 = current_ring_node_asm_norm_f2->next;
93 117 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
94 118 || (lfrCurrentMode == LFR_MODE_SBM2) )
95 119 {
96 120 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP1_F2;
97 121 }
98 122 }
99 123
100 124 if (nb_norm_bp2 == nb_sm_before_f2.norm_bp2)
101 125 {
102 126 nb_norm_bp2 = 0;
103 127 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
104 128 || (lfrCurrentMode == LFR_MODE_SBM2) )
105 129 {
106 130 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP2_F2;
107 131 }
108 132 }
109 133
110 134 if (nb_norm_asm == nb_sm_before_f2.norm_asm)
111 135 {
112 136 nb_norm_asm = 0;
113 137 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
114 138 || (lfrCurrentMode == LFR_MODE_SBM2) )
115 139 {
116 140 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_ASM_F2;
117 141 }
118 142 }
119 143
120 144 //*************************
121 145 // send the message to PRC2
122 146 if (msgForPRC.event != EVENT_SETS_NONE_PENDING)
123 147 {
124 148 status = rtems_message_queue_send( queue_id_prc2, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC2);
125 149 }
126 150
127 151 if (status != RTEMS_SUCCESSFUL) {
128 152 PRINTF1("in AVF2 *** Error sending message to PRC2, code %d\n", status)
129 153 }
130 154 }
131 155 }
132 156
133 157 rtems_task prc2_task( rtems_task_argument argument )
134 158 {
135 159 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
136 160 size_t size; // size of the incoming TC packet
137 161 asm_msg *incomingMsg;
138 162 //
139 163 rtems_status_code status;
140 164 rtems_id queue_id_send;
141 165 rtems_id queue_id_q_p2;
142 166 bp_packet __attribute__((aligned(4))) packet_norm_bp1;
143 167 bp_packet __attribute__((aligned(4))) packet_norm_bp2;
144 168 ring_node *current_ring_node_to_send_asm_f2;
145 169 float nbSMInASMNORM;
146 170
147 171 unsigned long long int localTime;
148 172
149 173 size = 0;
150 174 queue_id_send = RTEMS_ID_NONE;
151 175 queue_id_q_p2 = RTEMS_ID_NONE;
152 176 memset( &packet_norm_bp1, 0, sizeof(bp_packet) );
153 177 memset( &packet_norm_bp2, 0, sizeof(bp_packet) );
154 178
155 179 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
156 180 init_ring( ring_to_send_asm_f2, NB_RING_NODES_ASM_F2, (volatile int*) buffer_asm_f2, TOTAL_SIZE_SM );
157 181 current_ring_node_to_send_asm_f2 = ring_to_send_asm_f2;
158 182
159 183 //*************
160 184 // NORM headers
161 185 BP_init_header( &packet_norm_bp1,
162 186 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F2,
163 187 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2, NB_BINS_COMPRESSED_SM_F2 );
164 188 BP_init_header( &packet_norm_bp2,
165 189 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F2,
166 190 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2, NB_BINS_COMPRESSED_SM_F2 );
167 191
168 192 status = get_message_queue_id_send( &queue_id_send );
169 193 if (status != RTEMS_SUCCESSFUL)
170 194 {
171 195 PRINTF1("in PRC2 *** ERR get_message_queue_id_send %d\n", status)
172 196 }
173 197 status = get_message_queue_id_prc2( &queue_id_q_p2);
174 198 if (status != RTEMS_SUCCESSFUL)
175 199 {
176 200 PRINTF1("in PRC2 *** ERR get_message_queue_id_prc2 %d\n", status)
177 201 }
178 202
179 203 BOOT_PRINTF("in PRC2 ***\n")
180 204
181 205 while(1){
182 206 status = rtems_message_queue_receive( queue_id_q_p2, incomingData, &size, //************************************
183 207 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF2
184 208
185 209 incomingMsg = (asm_msg*) incomingData;
186 210
187 211 ASM_patch( incomingMsg->norm->matrix, asm_f2_patched_norm );
188 212
189 213 localTime = getTimeAsUnsignedLongLongInt( );
190 214
191 215 nbSMInASMNORM = incomingMsg->numberOfSMInASMNORM;
192 216
193 217 //*****
194 218 //*****
195 219 // NORM
196 220 //*****
197 221 //*****
198 222 // 1) compress the matrix for Basic Parameters calculation
199 223 ASM_compress_reorganize_and_divide_mask( asm_f2_patched_norm, compressed_sm_norm_f2,
200 224 nbSMInASMNORM,
201 225 NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2,
202 226 ASM_F2_INDICE_START, CHANNELF2 );
203 227 // BP1_F2
204 228 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F2)
205 229 {
206 230 // 1) compute the BP1 set
207 231 BP1_set( compressed_sm_norm_f2, k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp1.data );
208 232 // 2) send the BP1 set
209 233 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
210 234 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
211 235 packet_norm_bp1.pa_bia_status_info = pa_bia_status_info;
212 236 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
213 237 BP_send( (char *) &packet_norm_bp1, queue_id_send,
214 238 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2 + PACKET_LENGTH_DELTA,
215 239 SID_NORM_BP1_F2 );
216 240 }
217 241 // BP2_F2
218 242 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F2)
219 243 {
220 244 // 1) compute the BP2 set
221 245 BP2_set( compressed_sm_norm_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp2.data );
222 246 // 2) send the BP2 set
223 247 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
224 248 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
225 249 packet_norm_bp2.pa_bia_status_info = pa_bia_status_info;
226 250 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
227 251 BP_send( (char *) &packet_norm_bp2, queue_id_send,
228 252 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2 + PACKET_LENGTH_DELTA,
229 253 SID_NORM_BP2_F2 );
230 254 }
231 255
232 256 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F2)
233 257 {
234 258 // 1) reorganize the ASM and divide
235 259 ASM_reorganize_and_divide( asm_f2_patched_norm,
236 260 (float*) current_ring_node_to_send_asm_f2->buffer_address,
237 261 nb_sm_before_f2.norm_bp1 );
238 262 current_ring_node_to_send_asm_f2->coarseTime = incomingMsg->coarseTimeNORM;
239 263 current_ring_node_to_send_asm_f2->fineTime = incomingMsg->fineTimeNORM;
240 264 current_ring_node_to_send_asm_f2->sid = SID_NORM_ASM_F2;
241 265
242 266 // 3) send the spectral matrix packets
243 267 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f2, sizeof( ring_node* ) );
244 268
245 269 // change asm ring node
246 270 current_ring_node_to_send_asm_f2 = current_ring_node_to_send_asm_f2->next;
247 271 }
248 272
249 273 update_queue_max_count( queue_id_q_p2, &hk_lfr_q_p2_fifo_size_max );
250 274
251 275 }
252 276 }
253 277
254 278 //**********
255 279 // FUNCTIONS
256 280
257 281 void reset_nb_sm_f2( void )
258 282 {
259 283 nb_sm_before_f2.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0;
260 284 nb_sm_before_f2.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1;
261 285 nb_sm_before_f2.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_asm_p[1];
262 286 }
263 287
264 288 void SM_average_f2( float *averaged_spec_mat_f2,
265 289 ring_node *ring_node,
266 290 unsigned int nbAverageNormF2,
267 291 asm_msg *msgForMATR )
268 292 {
269 293 float sum;
270 294 unsigned int i;
271 295 unsigned char keepMatrix;
272 296
273 297 // test acquisitionTime validity
274 298 keepMatrix = acquisitionTimeIsValid( ring_node->coarseTime, ring_node->fineTime, CHANNELF2 );
275 299
276 300 for(i=0; i<TOTAL_SIZE_SM; i++)
277 301 {
278 302 sum = ( (int *) (ring_node->buffer_address) ) [ i ];
279 303 if ( (nbAverageNormF2 == 0) ) // average initialization
280 304 {
281 305 if (keepMatrix == MATRIX_IS_NOT_POLLUTED) // keep the matrix and add it to the average
282 306 {
283 307 averaged_spec_mat_f2[ i ] = sum;
284 308 }
285 309 else // drop the matrix and initialize the average
286 310 {
287 311 averaged_spec_mat_f2[ i ] = INIT_FLOAT;
288 312 }
289 313 msgForMATR->coarseTimeNORM = ring_node->coarseTime;
290 314 msgForMATR->fineTimeNORM = ring_node->fineTime;
291 315 }
292 316 else
293 317 {
294 318 if (keepMatrix == MATRIX_IS_NOT_POLLUTED) // keep the matrix and add it to the average
295 319 {
296 320 averaged_spec_mat_f2[ i ] = ( averaged_spec_mat_f2[ i ] + sum );
297 321 }
298 322 else
299 323 {
300 324 // nothing to do, the matrix is not valid
301 325 }
302 326 }
303 327 }
304 328
305 329 if (keepMatrix == 1)
306 330 {
307 331 if ( (nbAverageNormF2 == 0) )
308 332 {
309 333 msgForMATR->numberOfSMInASMNORM = 1;
310 334 }
311 335 else
312 336 {
313 337 msgForMATR->numberOfSMInASMNORM++;
314 338 }
315 339 }
316 340 else
317 341 {
318 342 if ( (nbAverageNormF2 == 0) )
319 343 {
320 344 msgForMATR->numberOfSMInASMNORM = 0;
321 345 }
322 346 else
323 347 {
324 348 // nothing to do
325 349 }
326 350 }
327 351 }
328 352
329 353 void init_k_coefficients_prc2( void )
330 354 {
331 355 init_k_coefficients( k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2);
332 356 }
Show file before | Show file after | Hide comments
@@ -1,840 +1,898
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** Functions related to data processing.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 31 *
8 32 */
9 33
10 34 #include "fsw_processing.h"
11 35 #include "fsw_processing_globals.c"
12 36 #include "fsw_init.h"
13 37
14 38 unsigned int nb_sm_f0 = 0;
15 39 unsigned int nb_sm_f0_aux_f1= 0;
16 40 unsigned int nb_sm_f1 = 0;
17 41 unsigned int nb_sm_f0_aux_f2= 0;
18 42
19 43 typedef enum restartState_t
20 44 {
21 45 WAIT_FOR_F2,
22 46 WAIT_FOR_F1,
23 47 WAIT_FOR_F0
24 48 } restartState;
25 49
26 50 //************************
27 51 // spectral matrices rings
28 52 ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ] = {0};
29 53 ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ] = {0};
30 54 ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ] = {0};
31 55 ring_node *current_ring_node_sm_f0 = NULL;
32 56 ring_node *current_ring_node_sm_f1 = NULL;
33 57 ring_node *current_ring_node_sm_f2 = NULL;
34 58 ring_node *ring_node_for_averaging_sm_f0= NULL;
35 59 ring_node *ring_node_for_averaging_sm_f1= NULL;
36 60 ring_node *ring_node_for_averaging_sm_f2= NULL;
37 61
38 62 //
39 63 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
40 64 {
41 65 ring_node *node;
42 66
43 67 node = NULL;
44 68 switch ( frequencyChannel ) {
45 69 case CHANNELF0:
46 70 node = ring_node_for_averaging_sm_f0;
47 71 break;
48 72 case CHANNELF1:
49 73 node = ring_node_for_averaging_sm_f1;
50 74 break;
51 75 case CHANNELF2:
52 76 node = ring_node_for_averaging_sm_f2;
53 77 break;
54 78 default:
55 79 break;
56 80 }
57 81
58 82 return node;
59 83 }
60 84
61 85 //***********************************************************
62 86 // Interrupt Service Routine for spectral matrices processing
63 87
64 88 void spectral_matrices_isr_f0( int statusReg )
65 89 {
66 90 unsigned char status;
67 91 rtems_status_code status_code;
68 92 ring_node *full_ring_node;
69 93
70 94 status = (unsigned char) (statusReg & BITS_STATUS_F0); // [0011] get the status_ready_matrix_f0_x bits
71 95
72 96 switch(status)
73 97 {
74 98 case 0:
75 99 break;
76 100 case BIT_READY_0_1:
77 101 // UNEXPECTED VALUE
78 102 spectral_matrix_regs->status = BIT_READY_0_1; // [0011]
79 103 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
80 104 break;
81 105 case BIT_READY_0:
82 106 full_ring_node = current_ring_node_sm_f0->previous;
83 107 full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
84 108 full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time;
85 109 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
86 110 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
87 111 // if there are enough ring nodes ready, wake up an AVFx task
88 112 nb_sm_f0 = nb_sm_f0 + 1;
89 113 if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
90 114 {
91 115 ring_node_for_averaging_sm_f0 = full_ring_node;
92 116 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
93 117 {
94 118 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
95 119 }
96 120 nb_sm_f0 = 0;
97 121 }
98 122 spectral_matrix_regs->status = BIT_READY_0; // [0000 0001]
99 123 break;
100 124 case BIT_READY_1:
101 125 full_ring_node = current_ring_node_sm_f0->previous;
102 126 full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
103 127 full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time;
104 128 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
105 129 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
106 130 // if there are enough ring nodes ready, wake up an AVFx task
107 131 nb_sm_f0 = nb_sm_f0 + 1;
108 132 if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
109 133 {
110 134 ring_node_for_averaging_sm_f0 = full_ring_node;
111 135 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
112 136 {
113 137 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
114 138 }
115 139 nb_sm_f0 = 0;
116 140 }
117 141 spectral_matrix_regs->status = BIT_READY_1; // [0000 0010]
118 142 break;
119 143 default:
120 144 break;
121 145 }
122 146 }
123 147
124 148 void spectral_matrices_isr_f1( int statusReg )
125 149 {
126 150 rtems_status_code status_code;
127 151 unsigned char status;
128 152 ring_node *full_ring_node;
129 153
130 154 status = (unsigned char) ((statusReg & BITS_STATUS_F1) >> SHIFT_2_BITS); // [1100] get the status_ready_matrix_f1_x bits
131 155
132 156 switch(status)
133 157 {
134 158 case 0:
135 159 break;
136 160 case BIT_READY_0_1:
137 161 // UNEXPECTED VALUE
138 162 spectral_matrix_regs->status = BITS_STATUS_F1; // [1100]
139 163 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
140 164 break;
141 165 case BIT_READY_0:
142 166 full_ring_node = current_ring_node_sm_f1->previous;
143 167 full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
144 168 full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
145 169 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
146 170 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
147 171 // if there are enough ring nodes ready, wake up an AVFx task
148 172 nb_sm_f1 = nb_sm_f1 + 1;
149 173 if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
150 174 {
151 175 ring_node_for_averaging_sm_f1 = full_ring_node;
152 176 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
153 177 {
154 178 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
155 179 }
156 180 nb_sm_f1 = 0;
157 181 }
158 182 spectral_matrix_regs->status = BIT_STATUS_F1_0; // [0000 0100]
159 183 break;
160 184 case BIT_READY_1:
161 185 full_ring_node = current_ring_node_sm_f1->previous;
162 186 full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
163 187 full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
164 188 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
165 189 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
166 190 // if there are enough ring nodes ready, wake up an AVFx task
167 191 nb_sm_f1 = nb_sm_f1 + 1;
168 192 if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
169 193 {
170 194 ring_node_for_averaging_sm_f1 = full_ring_node;
171 195 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
172 196 {
173 197 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
174 198 }
175 199 nb_sm_f1 = 0;
176 200 }
177 201 spectral_matrix_regs->status = BIT_STATUS_F1_1; // [1000 0000]
178 202 break;
179 203 default:
180 204 break;
181 205 }
182 206 }
183 207
184 208 void spectral_matrices_isr_f2( int statusReg )
185 209 {
186 210 unsigned char status;
187 211 rtems_status_code status_code;
188 212
189 213 status = (unsigned char) ((statusReg & BITS_STATUS_F2) >> SHIFT_4_BITS); // [0011 0000] get the status_ready_matrix_f2_x bits
190 214
191 215 switch(status)
192 216 {
193 217 case 0:
194 218 break;
195 219 case BIT_READY_0_1:
196 220 // UNEXPECTED VALUE
197 221 spectral_matrix_regs->status = BITS_STATUS_F2; // [0011 0000]
198 222 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
199 223 break;
200 224 case BIT_READY_0:
201 225 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
202 226 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
203 227 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
204 228 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
205 229 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
206 230 spectral_matrix_regs->status = BIT_STATUS_F2_0; // [0001 0000]
207 231 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
208 232 {
209 233 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
210 234 }
211 235 break;
212 236 case BIT_READY_1:
213 237 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
214 238 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
215 239 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
216 240 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
217 241 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
218 242 spectral_matrix_regs->status = BIT_STATUS_F2_1; // [0010 0000]
219 243 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
220 244 {
221 245 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
222 246 }
223 247 break;
224 248 default:
225 249 break;
226 250 }
227 251 }
228 252
229 253 void spectral_matrix_isr_error_handler( int statusReg )
230 254 {
231 255 // STATUS REGISTER
232 256 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
233 257 // 10 9 8
234 258 // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
235 259 // 7 6 5 4 3 2 1 0
236 260 // [bad_component_err] not defined in the last version of the VHDL code
237 261
238 262 rtems_status_code status_code;
239 263
240 264 //***************************************************
241 265 // the ASM status register is copied in the HK packet
242 266 housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) ((statusReg & BITS_HK_AA_SM) >> SHIFT_7_BITS); // [0111 1000 0000]
243 267
244 268 if (statusReg & BITS_SM_ERR) // [0111 1100 0000]
245 269 {
246 270 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
247 271 }
248 272
249 273 spectral_matrix_regs->status = spectral_matrix_regs->status & BITS_SM_ERR;
250 274
251 275 }
252 276
253 277 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
254 278 {
255 279 // STATUS REGISTER
256 280 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
257 281 // 10 9 8
258 282 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
259 283 // 7 6 5 4 3 2 1 0
260 284
261 285 int statusReg;
262 286
263 287 static restartState state = WAIT_FOR_F2;
264 288
265 289 statusReg = spectral_matrix_regs->status;
266 290
267 291 if (thisIsAnASMRestart == 0)
268 292 { // this is not a restart sequence, process incoming matrices normally
269 293 spectral_matrices_isr_f0( statusReg );
270 294
271 295 spectral_matrices_isr_f1( statusReg );
272 296
273 297 spectral_matrices_isr_f2( statusReg );
274 298 }
275 299 else
276 300 { // a restart sequence has to be launched
277 301 switch (state) {
278 302 case WAIT_FOR_F2:
279 303 if ((statusReg & BITS_STATUS_F2) != INIT_CHAR) // [0011 0000] check the status_ready_matrix_f2_x bits
280 304 {
281 305 state = WAIT_FOR_F1;
282 306 }
283 307 break;
284 308 case WAIT_FOR_F1:
285 309 if ((statusReg & BITS_STATUS_F1) != INIT_CHAR) // [0000 1100] check the status_ready_matrix_f1_x bits
286 310 {
287 311 state = WAIT_FOR_F0;
288 312 }
289 313 break;
290 314 case WAIT_FOR_F0:
291 315 if ((statusReg & BITS_STATUS_F0) != INIT_CHAR) // [0000 0011] check the status_ready_matrix_f0_x bits
292 316 {
293 317 state = WAIT_FOR_F2;
294 318 thisIsAnASMRestart = 0;
295 319 }
296 320 break;
297 321 default:
298 322 break;
299 323 }
300 324 reset_sm_status();
301 325 }
302 326
303 327 spectral_matrix_isr_error_handler( statusReg );
304 328
305 329 }
306 330
307 331 //******************
308 332 // Spectral Matrices
309 333
310 334 void reset_nb_sm( void )
311 335 {
312 336 nb_sm_f0 = 0;
313 337 nb_sm_f0_aux_f1 = 0;
314 338 nb_sm_f0_aux_f2 = 0;
315 339
316 340 nb_sm_f1 = 0;
317 341 }
318 342
319 343 void SM_init_rings( void )
320 344 {
321 345 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
322 346 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
323 347 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
324 348
325 349 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
326 350 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
327 351 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
328 352 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
329 353 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
330 354 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
331 355 }
332 356
333 357 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
334 358 {
335 359 unsigned char i;
336 360
337 361 ring[ nbNodes - 1 ].next
338 362 = (ring_node_asm*) &ring[ 0 ];
339 363
340 364 for(i=0; i<nbNodes-1; i++)
341 365 {
342 366 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
343 367 }
344 368 }
345 369
346 370 void SM_reset_current_ring_nodes( void )
347 371 {
348 372 current_ring_node_sm_f0 = sm_ring_f0[0].next;
349 373 current_ring_node_sm_f1 = sm_ring_f1[0].next;
350 374 current_ring_node_sm_f2 = sm_ring_f2[0].next;
351 375
352 376 ring_node_for_averaging_sm_f0 = NULL;
353 377 ring_node_for_averaging_sm_f1 = NULL;
354 378 ring_node_for_averaging_sm_f2 = NULL;
355 379 }
356 380
357 381 //*****************
358 382 // Basic Parameters
359 383
360 384 void BP_init_header( bp_packet *packet,
361 385 unsigned int apid, unsigned char sid,
362 386 unsigned int packetLength, unsigned char blkNr )
363 387 {
364 388 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
365 389 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
366 390 packet->reserved = INIT_CHAR;
367 391 packet->userApplication = CCSDS_USER_APP;
368 392 packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
369 393 packet->packetID[1] = (unsigned char) (apid);
370 394 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
371 395 packet->packetSequenceControl[1] = INIT_CHAR;
372 396 packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
373 397 packet->packetLength[1] = (unsigned char) (packetLength);
374 398 // DATA FIELD HEADER
375 399 packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
376 400 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
377 401 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
378 402 packet->destinationID = TM_DESTINATION_ID_GROUND;
379 403 packet->time[BYTE_0] = INIT_CHAR;
380 404 packet->time[BYTE_1] = INIT_CHAR;
381 405 packet->time[BYTE_2] = INIT_CHAR;
382 406 packet->time[BYTE_3] = INIT_CHAR;
383 407 packet->time[BYTE_4] = INIT_CHAR;
384 408 packet->time[BYTE_5] = INIT_CHAR;
385 409 // AUXILIARY DATA HEADER
386 410 packet->sid = sid;
387 411 packet->pa_bia_status_info = INIT_CHAR;
388 412 packet->sy_lfr_common_parameters_spare = INIT_CHAR;
389 413 packet->sy_lfr_common_parameters = INIT_CHAR;
390 414 packet->acquisitionTime[BYTE_0] = INIT_CHAR;
391 415 packet->acquisitionTime[BYTE_1] = INIT_CHAR;
392 416 packet->acquisitionTime[BYTE_2] = INIT_CHAR;
393 417 packet->acquisitionTime[BYTE_3] = INIT_CHAR;
394 418 packet->acquisitionTime[BYTE_4] = INIT_CHAR;
395 419 packet->acquisitionTime[BYTE_5] = INIT_CHAR;
396 420 packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB
397 421 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
398 422 }
399 423
400 424 void BP_init_header_with_spare( bp_packet_with_spare *packet,
401 425 unsigned int apid, unsigned char sid,
402 426 unsigned int packetLength , unsigned char blkNr)
403 427 {
404 428 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
405 429 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
406 430 packet->reserved = INIT_CHAR;
407 431 packet->userApplication = CCSDS_USER_APP;
408 432 packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
409 433 packet->packetID[1] = (unsigned char) (apid);
410 434 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
411 435 packet->packetSequenceControl[1] = INIT_CHAR;
412 436 packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
413 437 packet->packetLength[1] = (unsigned char) (packetLength);
414 438 // DATA FIELD HEADER
415 439 packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
416 440 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
417 441 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
418 442 packet->destinationID = TM_DESTINATION_ID_GROUND;
419 443 // AUXILIARY DATA HEADER
420 444 packet->sid = sid;
421 445 packet->pa_bia_status_info = INIT_CHAR;
422 446 packet->sy_lfr_common_parameters_spare = INIT_CHAR;
423 447 packet->sy_lfr_common_parameters = INIT_CHAR;
424 448 packet->time[BYTE_0] = INIT_CHAR;
425 449 packet->time[BYTE_1] = INIT_CHAR;
426 450 packet->time[BYTE_2] = INIT_CHAR;
427 451 packet->time[BYTE_3] = INIT_CHAR;
428 452 packet->time[BYTE_4] = INIT_CHAR;
429 453 packet->time[BYTE_5] = INIT_CHAR;
430 454 packet->source_data_spare = INIT_CHAR;
431 455 packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB
432 456 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
433 457 }
434 458
435 459 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
436 460 {
437 461 rtems_status_code status;
438 462
439 463 // SEND PACKET
440 464 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
441 465 if (status != RTEMS_SUCCESSFUL)
442 466 {
443 467 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
444 468 }
445 469 }
446 470
447 471 void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
448 472 {
449 473 /** This function is used to send the BP paquets when needed.
450 474 *
451 475 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
452 476 *
453 477 * @return void
454 478 *
455 479 * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition.
456 480 * BURST paquets are sent everytime.
457 481 *
458 482 */
459 483
460 484 rtems_status_code status;
461 485
462 486 // SEND PACKET
463 487 // before lastValidTransitionDate, the data are drops even if they are ready
464 488 // this guarantees that no SBM packets will be received before the requested enter mode time
465 489 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
466 490 {
467 491 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
468 492 if (status != RTEMS_SUCCESSFUL)
469 493 {
470 494 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
471 495 }
472 496 }
473 497 }
474 498
475 499 //******************
476 500 // general functions
477 501
478 502 void reset_sm_status( void )
479 503 {
480 504 // error
481 505 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
482 506 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
483 507 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
484 508 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
485 509
486 510 spectral_matrix_regs->status = BITS_STATUS_REG; // [0111 1111 1111]
487 511 }
488 512
489 513 void reset_spectral_matrix_regs( void )
490 514 {
491 515 /** This function resets the spectral matrices module registers.
492 516 *
493 517 * The registers affected by this function are located at the following offset addresses:
494 518 *
495 519 * - 0x00 config
496 520 * - 0x04 status
497 521 * - 0x08 matrixF0_Address0
498 522 * - 0x10 matrixFO_Address1
499 523 * - 0x14 matrixF1_Address
500 524 * - 0x18 matrixF2_Address
501 525 *
502 526 */
503 527
504 528 set_sm_irq_onError( 0 );
505 529
506 530 set_sm_irq_onNewMatrix( 0 );
507 531
508 532 reset_sm_status();
509 533
510 534 // F1
511 535 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
512 536 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
513 537 // F2
514 538 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
515 539 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
516 540 // F3
517 541 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
518 542 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
519 543
520 544 spectral_matrix_regs->matrix_length = DEFAULT_MATRIX_LENGTH; // 25 * 128 / 16 = 200 = 0xc8
521 545 }
522 546
523 547 void set_time( unsigned char *time, unsigned char * timeInBuffer )
524 548 {
525 549 time[BYTE_0] = timeInBuffer[BYTE_0];
526 550 time[BYTE_1] = timeInBuffer[BYTE_1];
527 551 time[BYTE_2] = timeInBuffer[BYTE_2];
528 552 time[BYTE_3] = timeInBuffer[BYTE_3];
529 553 time[BYTE_4] = timeInBuffer[BYTE_6];
530 554 time[BYTE_5] = timeInBuffer[BYTE_7];
531 555 }
532 556
533 557 unsigned long long int get_acquisition_time( unsigned char *timePtr )
534 558 {
535 559 unsigned long long int acquisitionTimeAslong;
536 560 acquisitionTimeAslong = INIT_CHAR;
537 561 acquisitionTimeAslong =
538 562 ( (unsigned long long int) (timePtr[BYTE_0] & SYNC_BIT_MASK) << SHIFT_5_BYTES ) // [0111 1111] mask the synchronization bit
539 563 + ( (unsigned long long int) timePtr[BYTE_1] << SHIFT_4_BYTES )
540 564 + ( (unsigned long long int) timePtr[BYTE_2] << SHIFT_3_BYTES )
541 565 + ( (unsigned long long int) timePtr[BYTE_3] << SHIFT_2_BYTES )
542 566 + ( (unsigned long long int) timePtr[BYTE_6] << SHIFT_1_BYTE )
543 567 + ( (unsigned long long int) timePtr[BYTE_7] );
544 568 return acquisitionTimeAslong;
545 569 }
546 570
547 571 unsigned char getSID( rtems_event_set event )
548 572 {
549 573 unsigned char sid;
550 574
551 575 rtems_event_set eventSetBURST;
552 576 rtems_event_set eventSetSBM;
553 577
554 578 sid = 0;
555 579
556 580 //******
557 581 // BURST
558 582 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
559 583 | RTEMS_EVENT_BURST_BP1_F1
560 584 | RTEMS_EVENT_BURST_BP2_F0
561 585 | RTEMS_EVENT_BURST_BP2_F1;
562 586
563 587 //****
564 588 // SBM
565 589 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
566 590 | RTEMS_EVENT_SBM_BP1_F1
567 591 | RTEMS_EVENT_SBM_BP2_F0
568 592 | RTEMS_EVENT_SBM_BP2_F1;
569 593
570 594 if (event & eventSetBURST)
571 595 {
572 596 sid = SID_BURST_BP1_F0;
573 597 }
574 598 else if (event & eventSetSBM)
575 599 {
576 600 sid = SID_SBM1_BP1_F0;
577 601 }
578 602 else
579 603 {
580 604 sid = 0;
581 605 }
582 606
583 607 return sid;
584 608 }
585 609
610 /**
611 * @brief extractReImVectors converts a given ASM component from interleaved to split representation
612 * @param inputASM
613 * @param outputASM
614 * @param asmComponent
615 */
586 616 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
587 617 {
588 618 unsigned int i;
589 619 float re;
590 620 float im;
591 621
592 622 for (i=0; i<NB_BINS_PER_SM; i++){
593 623 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) ];
594 624 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) + 1];
595 625 outputASM[ ( asmComponent *NB_BINS_PER_SM) + i] = re;
596 626 outputASM[ ((asmComponent+1)*NB_BINS_PER_SM) + i] = im;
597 627 }
598 628 }
599 629
630 /**
631 * @brief copyReVectors copies real part of a given ASM from inputASM to outputASM
632 * @param inputASM
633 * @param outputASM
634 * @param asmComponent
635 */
600 636 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
601 637 {
602 638 unsigned int i;
603 639 float re;
604 640
605 641 for (i=0; i<NB_BINS_PER_SM; i++){
606 642 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
607 643 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
608 644 }
609 645 }
610 646
647 /**
648 * @brief ASM_patch, converts ASM from interleaved to split representation
649 * @param inputASM
650 * @param outputASM
651 * @note inputASM and outputASM must be different, in other words this function can't do in place convertion
652 * @see extractReImVectors
653 */
611 654 void ASM_patch( float *inputASM, float *outputASM )
612 655 {
613 656 extractReImVectors( inputASM, outputASM, ASM_COMP_B1B2); // b1b2
614 657 extractReImVectors( inputASM, outputASM, ASM_COMP_B1B3 ); // b1b3
615 658 extractReImVectors( inputASM, outputASM, ASM_COMP_B1E1 ); // b1e1
616 659 extractReImVectors( inputASM, outputASM, ASM_COMP_B1E2 ); // b1e2
617 660 extractReImVectors( inputASM, outputASM, ASM_COMP_B2B3 ); // b2b3
618 661 extractReImVectors( inputASM, outputASM, ASM_COMP_B2E1 ); // b2e1
619 662 extractReImVectors( inputASM, outputASM, ASM_COMP_B2E2 ); // b2e2
620 663 extractReImVectors( inputASM, outputASM, ASM_COMP_B3E1 ); // b3e1
621 664 extractReImVectors( inputASM, outputASM, ASM_COMP_B3E2 ); // b3e2
622 665 extractReImVectors( inputASM, outputASM, ASM_COMP_E1E2 ); // e1e2
623 666
624 667 copyReVectors(inputASM, outputASM, ASM_COMP_B1B1 ); // b1b1
625 668 copyReVectors(inputASM, outputASM, ASM_COMP_B2B2 ); // b2b2
626 669 copyReVectors(inputASM, outputASM, ASM_COMP_B3B3); // b3b3
627 670 copyReVectors(inputASM, outputASM, ASM_COMP_E1E1); // e1e1
628 671 copyReVectors(inputASM, outputASM, ASM_COMP_E2E2); // e2e2
629 672 }
630 673
631 674 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
632 675 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage,
633 676 unsigned char ASMIndexStart,
634 677 unsigned char channel )
635 678 {
636 679 //*************
637 680 // input format
638 681 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
639 682 //**************
640 683 // output format
641 684 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
642 685 //************
643 686 // compression
644 687 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
645 688 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
646 689
647 690 int frequencyBin;
648 691 int asmComponent;
649 692 int offsetASM;
650 693 int offsetCompressed;
651 694 int offsetFBin;
652 695 int fBinMask;
653 696 int k;
654 697
655 698 // BUILD DATA
656 699 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
657 700 {
658 701 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
659 702 {
660 703 offsetCompressed = // NO TIME OFFSET
661 704 (frequencyBin * NB_VALUES_PER_SM)
662 705 + asmComponent;
663 706 offsetASM = // NO TIME OFFSET
664 707 (asmComponent * NB_BINS_PER_SM)
665 708 + ASMIndexStart
666 709 + (frequencyBin * nbBinsToAverage);
667 710 offsetFBin = ASMIndexStart
668 711 + (frequencyBin * nbBinsToAverage);
669 712 compressed_spec_mat[ offsetCompressed ] = 0;
670 713 for ( k = 0; k < nbBinsToAverage; k++ )
671 714 {
672 715 fBinMask = getFBinMask( offsetFBin + k, channel );
673 716 compressed_spec_mat[offsetCompressed ] = compressed_spec_mat[ offsetCompressed ]
674 717 + (averaged_spec_mat[ offsetASM + k ] * fBinMask);
675 718 }
676 719 if (divider != 0)
677 720 {
678 721 compressed_spec_mat[ offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
679 722 }
680 723 else
681 724 {
682 725 compressed_spec_mat[ offsetCompressed ] = INIT_FLOAT;
683 726 }
684 727 }
685 728 }
686 729
687 730 }
688 731
689 732 int getFBinMask( int index, unsigned char channel )
690 733 {
691 734 unsigned int indexInChar;
692 735 unsigned int indexInTheChar;
693 736 int fbin;
694 737 unsigned char *sy_lfr_fbins_fx_word1;
695 738
696 739 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
697 740
698 741 switch(channel)
699 742 {
700 743 case CHANNELF0:
701 744 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0;
702 745 break;
703 746 case CHANNELF1:
704 747 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1;
705 748 break;
706 749 case CHANNELF2:
707 750 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2;
708 751 break;
709 752 default:
710 753 PRINTF("ERR *** in getFBinMask, wrong frequency channel")
711 754 }
712 755
713 756 indexInChar = index >> SHIFT_3_BITS;
714 757 indexInTheChar = index - (indexInChar * BITS_PER_BYTE);
715 758
716 759 fbin = (int) ((sy_lfr_fbins_fx_word1[ BYTES_PER_MASK - 1 - indexInChar] >> indexInTheChar) & 1);
717 760
718 761 return fbin;
719 762 }
720 763
764 /**
765 * @brief isPolluted returns MATRIX_IS_POLLUTED if there is any overlap between t0:t1 and tbad0:tbad1 ranges
766 * @param t0 Start acquisition time
767 * @param t1 End of acquisition time
768 * @param tbad0 Start time of poluting signal
769 * @param tbad1 End time of poluting signal
770 * @return
771 */
721 772 unsigned char isPolluted( u_int64_t t0, u_int64_t t1, u_int64_t tbad0, u_int64_t tbad1 )
722 773 {
723 774 unsigned char polluted;
724 775
725 776 polluted = MATRIX_IS_NOT_POLLUTED;
726 777
727 778 if ( ((tbad0 < t0) && (t0 < tbad1)) // t0 is inside the polluted range
728 779 || ((tbad0 < t1) && (t1 < tbad1)) // t1 is inside the polluted range
729 780 || ((t0 < tbad0) && (tbad1 < t1)) // the polluted range is inside the signal range
730 781 || ((tbad0 < t0) && (t1 < tbad1))) // the signal range is inside the polluted range
731 782 {
732 783 polluted = MATRIX_IS_POLLUTED;
733 784 }
734 785
735 786 return polluted;
736 787 }
737 788
789 /**
790 * @brief acquisitionTimeIsValid checks if the given acquisition time is poluted by PAS
791 * @param coarseTime Coarse acquisition time of the given SM
792 * @param fineTime Fine acquisition time of the given ASM
793 * @param channel Frequency channel to check, will impact SM time footprint
794 * @return MATRIX_IS_POLLUTED if there is any time overlap between SM and PAS poluting signal
795 */
738 796 unsigned char acquisitionTimeIsValid( unsigned int coarseTime, unsigned int fineTime, unsigned char channel)
739 797 {
740 798 u_int64_t t0;
741 799 u_int64_t t1;
742 800 u_int64_t tc;
743 801 u_int64_t tbad0;
744 802 u_int64_t tbad1;
745 803
746 804 u_int64_t modulusInFineTime;
747 805 u_int64_t offsetInFineTime;
748 806 u_int64_t shiftInFineTime;
749 807 u_int64_t tbadInFineTime;
750 808
751 809 u_int64_t timecodeReference;
752 810
753 811 unsigned char pasFilteringIsEnabled;
754 812 unsigned char ret;
755 813
756 814 // compute acquisition time from caoarseTime and fineTime
757 815 t0 = ( ((u_int64_t)coarseTime) << SHIFT_2_BYTES ) + (u_int64_t) fineTime;
758 816 t1 = t0;
759 817 tc = t0;
760 818 tbad0 = t0;
761 819 tbad1 = t0;
762 820
763 821 switch(channel)
764 822 {
765 823 case CHANNELF0:
766 824 t1 = t0 + ACQUISITION_DURATION_F0;
767 825 tc = t0 + HALF_ACQUISITION_DURATION_F0;
768 826 break;
769 827 case CHANNELF1:
770 828 t1 = t0 + ACQUISITION_DURATION_F1;
771 829 tc = t0 + HALF_ACQUISITION_DURATION_F1;
772 830 break;
773 831 case CHANNELF2:
774 832 t1 = t0 + ACQUISITION_DURATION_F2;
775 833 tc = t0 + HALF_ACQUISITION_DURATION_F2;
776 834 break;
777 835 default:
778 836 break;
779 837 }
780 838
781 839 // compute the acquitionTime range
782 840 modulusInFineTime = filterPar.modulus_in_finetime;
783 841 offsetInFineTime = filterPar.offset_in_finetime;
784 842 shiftInFineTime = filterPar.shift_in_finetime;
785 843 tbadInFineTime = filterPar.tbad_in_finetime;
786 844 timecodeReference = INIT_INT;
787 845
788 846 pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 1); // [0000 0001]
789 847 ret = MATRIX_IS_NOT_POLLUTED;
790 848
791 849 if ( (tbadInFineTime == 0) || (pasFilteringIsEnabled == 0) )
792 850 {
793 851 ret = MATRIX_IS_NOT_POLLUTED;
794 852 }
795 853 else
796 854 {
797 855 // INTERSECTION TEST #1
798 856 timecodeReference = (tc - (tc % modulusInFineTime)) - modulusInFineTime ;
799 857 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
800 858 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
801 859 ret = isPolluted( t0, t1, tbad0, tbad1 );
802 860
803 861 // INTERSECTION TEST #2
804 862 if (ret == MATRIX_IS_NOT_POLLUTED)
805 863 {
806 864 timecodeReference = (tc - (tc % modulusInFineTime)) ;
807 865 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
808 866 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
809 867 ret = isPolluted( t0, t1, tbad0, tbad1 );
810 868 }
811 869
812 870 // INTERSECTION TEST #3
813 871 if (ret == MATRIX_IS_NOT_POLLUTED)
814 872 {
815 873 timecodeReference = (tc - (tc % modulusInFineTime)) + modulusInFineTime ;
816 874 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
817 875 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
818 876 ret = isPolluted( t0, t1, tbad0, tbad1 );
819 877 }
820 878 }
821 879
822 880 return ret;
823 881 }
824 882
825 883 void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
826 884 {
827 885 unsigned char bin;
828 886 unsigned char kcoeff;
829 887
830 888 for (bin=0; bin<nb_bins_norm; bin++)
831 889 {
832 890 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
833 891 {
834 892 output_kcoeff[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff ) * SBM_COEFF_PER_NORM_COEFF ]
835 893 = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
836 894 output_kcoeff[ ( ( (bin * NB_K_COEFF_PER_BIN ) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ) + 1 ]
837 895 = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
838 896 }
839 897 }
840 898 }
Show file before | Show file after | Hide comments
@@ -1,481 +1,504
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
1 24 /** Functions related to TeleCommand acceptance.
2 25 *
3 26 * @file
4 27 * @author P. LEROY
5 28 *
6 29 * A group of functions to handle TeleCommands parsing.\n
7 30 *
8 31 */
9 32
10 33 #include "tc_acceptance.h"
11 34 #include <stdio.h>
12 35
13 36 unsigned int lookUpTableForCRC[CONST_256] = {0};
14 37
15 38 //**********************
16 39 // GENERAL USE FUNCTIONS
17 40 unsigned int Crc_opt( unsigned char D, unsigned int Chk)
18 41 {
19 42 /** This function generate the CRC for one byte and returns the value of the new syndrome.
20 43 *
21 44 * @param D is the current byte of data.
22 45 * @param Chk is the current syndrom value.
23 46 *
24 47 * @return the value of the new syndrome on two bytes.
25 48 *
26 49 */
27 50
28 51 return(((Chk << SHIFT_1_BYTE) & BYTE0_MASK)^lookUpTableForCRC [(((Chk >> SHIFT_1_BYTE)^D) & BYTE1_MASK)]);
29 52 }
30 53
31 54 void initLookUpTableForCRC( void )
32 55 {
33 56 /** This function is used to initiates the look-up table for fast CRC computation.
34 57 *
35 58 * The global table lookUpTableForCRC[256] is initiated.
36 59 *
37 60 */
38 61
39 62 unsigned int i;
40 63 unsigned int tmp;
41 64
42 65 for (i=0; i<CONST_256; i++)
43 66 {
44 67 tmp = 0;
45 68 if((i & BIT_0) != 0) {
46 69 tmp = tmp ^ CONST_CRC_0;
47 70 }
48 71 if((i & BIT_1) != 0) {
49 72 tmp = tmp ^ CONST_CRC_1;
50 73 }
51 74 if((i & BIT_2) != 0) {
52 75 tmp = tmp ^ CONST_CRC_2;
53 76 }
54 77 if((i & BIT_3) != 0) {
55 78 tmp = tmp ^ CONST_CRC_3;
56 79 }
57 80 if((i & BIT_4) != 0) {
58 81 tmp = tmp ^ CONST_CRC_4;
59 82 }
60 83 if((i & BIT_5) != 0) {
61 84 tmp = tmp ^ CONST_CRC_5;
62 85 }
63 86 if((i & BIT_6) != 0) {
64 87 tmp = tmp ^ CONST_CRC_6;
65 88 }
66 89 if((i & BIT_7) != 0) {
67 90 tmp = tmp ^ CONST_CRC_7;
68 91 }
69 92 lookUpTableForCRC[i] = tmp;
70 93 }
71 94 }
72 95
73 96 void GetCRCAsTwoBytes(unsigned char* data, unsigned char* crcAsTwoBytes, unsigned int sizeOfData)
74 97 {
75 98 /** This function calculates a two bytes Cyclic Redundancy Code.
76 99 *
77 100 * @param data points to a buffer containing the data on which to compute the CRC.
78 101 * @param crcAsTwoBytes points points to a two bytes buffer in which the CRC is stored.
79 102 * @param sizeOfData is the number of bytes of *data* used to compute the CRC.
80 103 *
81 104 * The specification of the Cyclic Redundancy Code is described in the following document: ECSS-E-70-41-A.
82 105 *
83 106 */
84 107
85 108 unsigned int Chk;
86 109 int j;
87 110 Chk = CRC_RESET; // reset the syndrom to all ones
88 111 for (j=0; j<sizeOfData; j++) {
89 112 Chk = Crc_opt(data[j], Chk);
90 113 }
91 114 crcAsTwoBytes[0] = (unsigned char) (Chk >> SHIFT_1_BYTE);
92 115 crcAsTwoBytes[1] = (unsigned char) (Chk & BYTE1_MASK);
93 116 }
94 117
95 118 //*********************
96 119 // ACCEPTANCE FUNCTIONS
97 120 int tc_parser(ccsdsTelecommandPacket_t * TCPacket, unsigned int estimatedPacketLength, unsigned char *computed_CRC)
98 121 {
99 122 /** This function parses TeleCommands.
100 123 *
101 124 * @param TC points to the TeleCommand that will be parsed.
102 125 * @param estimatedPacketLength is the PACKET_LENGTH field calculated from the effective length of the received packet.
103 126 *
104 127 * @return Status code of the parsing.
105 128 *
106 129 * The parsing checks:
107 130 * - process id
108 131 * - category
109 132 * - length: a global check is performed and a per subtype check also
110 133 * - type
111 134 * - subtype
112 135 * - crc
113 136 *
114 137 */
115 138
116 139 int status;
117 140 int status_crc;
118 141 unsigned char pid;
119 142 unsigned char category;
120 143 unsigned int packetLength;
121 144 unsigned char packetType;
122 145 unsigned char packetSubtype;
123 146 unsigned char sid;
124 147
125 148 status = CCSDS_TM_VALID;
126 149
127 150 // APID check *** APID on 2 bytes
128 151 pid = ((TCPacket->packetID[0] & BITS_PID_0) << SHIFT_4_BITS)
129 152 + ( (TCPacket->packetID[1] >> SHIFT_4_BITS) & BITS_PID_1 ); // PID = 11 *** 7 bits xxxxx210 7654xxxx
130 153 category = (TCPacket->packetID[1] & BITS_CAT); // PACKET_CATEGORY = 12 *** 4 bits xxxxxxxx xxxx3210
131 154 packetLength = (TCPacket->packetLength[0] * CONST_256) + TCPacket->packetLength[1];
132 155 packetType = TCPacket->serviceType;
133 156 packetSubtype = TCPacket->serviceSubType;
134 157 sid = TCPacket->sourceID;
135 158
136 159 if ( pid != CCSDS_PROCESS_ID ) // CHECK THE PROCESS ID
137 160 {
138 161 status = ILLEGAL_APID;
139 162 }
140 163 if (status == CCSDS_TM_VALID) // CHECK THE CATEGORY
141 164 {
142 165 if ( category != CCSDS_PACKET_CATEGORY )
143 166 {
144 167 status = ILLEGAL_APID;
145 168 }
146 169 }
147 170 if (status == CCSDS_TM_VALID) // CHECK THE PACKET_LENGTH FIELD AND THE ESTIMATED PACKET_LENGTH COMPLIANCE
148 171 {
149 172 if (packetLength != estimatedPacketLength ) {
150 173 status = WRONG_LEN_PKT;
151 174 }
152 175 }
153 176 if (status == CCSDS_TM_VALID) // CHECK THAT THE PACKET DOES NOT EXCEED THE MAX SIZE
154 177 {
155 178 if ( packetLength > CCSDS_TC_PKT_MAX_SIZE ) {
156 179 status = WRONG_LEN_PKT;
157 180 }
158 181 }
159 182 if (status == CCSDS_TM_VALID) // CHECK THE TYPE
160 183 {
161 184 status = tc_check_type( packetType );
162 185 }
163 186 if (status == CCSDS_TM_VALID) // CHECK THE SUBTYPE
164 187 {
165 188 status = tc_check_type_subtype( packetType, packetSubtype );
166 189 }
167 190 if (status == CCSDS_TM_VALID) // CHECK THE SID
168 191 {
169 192 status = tc_check_sid( sid );
170 193 }
171 194 if (status == CCSDS_TM_VALID) // CHECK THE SUBTYPE AND LENGTH COMPLIANCE
172 195 {
173 196 status = tc_check_length( packetSubtype, packetLength );
174 197 }
175 198 status_crc = tc_check_crc( TCPacket, estimatedPacketLength, computed_CRC );
176 199 if (status == CCSDS_TM_VALID ) // CHECK CRC
177 200 {
178 201 status = status_crc;
179 202 }
180 203
181 204 return status;
182 205 }
183 206
184 207 int tc_check_type( unsigned char packetType )
185 208 {
186 209 /** This function checks that the type of a TeleCommand is valid.
187 210 *
188 211 * @param packetType is the type to check.
189 212 *
190 213 * @return Status code CCSDS_TM_VALID or ILL_TYPE.
191 214 *
192 215 */
193 216
194 217 int status;
195 218
196 219 status = ILL_TYPE;
197 220
198 221 if ( (packetType == TC_TYPE_GEN) || (packetType == TC_TYPE_TIME))
199 222 {
200 223 status = CCSDS_TM_VALID;
201 224 }
202 225 else
203 226 {
204 227 status = ILL_TYPE;
205 228 }
206 229
207 230 return status;
208 231 }
209 232
210 233 int tc_check_type_subtype( unsigned char packetType, unsigned char packetSubType )
211 234 {
212 235 /** This function checks that the subtype of a TeleCommand is valid and coherent with the type.
213 236 *
214 237 * @param packetType is the type of the TC.
215 238 * @param packetSubType is the subtype to check.
216 239 *
217 240 * @return Status code CCSDS_TM_VALID or ILL_SUBTYPE.
218 241 *
219 242 */
220 243
221 244 int status;
222 245
223 246 switch(packetType)
224 247 {
225 248 case TC_TYPE_GEN:
226 249 if ( (packetSubType == TC_SUBTYPE_RESET)
227 250 || (packetSubType == TC_SUBTYPE_LOAD_COMM)
228 251 || (packetSubType == TC_SUBTYPE_LOAD_NORM) || (packetSubType == TC_SUBTYPE_LOAD_BURST)
229 252 || (packetSubType == TC_SUBTYPE_LOAD_SBM1) || (packetSubType == TC_SUBTYPE_LOAD_SBM2)
230 253 || (packetSubType == TC_SUBTYPE_DUMP)
231 254 || (packetSubType == TC_SUBTYPE_ENTER)
232 255 || (packetSubType == TC_SUBTYPE_UPDT_INFO)
233 256 || (packetSubType == TC_SUBTYPE_EN_CAL) || (packetSubType == TC_SUBTYPE_DIS_CAL)
234 257 || (packetSubType == TC_SUBTYPE_LOAD_K) || (packetSubType == TC_SUBTYPE_DUMP_K)
235 258 || (packetSubType == TC_SUBTYPE_LOAD_FBINS)
236 259 || (packetSubType == TC_SUBTYPE_LOAD_FILTER_PAR))
237 260 {
238 261 status = CCSDS_TM_VALID;
239 262 }
240 263 else
241 264 {
242 265 status = ILL_SUBTYPE;
243 266 }
244 267 break;
245 268
246 269 case TC_TYPE_TIME:
247 270 if (packetSubType == TC_SUBTYPE_UPDT_TIME)
248 271 {
249 272 status = CCSDS_TM_VALID;
250 273 }
251 274 else
252 275 {
253 276 status = ILL_SUBTYPE;
254 277 }
255 278 break;
256 279
257 280 default:
258 281 status = ILL_SUBTYPE;
259 282 break;
260 283 }
261 284
262 285 return status;
263 286 }
264 287
265 288 int tc_check_sid( unsigned char sid )
266 289 {
267 290 /** This function checks that the sid of a TeleCommand is valid.
268 291 *
269 292 * @param sid is the sid to check.
270 293 *
271 294 * @return Status code CCSDS_TM_VALID or CORRUPTED.
272 295 *
273 296 */
274 297
275 298 int status;
276 299
277 300 status = WRONG_SRC_ID;
278 301
279 302 if ( (sid == SID_TC_MISSION_TIMELINE) || (sid == SID_TC_TC_SEQUENCES) || (sid == SID_TC_RECOVERY_ACTION_CMD)
280 303 || (sid == SID_TC_BACKUP_MISSION_TIMELINE)
281 304 || (sid == SID_TC_DIRECT_CMD) || (sid == SID_TC_SPARE_GRD_SRC1) || (sid == SID_TC_SPARE_GRD_SRC2)
282 305 || (sid == SID_TC_OBCP) || (sid == SID_TC_SYSTEM_CONTROL) || (sid == SID_TC_AOCS)
283 306 || (sid == SID_TC_RPW_INTERNAL))
284 307 {
285 308 status = CCSDS_TM_VALID;
286 309 }
287 310 else
288 311 {
289 312 status = WRONG_SRC_ID;
290 313 }
291 314
292 315 return status;
293 316 }
294 317
295 318 int tc_check_length( unsigned char packetSubType, unsigned int length )
296 319 {
297 320 /** This function checks that the subtype and the length are compliant.
298 321 *
299 322 * @param packetSubType is the subtype to check.
300 323 * @param length is the length to check.
301 324 *
302 325 * @return Status code CCSDS_TM_VALID or ILL_TYPE.
303 326 *
304 327 */
305 328
306 329 int status;
307 330
308 331 status = LFR_SUCCESSFUL;
309 332
310 333 switch(packetSubType)
311 334 {
312 335 case TC_SUBTYPE_RESET:
313 336 if (length!=(TC_LEN_RESET-CCSDS_TC_TM_PACKET_OFFSET)) {
314 337 status = WRONG_LEN_PKT;
315 338 }
316 339 else {
317 340 status = CCSDS_TM_VALID;
318 341 }
319 342 break;
320 343 case TC_SUBTYPE_LOAD_COMM:
321 344 if (length!=(TC_LEN_LOAD_COMM-CCSDS_TC_TM_PACKET_OFFSET)) {
322 345 status = WRONG_LEN_PKT;
323 346 }
324 347 else {
325 348 status = CCSDS_TM_VALID;
326 349 }
327 350 break;
328 351 case TC_SUBTYPE_LOAD_NORM:
329 352 if (length!=(TC_LEN_LOAD_NORM-CCSDS_TC_TM_PACKET_OFFSET)) {
330 353 status = WRONG_LEN_PKT;
331 354 }
332 355 else {
333 356 status = CCSDS_TM_VALID;
334 357 }
335 358 break;
336 359 case TC_SUBTYPE_LOAD_BURST:
337 360 if (length!=(TC_LEN_LOAD_BURST-CCSDS_TC_TM_PACKET_OFFSET)) {
338 361 status = WRONG_LEN_PKT;
339 362 }
340 363 else {
341 364 status = CCSDS_TM_VALID;
342 365 }
343 366 break;
344 367 case TC_SUBTYPE_LOAD_SBM1:
345 368 if (length!=(TC_LEN_LOAD_SBM1-CCSDS_TC_TM_PACKET_OFFSET)) {
346 369 status = WRONG_LEN_PKT;
347 370 }
348 371 else {
349 372 status = CCSDS_TM_VALID;
350 373 }
351 374 break;
352 375 case TC_SUBTYPE_LOAD_SBM2:
353 376 if (length!=(TC_LEN_LOAD_SBM2-CCSDS_TC_TM_PACKET_OFFSET)) {
354 377 status = WRONG_LEN_PKT;
355 378 }
356 379 else {
357 380 status = CCSDS_TM_VALID;
358 381 }
359 382 break;
360 383 case TC_SUBTYPE_DUMP:
361 384 if (length!=(TC_LEN_DUMP-CCSDS_TC_TM_PACKET_OFFSET)) {
362 385 status = WRONG_LEN_PKT;
363 386 }
364 387 else {
365 388 status = CCSDS_TM_VALID;
366 389 }
367 390 break;
368 391 case TC_SUBTYPE_ENTER:
369 392 if (length!=(TC_LEN_ENTER-CCSDS_TC_TM_PACKET_OFFSET)) {
370 393 status = WRONG_LEN_PKT;
371 394 }
372 395 else {
373 396 status = CCSDS_TM_VALID;
374 397 }
375 398 break;
376 399 case TC_SUBTYPE_UPDT_INFO:
377 400 if (length!=(TC_LEN_UPDT_INFO-CCSDS_TC_TM_PACKET_OFFSET)) {
378 401 status = WRONG_LEN_PKT;
379 402 }
380 403 else {
381 404 status = CCSDS_TM_VALID;
382 405 }
383 406 break;
384 407 case TC_SUBTYPE_EN_CAL:
385 408 if (length!=(TC_LEN_EN_CAL-CCSDS_TC_TM_PACKET_OFFSET)) {
386 409 status = WRONG_LEN_PKT;
387 410 }
388 411 else {
389 412 status = CCSDS_TM_VALID;
390 413 }
391 414 break;
392 415 case TC_SUBTYPE_DIS_CAL:
393 416 if (length!=(TC_LEN_DIS_CAL-CCSDS_TC_TM_PACKET_OFFSET)) {
394 417 status = WRONG_LEN_PKT;
395 418 }
396 419 else {
397 420 status = CCSDS_TM_VALID;
398 421 }
399 422 break;
400 423 case TC_SUBTYPE_LOAD_K:
401 424 if (length!=(TC_LEN_LOAD_K-CCSDS_TC_TM_PACKET_OFFSET)) {
402 425 status = WRONG_LEN_PKT;
403 426 }
404 427 else {
405 428 status = CCSDS_TM_VALID;
406 429 }
407 430 break;
408 431 case TC_SUBTYPE_DUMP_K:
409 432 if (length!=(TC_LEN_DUMP_K-CCSDS_TC_TM_PACKET_OFFSET)) {
410 433 status = WRONG_LEN_PKT;
411 434 }
412 435 else {
413 436 status = CCSDS_TM_VALID;
414 437 }
415 438 break;
416 439 case TC_SUBTYPE_LOAD_FBINS:
417 440 if (length!=(TC_LEN_LOAD_FBINS-CCSDS_TC_TM_PACKET_OFFSET)) {
418 441 status = WRONG_LEN_PKT;
419 442 }
420 443 else {
421 444 status = CCSDS_TM_VALID;
422 445 }
423 446 break;
424 447 case TC_SUBTYPE_LOAD_FILTER_PAR:
425 448 if (length!=(TC_LEN_LOAD_FILTER_PAR-CCSDS_TC_TM_PACKET_OFFSET)) {
426 449 status = WRONG_LEN_PKT;
427 450 }
428 451 else {
429 452 status = CCSDS_TM_VALID;
430 453 }
431 454 break;
432 455 case TC_SUBTYPE_UPDT_TIME:
433 456 if (length!=(TC_LEN_UPDT_TIME-CCSDS_TC_TM_PACKET_OFFSET)) {
434 457 status = WRONG_LEN_PKT;
435 458 }
436 459 else {
437 460 status = CCSDS_TM_VALID;
438 461 }
439 462 break;
440 463 default: // if the subtype is not a legal value, return ILL_SUBTYPE
441 464 status = ILL_SUBTYPE;
442 465 break ;
443 466 }
444 467
445 468 return status;
446 469 }
447 470
448 471 int tc_check_crc( ccsdsTelecommandPacket_t * TCPacket, unsigned int length, unsigned char *computed_CRC )
449 472 {
450 473 /** This function checks the CRC validity of the corresponding TeleCommand packet.
451 474 *
452 475 * @param TCPacket points to the TeleCommand packet to check.
453 476 * @param length is the length of the TC packet.
454 477 *
455 478 * @return Status code CCSDS_TM_VALID or INCOR_CHECKSUM.
456 479 *
457 480 */
458 481
459 482 int status;
460 483 unsigned char * CCSDSContent;
461 484
462 485 status = INCOR_CHECKSUM;
463 486
464 487 CCSDSContent = (unsigned char*) TCPacket->packetID;
465 488 GetCRCAsTwoBytes(CCSDSContent, computed_CRC, length + CCSDS_TC_TM_PACKET_OFFSET - BYTES_PER_CRC); // 2 CRC bytes removed from the calculation of the CRC
466 489
467 490 if (computed_CRC[0] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET - BYTES_PER_CRC]) {
468 491 status = INCOR_CHECKSUM;
469 492 }
470 493 else if (computed_CRC[1] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -1]) {
471 494 status = INCOR_CHECKSUM;
472 495 }
473 496 else {
474 497 status = CCSDS_TM_VALID;
475 498 }
476 499
477 500 return status;
478 501 }
479 502
480 503
481 504
Show file before | Show file after | Hide comments
@@ -1,1673 +1,1696
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
1 24 /** Functions and tasks related to TeleCommand handling.
2 25 *
3 26 * @file
4 27 * @author P. LEROY
5 28 *
6 29 * A group of functions to handle TeleCommands:\n
7 30 * action launching\n
8 31 * TC parsing\n
9 32 * ...
10 33 *
11 34 */
12 35
13 36 #include "tc_handler.h"
14 37 #include "math.h"
15 38
16 39 //***********
17 40 // RTEMS TASK
18 41
19 42 rtems_task actn_task( rtems_task_argument unused )
20 43 {
21 44 /** This RTEMS task is responsible for launching actions upton the reception of valid TeleCommands.
22 45 *
23 46 * @param unused is the starting argument of the RTEMS task
24 47 *
25 48 * The ACTN task waits for data coming from an RTEMS msesage queue. When data arrives, it launches specific actions depending
26 49 * on the incoming TeleCommand.
27 50 *
28 51 */
29 52
30 53 int result;
31 54 rtems_status_code status; // RTEMS status code
32 55 ccsdsTelecommandPacket_t __attribute__((aligned(4))) TC; // TC sent to the ACTN task
33 56 size_t size; // size of the incoming TC packet
34 57 unsigned char subtype; // subtype of the current TC packet
35 58 unsigned char time[BYTES_PER_TIME];
36 59 rtems_id queue_rcv_id;
37 60 rtems_id queue_snd_id;
38 61
39 62 memset(&TC, 0, sizeof(ccsdsTelecommandPacket_t));
40 63 size = 0;
41 64 queue_rcv_id = RTEMS_ID_NONE;
42 65 queue_snd_id = RTEMS_ID_NONE;
43 66
44 67 status = get_message_queue_id_recv( &queue_rcv_id );
45 68 if (status != RTEMS_SUCCESSFUL)
46 69 {
47 70 PRINTF1("in ACTN *** ERR get_message_queue_id_recv %d\n", status)
48 71 }
49 72
50 73 status = get_message_queue_id_send( &queue_snd_id );
51 74 if (status != RTEMS_SUCCESSFUL)
52 75 {
53 76 PRINTF1("in ACTN *** ERR get_message_queue_id_send %d\n", status)
54 77 }
55 78
56 79 result = LFR_SUCCESSFUL;
57 80 subtype = 0; // subtype of the current TC packet
58 81
59 82 BOOT_PRINTF("in ACTN *** \n");
60 83
61 84 while(1)
62 85 {
63 86 status = rtems_message_queue_receive( queue_rcv_id, (char*) &TC, &size,
64 87 RTEMS_WAIT, RTEMS_NO_TIMEOUT);
65 88 getTime( time ); // set time to the current time
66 89 if (status!=RTEMS_SUCCESSFUL)
67 90 {
68 91 PRINTF1("ERR *** in task ACTN *** error receiving a message, code %d \n", status)
69 92 }
70 93 else
71 94 {
72 95 subtype = TC.serviceSubType;
73 96 switch(subtype)
74 97 {
75 98 case TC_SUBTYPE_RESET:
76 99 result = action_reset( &TC, queue_snd_id, time );
77 100 close_action( &TC, result, queue_snd_id );
78 101 break;
79 102 case TC_SUBTYPE_LOAD_COMM:
80 103 result = action_load_common_par( &TC );
81 104 close_action( &TC, result, queue_snd_id );
82 105 break;
83 106 case TC_SUBTYPE_LOAD_NORM:
84 107 result = action_load_normal_par( &TC, queue_snd_id, time );
85 108 close_action( &TC, result, queue_snd_id );
86 109 break;
87 110 case TC_SUBTYPE_LOAD_BURST:
88 111 result = action_load_burst_par( &TC, queue_snd_id, time );
89 112 close_action( &TC, result, queue_snd_id );
90 113 break;
91 114 case TC_SUBTYPE_LOAD_SBM1:
92 115 result = action_load_sbm1_par( &TC, queue_snd_id, time );
93 116 close_action( &TC, result, queue_snd_id );
94 117 break;
95 118 case TC_SUBTYPE_LOAD_SBM2:
96 119 result = action_load_sbm2_par( &TC, queue_snd_id, time );
97 120 close_action( &TC, result, queue_snd_id );
98 121 break;
99 122 case TC_SUBTYPE_DUMP:
100 123 result = action_dump_par( &TC, queue_snd_id );
101 124 close_action( &TC, result, queue_snd_id );
102 125 break;
103 126 case TC_SUBTYPE_ENTER:
104 127 result = action_enter_mode( &TC, queue_snd_id );
105 128 close_action( &TC, result, queue_snd_id );
106 129 break;
107 130 case TC_SUBTYPE_UPDT_INFO:
108 131 result = action_update_info( &TC, queue_snd_id );
109 132 close_action( &TC, result, queue_snd_id );
110 133 break;
111 134 case TC_SUBTYPE_EN_CAL:
112 135 result = action_enable_calibration( &TC, queue_snd_id, time );
113 136 close_action( &TC, result, queue_snd_id );
114 137 break;
115 138 case TC_SUBTYPE_DIS_CAL:
116 139 result = action_disable_calibration( &TC, queue_snd_id, time );
117 140 close_action( &TC, result, queue_snd_id );
118 141 break;
119 142 case TC_SUBTYPE_LOAD_K:
120 143 result = action_load_kcoefficients( &TC, queue_snd_id, time );
121 144 close_action( &TC, result, queue_snd_id );
122 145 break;
123 146 case TC_SUBTYPE_DUMP_K:
124 147 result = action_dump_kcoefficients( &TC, queue_snd_id, time );
125 148 close_action( &TC, result, queue_snd_id );
126 149 break;
127 150 case TC_SUBTYPE_LOAD_FBINS:
128 151 result = action_load_fbins_mask( &TC, queue_snd_id, time );
129 152 close_action( &TC, result, queue_snd_id );
130 153 break;
131 154 case TC_SUBTYPE_LOAD_FILTER_PAR:
132 155 result = action_load_filter_par( &TC, queue_snd_id, time );
133 156 close_action( &TC, result, queue_snd_id );
134 157 break;
135 158 case TC_SUBTYPE_UPDT_TIME:
136 159 result = action_update_time( &TC );
137 160 close_action( &TC, result, queue_snd_id );
138 161 break;
139 162 default:
140 163 break;
141 164 }
142 165 }
143 166 }
144 167 }
145 168
146 169 //***********
147 170 // TC ACTIONS
148 171
149 172 int action_reset(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
150 173 {
151 174 /** This function executes specific actions when a TC_LFR_RESET TeleCommand has been received.
152 175 *
153 176 * @param TC points to the TeleCommand packet that is being processed
154 177 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
155 178 *
156 179 */
157 180
158 181 PRINTF("this is the end!!!\n");
159 182 exit(0);
160 183
161 184 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
162 185
163 186 return LFR_DEFAULT;
164 187 }
165 188
166 189 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
167 190 {
168 191 /** This function executes specific actions when a TC_LFR_ENTER_MODE TeleCommand has been received.
169 192 *
170 193 * @param TC points to the TeleCommand packet that is being processed
171 194 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
172 195 *
173 196 */
174 197
175 198 rtems_status_code status;
176 199 unsigned char requestedMode;
177 200 unsigned int transitionCoarseTime;
178 201 unsigned char * bytePosPtr;
179 202
180 203 bytePosPtr = (unsigned char *) &TC->packetID;
181 204 requestedMode = bytePosPtr[ BYTE_POS_CP_MODE_LFR_SET ];
182 205 copyInt32ByChar( (char*) &transitionCoarseTime, &bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME ] );
183 206 transitionCoarseTime = transitionCoarseTime & COARSE_TIME_MASK;
184 207 status = check_mode_value( requestedMode );
185 208
186 209 if ( status != LFR_SUCCESSFUL ) // the mode value is inconsistent
187 210 {
188 211 send_tm_lfr_tc_exe_inconsistent( TC, queue_id, BYTE_POS_CP_MODE_LFR_SET, requestedMode );
189 212 }
190 213
191 214 else // the mode value is valid, check the transition
192 215 {
193 216 status = check_mode_transition(requestedMode);
194 217 if (status != LFR_SUCCESSFUL)
195 218 {
196 219 PRINTF("ERR *** in action_enter_mode *** check_mode_transition\n")
197 220 send_tm_lfr_tc_exe_not_executable( TC, queue_id );
198 221 }
199 222 }
200 223
201 224 if ( status == LFR_SUCCESSFUL ) // the transition is valid, check the date
202 225 {
203 226 status = check_transition_date( transitionCoarseTime );
204 227 if (status != LFR_SUCCESSFUL)
205 228 {
206 229 PRINTF("ERR *** in action_enter_mode *** check_transition_date\n");
207 230 send_tm_lfr_tc_exe_not_executable(TC, queue_id );
208 231 }
209 232 }
210 233
211 234 if ( status == LFR_SUCCESSFUL ) // the date is valid, enter the mode
212 235 {
213 236 PRINTF1("OK *** in action_enter_mode *** enter mode %d\n", requestedMode);
214 237
215 238 switch(requestedMode)
216 239 {
217 240 case LFR_MODE_STANDBY:
218 241 status = enter_mode_standby();
219 242 break;
220 243 case LFR_MODE_NORMAL:
221 244 status = enter_mode_normal( transitionCoarseTime );
222 245 break;
223 246 case LFR_MODE_BURST:
224 247 status = enter_mode_burst( transitionCoarseTime );
225 248 break;
226 249 case LFR_MODE_SBM1:
227 250 status = enter_mode_sbm1( transitionCoarseTime );
228 251 break;
229 252 case LFR_MODE_SBM2:
230 253 status = enter_mode_sbm2( transitionCoarseTime );
231 254 break;
232 255 default:
233 256 break;
234 257 }
235 258
236 259 if (status != RTEMS_SUCCESSFUL)
237 260 {
238 261 status = LFR_EXE_ERROR;
239 262 }
240 263 }
241 264
242 265 return status;
243 266 }
244 267
245 268 int action_update_info(ccsdsTelecommandPacket_t *TC, rtems_id queue_id)
246 269 {
247 270 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
248 271 *
249 272 * @param TC points to the TeleCommand packet that is being processed
250 273 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
251 274 *
252 275 * @return LFR directive status code:
253 276 * - LFR_DEFAULT
254 277 * - LFR_SUCCESSFUL
255 278 *
256 279 */
257 280
258 281 unsigned int val;
259 282 unsigned int status;
260 283 unsigned char mode;
261 284 unsigned char * bytePosPtr;
262 285 int pos;
263 286 float value;
264 287
265 288 pos = INIT_CHAR;
266 289 value = INIT_FLOAT;
267 290
268 291 status = LFR_DEFAULT;
269 292
270 293 bytePosPtr = (unsigned char *) &TC->packetID;
271 294
272 295 // check LFR mode
273 296 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET5 ] & BITS_LFR_MODE) >> SHIFT_LFR_MODE;
274 297 status = check_update_info_hk_lfr_mode( mode );
275 298 if (status == LFR_SUCCESSFUL) // check TDS mode
276 299 {
277 300 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & BITS_TDS_MODE) >> SHIFT_TDS_MODE;
278 301 status = check_update_info_hk_tds_mode( mode );
279 302 }
280 303 if (status == LFR_SUCCESSFUL) // check THR mode
281 304 {
282 305 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & BITS_THR_MODE);
283 306 status = check_update_info_hk_thr_mode( mode );
284 307 }
285 308 if (status == LFR_SUCCESSFUL) // check reaction wheels frequencies
286 309 {
287 310 status = check_all_sy_lfr_rw_f(TC, &pos, &value);
288 311 }
289 312
290 313 // if the parameters checking succeeds, udpate all parameters
291 314 if (status == LFR_SUCCESSFUL)
292 315 {
293 316 // pa_bia_status_info
294 317 // => pa_bia_mode_mux_set 3 bits
295 318 // => pa_bia_mode_hv_enabled 1 bit
296 319 // => pa_bia_mode_bias1_enabled 1 bit
297 320 // => pa_bia_mode_bias2_enabled 1 bit
298 321 // => pa_bia_mode_bias3_enabled 1 bit
299 322 // => pa_bia_on_off (cp_dpu_bias_on_off)
300 323 pa_bia_status_info = bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET2 ] & BITS_BIA; // [1111 1110]
301 324 pa_bia_status_info = pa_bia_status_info
302 325 | (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET1 ] & 1);
303 326
304 327 // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets)
305 328 getReactionWheelsFrequencies( TC );
306 329 set_hk_lfr_sc_rw_f_flags();
307 330 build_sy_lfr_rw_masks();
308 331
309 332 // once the masks are built, they have to be merged with the fbins_mask
310 333 merge_fbins_masks();
311 334
312 335 // increase the TC_LFR_UPDATE_INFO counter
313 336 if (status == LFR_SUCCESSFUL) // if the parameter check is successful
314 337 {
315 338 val = (housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * CONST_256)
316 339 + housekeeping_packet.hk_lfr_update_info_tc_cnt[1];
317 340 val++;
318 341 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
319 342 housekeeping_packet.hk_lfr_update_info_tc_cnt[1] = (unsigned char) (val);
320 343 }
321 344 }
322 345
323 346 return status;
324 347 }
325 348
326 349 int action_enable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
327 350 {
328 351 /** This function executes specific actions when a TC_LFR_ENABLE_CALIBRATION TeleCommand has been received.
329 352 *
330 353 * @param TC points to the TeleCommand packet that is being processed
331 354 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
332 355 *
333 356 */
334 357
335 358 int result;
336 359
337 360 result = LFR_DEFAULT;
338 361
339 362 setCalibration( true );
340 363
341 364 result = LFR_SUCCESSFUL;
342 365
343 366 return result;
344 367 }
345 368
346 369 int action_disable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
347 370 {
348 371 /** This function executes specific actions when a TC_LFR_DISABLE_CALIBRATION TeleCommand has been received.
349 372 *
350 373 * @param TC points to the TeleCommand packet that is being processed
351 374 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
352 375 *
353 376 */
354 377
355 378 int result;
356 379
357 380 result = LFR_DEFAULT;
358 381
359 382 setCalibration( false );
360 383
361 384 result = LFR_SUCCESSFUL;
362 385
363 386 return result;
364 387 }
365 388
366 389 int action_update_time(ccsdsTelecommandPacket_t *TC)
367 390 {
368 391 /** This function executes specific actions when a TC_LFR_UPDATE_TIME TeleCommand has been received.
369 392 *
370 393 * @param TC points to the TeleCommand packet that is being processed
371 394 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
372 395 *
373 396 * @return LFR_SUCCESSFUL
374 397 *
375 398 */
376 399
377 400 unsigned int val;
378 401
379 402 time_management_regs->coarse_time_load = (TC->dataAndCRC[BYTE_0] << SHIFT_3_BYTES)
380 403 + (TC->dataAndCRC[BYTE_1] << SHIFT_2_BYTES)
381 404 + (TC->dataAndCRC[BYTE_2] << SHIFT_1_BYTE)
382 405 + TC->dataAndCRC[BYTE_3];
383 406
384 407 val = (housekeeping_packet.hk_lfr_update_time_tc_cnt[0] * CONST_256)
385 408 + housekeeping_packet.hk_lfr_update_time_tc_cnt[1];
386 409 val++;
387 410 housekeeping_packet.hk_lfr_update_time_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
388 411 housekeeping_packet.hk_lfr_update_time_tc_cnt[1] = (unsigned char) (val);
389 412
390 413 oneTcLfrUpdateTimeReceived = 1;
391 414
392 415 return LFR_SUCCESSFUL;
393 416 }
394 417
395 418 //*******************
396 419 // ENTERING THE MODES
397 420 int check_mode_value( unsigned char requestedMode )
398 421 {
399 422 int status;
400 423
401 424 status = LFR_DEFAULT;
402 425
403 426 if ( (requestedMode != LFR_MODE_STANDBY)
404 427 && (requestedMode != LFR_MODE_NORMAL) && (requestedMode != LFR_MODE_BURST)
405 428 && (requestedMode != LFR_MODE_SBM1) && (requestedMode != LFR_MODE_SBM2) )
406 429 {
407 430 status = LFR_DEFAULT;
408 431 }
409 432 else
410 433 {
411 434 status = LFR_SUCCESSFUL;
412 435 }
413 436
414 437 return status;
415 438 }
416 439
417 440 int check_mode_transition( unsigned char requestedMode )
418 441 {
419 442 /** This function checks the validity of the transition requested by the TC_LFR_ENTER_MODE.
420 443 *
421 444 * @param requestedMode is the mode requested by the TC_LFR_ENTER_MODE
422 445 *
423 446 * @return LFR directive status codes:
424 447 * - LFR_SUCCESSFUL - the transition is authorized
425 448 * - LFR_DEFAULT - the transition is not authorized
426 449 *
427 450 */
428 451
429 452 int status;
430 453
431 454 switch (requestedMode)
432 455 {
433 456 case LFR_MODE_STANDBY:
434 457 if ( lfrCurrentMode == LFR_MODE_STANDBY ) {
435 458 status = LFR_DEFAULT;
436 459 }
437 460 else
438 461 {
439 462 status = LFR_SUCCESSFUL;
440 463 }
441 464 break;
442 465 case LFR_MODE_NORMAL:
443 466 if ( lfrCurrentMode == LFR_MODE_NORMAL ) {
444 467 status = LFR_DEFAULT;
445 468 }
446 469 else {
447 470 status = LFR_SUCCESSFUL;
448 471 }
449 472 break;
450 473 case LFR_MODE_BURST:
451 474 if ( lfrCurrentMode == LFR_MODE_BURST ) {
452 475 status = LFR_DEFAULT;
453 476 }
454 477 else {
455 478 status = LFR_SUCCESSFUL;
456 479 }
457 480 break;
458 481 case LFR_MODE_SBM1:
459 482 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
460 483 status = LFR_DEFAULT;
461 484 }
462 485 else {
463 486 status = LFR_SUCCESSFUL;
464 487 }
465 488 break;
466 489 case LFR_MODE_SBM2:
467 490 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
468 491 status = LFR_DEFAULT;
469 492 }
470 493 else {
471 494 status = LFR_SUCCESSFUL;
472 495 }
473 496 break;
474 497 default:
475 498 status = LFR_DEFAULT;
476 499 break;
477 500 }
478 501
479 502 return status;
480 503 }
481 504
482 505 void update_last_valid_transition_date( unsigned int transitionCoarseTime )
483 506 {
484 507 if (transitionCoarseTime == 0)
485 508 {
486 509 lastValidEnterModeTime = time_management_regs->coarse_time + 1;
487 510 PRINTF1("lastValidEnterModeTime = 0x%x (transitionCoarseTime = 0 => coarse_time+1)\n", lastValidEnterModeTime);
488 511 }
489 512 else
490 513 {
491 514 lastValidEnterModeTime = transitionCoarseTime;
492 515 PRINTF1("lastValidEnterModeTime = 0x%x\n", transitionCoarseTime);
493 516 }
494 517 }
495 518
496 519 int check_transition_date( unsigned int transitionCoarseTime )
497 520 {
498 521 int status;
499 522 unsigned int localCoarseTime;
500 523 unsigned int deltaCoarseTime;
501 524
502 525 status = LFR_SUCCESSFUL;
503 526
504 527 if (transitionCoarseTime == 0) // transition time = 0 means an instant transition
505 528 {
506 529 status = LFR_SUCCESSFUL;
507 530 }
508 531 else
509 532 {
510 533 localCoarseTime = time_management_regs->coarse_time & COARSE_TIME_MASK;
511 534
512 535 PRINTF2("localTime = %x, transitionTime = %x\n", localCoarseTime, transitionCoarseTime);
513 536
514 537 if ( transitionCoarseTime <= localCoarseTime ) // SSS-CP-EQS-322
515 538 {
516 539 status = LFR_DEFAULT;
517 540 PRINTF("ERR *** in check_transition_date *** transitionCoarseTime <= localCoarseTime\n");
518 541 }
519 542
520 543 if (status == LFR_SUCCESSFUL)
521 544 {
522 545 deltaCoarseTime = transitionCoarseTime - localCoarseTime;
523 546 if ( deltaCoarseTime > MAX_DELTA_COARSE_TIME ) // SSS-CP-EQS-323
524 547 {
525 548 status = LFR_DEFAULT;
526 549 PRINTF1("ERR *** in check_transition_date *** deltaCoarseTime = %x\n", deltaCoarseTime)
527 550 }
528 551 }
529 552 }
530 553
531 554 return status;
532 555 }
533 556
534 557 int restart_asm_activities( unsigned char lfrRequestedMode )
535 558 {
536 559 rtems_status_code status;
537 560
538 561 status = stop_spectral_matrices();
539 562
540 563 thisIsAnASMRestart = 1;
541 564
542 565 status = restart_asm_tasks( lfrRequestedMode );
543 566
544 567 launch_spectral_matrix();
545 568
546 569 return status;
547 570 }
548 571
549 572 int stop_spectral_matrices( void )
550 573 {
551 574 /** This function stops and restarts the current mode average spectral matrices activities.
552 575 *
553 576 * @return RTEMS directive status codes:
554 577 * - RTEMS_SUCCESSFUL - task restarted successfully
555 578 * - RTEMS_INVALID_ID - task id invalid
556 579 * - RTEMS_ALREADY_SUSPENDED - task already suspended
557 580 *
558 581 */
559 582
560 583 rtems_status_code status;
561 584
562 585 status = RTEMS_SUCCESSFUL;
563 586
564 587 // (1) mask interruptions
565 588 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // mask spectral matrix interrupt
566 589
567 590 // (2) reset spectral matrices registers
568 591 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
569 592 reset_sm_status();
570 593
571 594 // (3) clear interruptions
572 595 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
573 596
574 597 // suspend several tasks
575 598 if (lfrCurrentMode != LFR_MODE_STANDBY) {
576 599 status = suspend_asm_tasks();
577 600 }
578 601
579 602 if (status != RTEMS_SUCCESSFUL)
580 603 {
581 604 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
582 605 }
583 606
584 607 return status;
585 608 }
586 609
587 610 int stop_current_mode( void )
588 611 {
589 612 /** This function stops the current mode by masking interrupt lines and suspending science tasks.
590 613 *
591 614 * @return RTEMS directive status codes:
592 615 * - RTEMS_SUCCESSFUL - task restarted successfully
593 616 * - RTEMS_INVALID_ID - task id invalid
594 617 * - RTEMS_ALREADY_SUSPENDED - task already suspended
595 618 *
596 619 */
597 620
598 621 rtems_status_code status;
599 622
600 623 status = RTEMS_SUCCESSFUL;
601 624
602 625 // (1) mask interruptions
603 626 LEON_Mask_interrupt( IRQ_WAVEFORM_PICKER ); // mask waveform picker interrupt
604 627 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
605 628
606 629 // (2) reset waveform picker registers
607 630 reset_wfp_burst_enable(); // reset burst and enable bits
608 631 reset_wfp_status(); // reset all the status bits
609 632
610 633 // (3) reset spectral matrices registers
611 634 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
612 635 reset_sm_status();
613 636
614 637 // reset lfr VHDL module
615 638 reset_lfr();
616 639
617 640 reset_extractSWF(); // reset the extractSWF flag to false
618 641
619 642 // (4) clear interruptions
620 643 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER ); // clear waveform picker interrupt
621 644 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
622 645
623 646 // suspend several tasks
624 647 if (lfrCurrentMode != LFR_MODE_STANDBY) {
625 648 status = suspend_science_tasks();
626 649 }
627 650
628 651 if (status != RTEMS_SUCCESSFUL)
629 652 {
630 653 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
631 654 }
632 655
633 656 return status;
634 657 }
635 658
636 659 int enter_mode_standby( void )
637 660 {
638 661 /** This function is used to put LFR in the STANDBY mode.
639 662 *
640 663 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
641 664 *
642 665 * @return RTEMS directive status codes:
643 666 * - RTEMS_SUCCESSFUL - task restarted successfully
644 667 * - RTEMS_INVALID_ID - task id invalid
645 668 * - RTEMS_INCORRECT_STATE - task never started
646 669 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
647 670 *
648 671 * The STANDBY mode does not depends on a specific transition date, the effect of the TC_LFR_ENTER_MODE
649 672 * is immediate.
650 673 *
651 674 */
652 675
653 676 int status;
654 677
655 678 status = stop_current_mode(); // STOP THE CURRENT MODE
656 679
657 680 #ifdef PRINT_TASK_STATISTICS
658 681 rtems_cpu_usage_report();
659 682 #endif
660 683
661 684 #ifdef PRINT_STACK_REPORT
662 685 PRINTF("stack report selected\n")
663 686 rtems_stack_checker_report_usage();
664 687 #endif
665 688
666 689 return status;
667 690 }
668 691
669 692 int enter_mode_normal( unsigned int transitionCoarseTime )
670 693 {
671 694 /** This function is used to start the NORMAL mode.
672 695 *
673 696 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
674 697 *
675 698 * @return RTEMS directive status codes:
676 699 * - RTEMS_SUCCESSFUL - task restarted successfully
677 700 * - RTEMS_INVALID_ID - task id invalid
678 701 * - RTEMS_INCORRECT_STATE - task never started
679 702 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
680 703 *
681 704 * The way the NORMAL mode is started depends on the LFR current mode. If LFR is in SBM1 or SBM2,
682 705 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected.
683 706 *
684 707 */
685 708
686 709 int status;
687 710
688 711 #ifdef PRINT_TASK_STATISTICS
689 712 rtems_cpu_usage_reset();
690 713 #endif
691 714
692 715 status = RTEMS_UNSATISFIED;
693 716
694 717 switch( lfrCurrentMode )
695 718 {
696 719 case LFR_MODE_STANDBY:
697 720 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart science tasks
698 721 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
699 722 {
700 723 launch_spectral_matrix( );
701 724 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
702 725 }
703 726 break;
704 727 case LFR_MODE_BURST:
705 728 status = stop_current_mode(); // stop the current mode
706 729 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart the science tasks
707 730 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
708 731 {
709 732 launch_spectral_matrix( );
710 733 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
711 734 }
712 735 break;
713 736 case LFR_MODE_SBM1:
714 737 status = restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
715 738 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
716 739 update_last_valid_transition_date( transitionCoarseTime );
717 740 break;
718 741 case LFR_MODE_SBM2:
719 742 status = restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
720 743 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
721 744 update_last_valid_transition_date( transitionCoarseTime );
722 745 break;
723 746 default:
724 747 break;
725 748 }
726 749
727 750 if (status != RTEMS_SUCCESSFUL)
728 751 {
729 752 PRINTF1("ERR *** in enter_mode_normal *** status = %d\n", status)
730 753 status = RTEMS_UNSATISFIED;
731 754 }
732 755
733 756 return status;
734 757 }
735 758
736 759 int enter_mode_burst( unsigned int transitionCoarseTime )
737 760 {
738 761 /** This function is used to start the BURST mode.
739 762 *
740 763 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
741 764 *
742 765 * @return RTEMS directive status codes:
743 766 * - RTEMS_SUCCESSFUL - task restarted successfully
744 767 * - RTEMS_INVALID_ID - task id invalid
745 768 * - RTEMS_INCORRECT_STATE - task never started
746 769 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
747 770 *
748 771 * The way the BURST mode is started does not depend on the LFR current mode.
749 772 *
750 773 */
751 774
752 775
753 776 int status;
754 777
755 778 #ifdef PRINT_TASK_STATISTICS
756 779 rtems_cpu_usage_reset();
757 780 #endif
758 781
759 782 status = stop_current_mode(); // stop the current mode
760 783 status = restart_science_tasks( LFR_MODE_BURST ); // restart the science tasks
761 784 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
762 785 {
763 786 launch_spectral_matrix( );
764 787 launch_waveform_picker( LFR_MODE_BURST, transitionCoarseTime );
765 788 }
766 789
767 790 if (status != RTEMS_SUCCESSFUL)
768 791 {
769 792 PRINTF1("ERR *** in enter_mode_burst *** status = %d\n", status)
770 793 status = RTEMS_UNSATISFIED;
771 794 }
772 795
773 796 return status;
774 797 }
775 798
776 799 int enter_mode_sbm1( unsigned int transitionCoarseTime )
777 800 {
778 801 /** This function is used to start the SBM1 mode.
779 802 *
780 803 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
781 804 *
782 805 * @return RTEMS directive status codes:
783 806 * - RTEMS_SUCCESSFUL - task restarted successfully
784 807 * - RTEMS_INVALID_ID - task id invalid
785 808 * - RTEMS_INCORRECT_STATE - task never started
786 809 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
787 810 *
788 811 * The way the SBM1 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM2,
789 812 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
790 813 * cases, the acquisition is completely restarted.
791 814 *
792 815 */
793 816
794 817 int status;
795 818
796 819 #ifdef PRINT_TASK_STATISTICS
797 820 rtems_cpu_usage_reset();
798 821 #endif
799 822
800 823 status = RTEMS_UNSATISFIED;
801 824
802 825 switch( lfrCurrentMode )
803 826 {
804 827 case LFR_MODE_STANDBY:
805 828 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart science tasks
806 829 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
807 830 {
808 831 launch_spectral_matrix( );
809 832 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
810 833 }
811 834 break;
812 835 case LFR_MODE_NORMAL: // lfrCurrentMode will be updated after the execution of close_action
813 836 status = restart_asm_activities( LFR_MODE_SBM1 );
814 837 status = LFR_SUCCESSFUL;
815 838 update_last_valid_transition_date( transitionCoarseTime );
816 839 break;
817 840 case LFR_MODE_BURST:
818 841 status = stop_current_mode(); // stop the current mode
819 842 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart the science tasks
820 843 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
821 844 {
822 845 launch_spectral_matrix( );
823 846 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
824 847 }
825 848 break;
826 849 case LFR_MODE_SBM2:
827 850 status = restart_asm_activities( LFR_MODE_SBM1 );
828 851 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
829 852 update_last_valid_transition_date( transitionCoarseTime );
830 853 break;
831 854 default:
832 855 break;
833 856 }
834 857
835 858 if (status != RTEMS_SUCCESSFUL)
836 859 {
837 860 PRINTF1("ERR *** in enter_mode_sbm1 *** status = %d\n", status);
838 861 status = RTEMS_UNSATISFIED;
839 862 }
840 863
841 864 return status;
842 865 }
843 866
844 867 int enter_mode_sbm2( unsigned int transitionCoarseTime )
845 868 {
846 869 /** This function is used to start the SBM2 mode.
847 870 *
848 871 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
849 872 *
850 873 * @return RTEMS directive status codes:
851 874 * - RTEMS_SUCCESSFUL - task restarted successfully
852 875 * - RTEMS_INVALID_ID - task id invalid
853 876 * - RTEMS_INCORRECT_STATE - task never started
854 877 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
855 878 *
856 879 * The way the SBM2 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM1,
857 880 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
858 881 * cases, the acquisition is completely restarted.
859 882 *
860 883 */
861 884
862 885 int status;
863 886
864 887 #ifdef PRINT_TASK_STATISTICS
865 888 rtems_cpu_usage_reset();
866 889 #endif
867 890
868 891 status = RTEMS_UNSATISFIED;
869 892
870 893 switch( lfrCurrentMode )
871 894 {
872 895 case LFR_MODE_STANDBY:
873 896 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart science tasks
874 897 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
875 898 {
876 899 launch_spectral_matrix( );
877 900 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
878 901 }
879 902 break;
880 903 case LFR_MODE_NORMAL:
881 904 status = restart_asm_activities( LFR_MODE_SBM2 );
882 905 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
883 906 update_last_valid_transition_date( transitionCoarseTime );
884 907 break;
885 908 case LFR_MODE_BURST:
886 909 status = stop_current_mode(); // stop the current mode
887 910 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart the science tasks
888 911 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
889 912 {
890 913 launch_spectral_matrix( );
891 914 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
892 915 }
893 916 break;
894 917 case LFR_MODE_SBM1:
895 918 status = restart_asm_activities( LFR_MODE_SBM2 );
896 919 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
897 920 update_last_valid_transition_date( transitionCoarseTime );
898 921 break;
899 922 default:
900 923 break;
901 924 }
902 925
903 926 if (status != RTEMS_SUCCESSFUL)
904 927 {
905 928 PRINTF1("ERR *** in enter_mode_sbm2 *** status = %d\n", status)
906 929 status = RTEMS_UNSATISFIED;
907 930 }
908 931
909 932 return status;
910 933 }
911 934
912 935 int restart_science_tasks( unsigned char lfrRequestedMode )
913 936 {
914 937 /** This function is used to restart all science tasks.
915 938 *
916 939 * @return RTEMS directive status codes:
917 940 * - RTEMS_SUCCESSFUL - task restarted successfully
918 941 * - RTEMS_INVALID_ID - task id invalid
919 942 * - RTEMS_INCORRECT_STATE - task never started
920 943 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
921 944 *
922 945 * Science tasks are AVF0, PRC0, WFRM, CWF3, CW2, CWF1
923 946 *
924 947 */
925 948
926 949 rtems_status_code status[NB_SCIENCE_TASKS];
927 950 rtems_status_code ret;
928 951
929 952 ret = RTEMS_SUCCESSFUL;
930 953
931 954 status[STATUS_0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
932 955 if (status[STATUS_0] != RTEMS_SUCCESSFUL)
933 956 {
934 957 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[STATUS_0])
935 958 }
936 959
937 960 status[STATUS_1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
938 961 if (status[STATUS_1] != RTEMS_SUCCESSFUL)
939 962 {
940 963 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[STATUS_1])
941 964 }
942 965
943 966 status[STATUS_2] = rtems_task_restart( Task_id[TASKID_WFRM],1 );
944 967 if (status[STATUS_2] != RTEMS_SUCCESSFUL)
945 968 {
946 969 PRINTF1("in restart_science_task *** WFRM ERR %d\n", status[STATUS_2])
947 970 }
948 971
949 972 status[STATUS_3] = rtems_task_restart( Task_id[TASKID_CWF3],1 );
950 973 if (status[STATUS_3] != RTEMS_SUCCESSFUL)
951 974 {
952 975 PRINTF1("in restart_science_task *** CWF3 ERR %d\n", status[STATUS_3])
953 976 }
954 977
955 978 status[STATUS_4] = rtems_task_restart( Task_id[TASKID_CWF2],1 );
956 979 if (status[STATUS_4] != RTEMS_SUCCESSFUL)
957 980 {
958 981 PRINTF1("in restart_science_task *** CWF2 ERR %d\n", status[STATUS_4])
959 982 }
960 983
961 984 status[STATUS_5] = rtems_task_restart( Task_id[TASKID_CWF1],1 );
962 985 if (status[STATUS_5] != RTEMS_SUCCESSFUL)
963 986 {
964 987 PRINTF1("in restart_science_task *** CWF1 ERR %d\n", status[STATUS_5])
965 988 }
966 989
967 990 status[STATUS_6] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
968 991 if (status[STATUS_6] != RTEMS_SUCCESSFUL)
969 992 {
970 993 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[STATUS_6])
971 994 }
972 995
973 996 status[STATUS_7] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
974 997 if (status[STATUS_7] != RTEMS_SUCCESSFUL)
975 998 {
976 999 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[STATUS_7])
977 1000 }
978 1001
979 1002 status[STATUS_8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
980 1003 if (status[STATUS_8] != RTEMS_SUCCESSFUL)
981 1004 {
982 1005 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[STATUS_8])
983 1006 }
984 1007
985 1008 status[STATUS_9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
986 1009 if (status[STATUS_9] != RTEMS_SUCCESSFUL)
987 1010 {
988 1011 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[STATUS_9])
989 1012 }
990 1013
991 1014 if ( (status[STATUS_0] != RTEMS_SUCCESSFUL) || (status[STATUS_1] != RTEMS_SUCCESSFUL) ||
992 1015 (status[STATUS_2] != RTEMS_SUCCESSFUL) || (status[STATUS_3] != RTEMS_SUCCESSFUL) ||
993 1016 (status[STATUS_4] != RTEMS_SUCCESSFUL) || (status[STATUS_5] != RTEMS_SUCCESSFUL) ||
994 1017 (status[STATUS_6] != RTEMS_SUCCESSFUL) || (status[STATUS_7] != RTEMS_SUCCESSFUL) ||
995 1018 (status[STATUS_8] != RTEMS_SUCCESSFUL) || (status[STATUS_9] != RTEMS_SUCCESSFUL) )
996 1019 {
997 1020 ret = RTEMS_UNSATISFIED;
998 1021 }
999 1022
1000 1023 return ret;
1001 1024 }
1002 1025
1003 1026 int restart_asm_tasks( unsigned char lfrRequestedMode )
1004 1027 {
1005 1028 /** This function is used to restart average spectral matrices tasks.
1006 1029 *
1007 1030 * @return RTEMS directive status codes:
1008 1031 * - RTEMS_SUCCESSFUL - task restarted successfully
1009 1032 * - RTEMS_INVALID_ID - task id invalid
1010 1033 * - RTEMS_INCORRECT_STATE - task never started
1011 1034 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
1012 1035 *
1013 1036 * ASM tasks are AVF0, PRC0, AVF1, PRC1, AVF2 and PRC2
1014 1037 *
1015 1038 */
1016 1039
1017 1040 rtems_status_code status[NB_ASM_TASKS];
1018 1041 rtems_status_code ret;
1019 1042
1020 1043 ret = RTEMS_SUCCESSFUL;
1021 1044
1022 1045 status[STATUS_0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
1023 1046 if (status[STATUS_0] != RTEMS_SUCCESSFUL)
1024 1047 {
1025 1048 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[STATUS_0])
1026 1049 }
1027 1050
1028 1051 status[STATUS_1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
1029 1052 if (status[STATUS_1] != RTEMS_SUCCESSFUL)
1030 1053 {
1031 1054 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[STATUS_1])
1032 1055 }
1033 1056
1034 1057 status[STATUS_2] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
1035 1058 if (status[STATUS_2] != RTEMS_SUCCESSFUL)
1036 1059 {
1037 1060 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[STATUS_2])
1038 1061 }
1039 1062
1040 1063 status[STATUS_3] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
1041 1064 if (status[STATUS_3] != RTEMS_SUCCESSFUL)
1042 1065 {
1043 1066 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[STATUS_3])
1044 1067 }
1045 1068
1046 1069 status[STATUS_4] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
1047 1070 if (status[STATUS_4] != RTEMS_SUCCESSFUL)
1048 1071 {
1049 1072 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[STATUS_4])
1050 1073 }
1051 1074
1052 1075 status[STATUS_5] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
1053 1076 if (status[STATUS_5] != RTEMS_SUCCESSFUL)
1054 1077 {
1055 1078 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[STATUS_5])
1056 1079 }
1057 1080
1058 1081 if ( (status[STATUS_0] != RTEMS_SUCCESSFUL) || (status[STATUS_1] != RTEMS_SUCCESSFUL) ||
1059 1082 (status[STATUS_2] != RTEMS_SUCCESSFUL) || (status[STATUS_3] != RTEMS_SUCCESSFUL) ||
1060 1083 (status[STATUS_4] != RTEMS_SUCCESSFUL) || (status[STATUS_5] != RTEMS_SUCCESSFUL) )
1061 1084 {
1062 1085 ret = RTEMS_UNSATISFIED;
1063 1086 }
1064 1087
1065 1088 return ret;
1066 1089 }
1067 1090
1068 1091 int suspend_science_tasks( void )
1069 1092 {
1070 1093 /** This function suspends the science tasks.
1071 1094 *
1072 1095 * @return RTEMS directive status codes:
1073 1096 * - RTEMS_SUCCESSFUL - task restarted successfully
1074 1097 * - RTEMS_INVALID_ID - task id invalid
1075 1098 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1076 1099 *
1077 1100 */
1078 1101
1079 1102 rtems_status_code status;
1080 1103
1081 1104 PRINTF("in suspend_science_tasks\n")
1082 1105
1083 1106 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1084 1107 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1085 1108 {
1086 1109 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1087 1110 }
1088 1111 else
1089 1112 {
1090 1113 status = RTEMS_SUCCESSFUL;
1091 1114 }
1092 1115 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1093 1116 {
1094 1117 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1095 1118 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1096 1119 {
1097 1120 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1098 1121 }
1099 1122 else
1100 1123 {
1101 1124 status = RTEMS_SUCCESSFUL;
1102 1125 }
1103 1126 }
1104 1127 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1105 1128 {
1106 1129 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1107 1130 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1108 1131 {
1109 1132 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1110 1133 }
1111 1134 else
1112 1135 {
1113 1136 status = RTEMS_SUCCESSFUL;
1114 1137 }
1115 1138 }
1116 1139 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1117 1140 {
1118 1141 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1119 1142 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1120 1143 {
1121 1144 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1122 1145 }
1123 1146 else
1124 1147 {
1125 1148 status = RTEMS_SUCCESSFUL;
1126 1149 }
1127 1150 }
1128 1151 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1129 1152 {
1130 1153 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1131 1154 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1132 1155 {
1133 1156 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1134 1157 }
1135 1158 else
1136 1159 {
1137 1160 status = RTEMS_SUCCESSFUL;
1138 1161 }
1139 1162 }
1140 1163 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1141 1164 {
1142 1165 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1143 1166 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1144 1167 {
1145 1168 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1146 1169 }
1147 1170 else
1148 1171 {
1149 1172 status = RTEMS_SUCCESSFUL;
1150 1173 }
1151 1174 }
1152 1175 if (status == RTEMS_SUCCESSFUL) // suspend WFRM
1153 1176 {
1154 1177 status = rtems_task_suspend( Task_id[TASKID_WFRM] );
1155 1178 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1156 1179 {
1157 1180 PRINTF1("in suspend_science_task *** WFRM ERR %d\n", status)
1158 1181 }
1159 1182 else
1160 1183 {
1161 1184 status = RTEMS_SUCCESSFUL;
1162 1185 }
1163 1186 }
1164 1187 if (status == RTEMS_SUCCESSFUL) // suspend CWF3
1165 1188 {
1166 1189 status = rtems_task_suspend( Task_id[TASKID_CWF3] );
1167 1190 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1168 1191 {
1169 1192 PRINTF1("in suspend_science_task *** CWF3 ERR %d\n", status)
1170 1193 }
1171 1194 else
1172 1195 {
1173 1196 status = RTEMS_SUCCESSFUL;
1174 1197 }
1175 1198 }
1176 1199 if (status == RTEMS_SUCCESSFUL) // suspend CWF2
1177 1200 {
1178 1201 status = rtems_task_suspend( Task_id[TASKID_CWF2] );
1179 1202 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1180 1203 {
1181 1204 PRINTF1("in suspend_science_task *** CWF2 ERR %d\n", status)
1182 1205 }
1183 1206 else
1184 1207 {
1185 1208 status = RTEMS_SUCCESSFUL;
1186 1209 }
1187 1210 }
1188 1211 if (status == RTEMS_SUCCESSFUL) // suspend CWF1
1189 1212 {
1190 1213 status = rtems_task_suspend( Task_id[TASKID_CWF1] );
1191 1214 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1192 1215 {
1193 1216 PRINTF1("in suspend_science_task *** CWF1 ERR %d\n", status)
1194 1217 }
1195 1218 else
1196 1219 {
1197 1220 status = RTEMS_SUCCESSFUL;
1198 1221 }
1199 1222 }
1200 1223
1201 1224 return status;
1202 1225 }
1203 1226
1204 1227 int suspend_asm_tasks( void )
1205 1228 {
1206 1229 /** This function suspends the science tasks.
1207 1230 *
1208 1231 * @return RTEMS directive status codes:
1209 1232 * - RTEMS_SUCCESSFUL - task restarted successfully
1210 1233 * - RTEMS_INVALID_ID - task id invalid
1211 1234 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1212 1235 *
1213 1236 */
1214 1237
1215 1238 rtems_status_code status;
1216 1239
1217 1240 PRINTF("in suspend_science_tasks\n")
1218 1241
1219 1242 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1220 1243 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1221 1244 {
1222 1245 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1223 1246 }
1224 1247 else
1225 1248 {
1226 1249 status = RTEMS_SUCCESSFUL;
1227 1250 }
1228 1251
1229 1252 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1230 1253 {
1231 1254 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1232 1255 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1233 1256 {
1234 1257 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1235 1258 }
1236 1259 else
1237 1260 {
1238 1261 status = RTEMS_SUCCESSFUL;
1239 1262 }
1240 1263 }
1241 1264
1242 1265 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1243 1266 {
1244 1267 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1245 1268 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1246 1269 {
1247 1270 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1248 1271 }
1249 1272 else
1250 1273 {
1251 1274 status = RTEMS_SUCCESSFUL;
1252 1275 }
1253 1276 }
1254 1277
1255 1278 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1256 1279 {
1257 1280 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1258 1281 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1259 1282 {
1260 1283 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1261 1284 }
1262 1285 else
1263 1286 {
1264 1287 status = RTEMS_SUCCESSFUL;
1265 1288 }
1266 1289 }
1267 1290
1268 1291 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1269 1292 {
1270 1293 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1271 1294 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1272 1295 {
1273 1296 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1274 1297 }
1275 1298 else
1276 1299 {
1277 1300 status = RTEMS_SUCCESSFUL;
1278 1301 }
1279 1302 }
1280 1303
1281 1304 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1282 1305 {
1283 1306 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1284 1307 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1285 1308 {
1286 1309 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1287 1310 }
1288 1311 else
1289 1312 {
1290 1313 status = RTEMS_SUCCESSFUL;
1291 1314 }
1292 1315 }
1293 1316
1294 1317 return status;
1295 1318 }
1296 1319
1297 1320 void launch_waveform_picker( unsigned char mode, unsigned int transitionCoarseTime )
1298 1321 {
1299 1322
1300 1323 WFP_reset_current_ring_nodes();
1301 1324
1302 1325 reset_waveform_picker_regs();
1303 1326
1304 1327 set_wfp_burst_enable_register( mode );
1305 1328
1306 1329 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER );
1307 1330 LEON_Unmask_interrupt( IRQ_WAVEFORM_PICKER );
1308 1331
1309 1332 if (transitionCoarseTime == 0)
1310 1333 {
1311 1334 // instant transition means transition on the next valid date
1312 1335 // this is mandatory to have a good snapshot period and a good correction of the snapshot period
1313 1336 waveform_picker_regs->start_date = time_management_regs->coarse_time + 1;
1314 1337 }
1315 1338 else
1316 1339 {
1317 1340 waveform_picker_regs->start_date = transitionCoarseTime;
1318 1341 }
1319 1342
1320 1343 update_last_valid_transition_date(waveform_picker_regs->start_date);
1321 1344
1322 1345 }
1323 1346
1324 1347 void launch_spectral_matrix( void )
1325 1348 {
1326 1349 SM_reset_current_ring_nodes();
1327 1350
1328 1351 reset_spectral_matrix_regs();
1329 1352
1330 1353 reset_nb_sm();
1331 1354
1332 1355 set_sm_irq_onNewMatrix( 1 );
1333 1356
1334 1357 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX );
1335 1358 LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRIX );
1336 1359
1337 1360 }
1338 1361
1339 1362 void set_sm_irq_onNewMatrix( unsigned char value )
1340 1363 {
1341 1364 if (value == 1)
1342 1365 {
1343 1366 spectral_matrix_regs->config = spectral_matrix_regs->config | BIT_IRQ_ON_NEW_MATRIX;
1344 1367 }
1345 1368 else
1346 1369 {
1347 1370 spectral_matrix_regs->config = spectral_matrix_regs->config & MASK_IRQ_ON_NEW_MATRIX; // 1110
1348 1371 }
1349 1372 }
1350 1373
1351 1374 void set_sm_irq_onError( unsigned char value )
1352 1375 {
1353 1376 if (value == 1)
1354 1377 {
1355 1378 spectral_matrix_regs->config = spectral_matrix_regs->config | BIT_IRQ_ON_ERROR;
1356 1379 }
1357 1380 else
1358 1381 {
1359 1382 spectral_matrix_regs->config = spectral_matrix_regs->config & MASK_IRQ_ON_ERROR; // 1101
1360 1383 }
1361 1384 }
1362 1385
1363 1386 //*****************************
1364 1387 // CONFIGURE CALIBRATION SIGNAL
1365 1388 void setCalibrationPrescaler( unsigned int prescaler )
1366 1389 {
1367 1390 // prescaling of the master clock (25 MHz)
1368 1391 // master clock is divided by 2^prescaler
1369 1392 time_management_regs->calPrescaler = prescaler;
1370 1393 }
1371 1394
1372 1395 void setCalibrationDivisor( unsigned int divisionFactor )
1373 1396 {
1374 1397 // division of the prescaled clock by the division factor
1375 1398 time_management_regs->calDivisor = divisionFactor;
1376 1399 }
1377 1400
1378 1401 void setCalibrationData( void )
1379 1402 {
1380 1403 /** This function is used to store the values used to drive the DAC in order to generate the SCM calibration signal
1381 1404 *
1382 1405 * @param void
1383 1406 *
1384 1407 * @return void
1385 1408 *
1386 1409 */
1387 1410
1388 1411 unsigned int k;
1389 1412 unsigned short data;
1390 1413 float val;
1391 1414 float Ts;
1392 1415
1393 1416 time_management_regs->calDataPtr = INIT_CHAR;
1394 1417
1395 1418 Ts = 1 / CAL_FS;
1396 1419
1397 1420 // build the signal for the SCM calibration
1398 1421 for (k = 0; k < CAL_NB_PTS; k++)
1399 1422 {
1400 1423 val = CAL_A0 * sin( CAL_W0 * k * Ts )
1401 1424 + CAL_A1 * sin( CAL_W1 * k * Ts );
1402 1425 data = (unsigned short) ((val * CAL_SCALE_FACTOR) + CONST_2048);
1403 1426 time_management_regs->calData = data & CAL_DATA_MASK;
1404 1427 }
1405 1428 }
1406 1429
1407 1430 void setCalibrationDataInterleaved( void )
1408 1431 {
1409 1432 /** This function is used to store the values used to drive the DAC in order to generate the SCM calibration signal
1410 1433 *
1411 1434 * @param void
1412 1435 *
1413 1436 * @return void
1414 1437 *
1415 1438 * In interleaved mode, one can store more values than in normal mode.
1416 1439 * The data are stored in bunch of 18 bits, 12 bits from one sample and 6 bits from another sample.
1417 1440 * T store 3 values, one need two write operations.
1418 1441 * s1 [ b11 b10 b9 b8 b7 b6 ] s0 [ b11 b10 b9 b8 b7 b6 b5 b3 b2 b1 b0 ]
1419 1442 * s1 [ b5 b4 b3 b2 b1 b0 ] s2 [ b11 b10 b9 b8 b7 b6 b5 b3 b2 b1 b0 ]
1420 1443 *
1421 1444 */
1422 1445
1423 1446 unsigned int k;
1424 1447 float val;
1425 1448 float Ts;
1426 1449 unsigned short data[CAL_NB_PTS_INTER];
1427 1450 unsigned char *dataPtr;
1428 1451
1429 1452 Ts = 1 / CAL_FS_INTER;
1430 1453
1431 1454 time_management_regs->calDataPtr = INIT_CHAR;
1432 1455
1433 1456 // build the signal for the SCM calibration
1434 1457 for (k=0; k<CAL_NB_PTS_INTER; k++)
1435 1458 {
1436 1459 val = sin( 2 * pi * CAL_F0 * k * Ts )
1437 1460 + sin( 2 * pi * CAL_F1 * k * Ts );
1438 1461 data[k] = (unsigned short) ((val * CONST_512) + CONST_2048);
1439 1462 }
1440 1463
1441 1464 // write the signal in interleaved mode
1442 1465 for (k=0; k < STEPS_FOR_STORAGE_INTER; k++)
1443 1466 {
1444 1467 dataPtr = (unsigned char*) &data[ (k * BYTES_FOR_2_SAMPLES) + 2 ];
1445 1468 time_management_regs->calData = ( data[ k * BYTES_FOR_2_SAMPLES ] & CAL_DATA_MASK )
1446 1469 + ( (dataPtr[0] & CAL_DATA_MASK_INTER) << CAL_DATA_SHIFT_INTER);
1447 1470 time_management_regs->calData = ( data[(k * BYTES_FOR_2_SAMPLES) + 1] & CAL_DATA_MASK )
1448 1471 + ( (dataPtr[1] & CAL_DATA_MASK_INTER) << CAL_DATA_SHIFT_INTER);
1449 1472 }
1450 1473 }
1451 1474
1452 1475 void setCalibrationReload( bool state)
1453 1476 {
1454 1477 if (state == true)
1455 1478 {
1456 1479 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | BIT_CAL_RELOAD; // [0001 0000]
1457 1480 }
1458 1481 else
1459 1482 {
1460 1483 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & MASK_CAL_RELOAD; // [1110 1111]
1461 1484 }
1462 1485 }
1463 1486
1464 1487 void setCalibrationEnable( bool state )
1465 1488 {
1466 1489 // this bit drives the multiplexer
1467 1490 if (state == true)
1468 1491 {
1469 1492 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | BIT_CAL_ENABLE; // [0100 0000]
1470 1493 }
1471 1494 else
1472 1495 {
1473 1496 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & MASK_CAL_ENABLE; // [1011 1111]
1474 1497 }
1475 1498 }
1476 1499
1477 1500 void setCalibrationInterleaved( bool state )
1478 1501 {
1479 1502 // this bit drives the multiplexer
1480 1503 if (state == true)
1481 1504 {
1482 1505 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | BIT_SET_INTERLEAVED; // [0010 0000]
1483 1506 }
1484 1507 else
1485 1508 {
1486 1509 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & MASK_SET_INTERLEAVED; // [1101 1111]
1487 1510 }
1488 1511 }
1489 1512
1490 1513 void setCalibration( bool state )
1491 1514 {
1492 1515 if (state == true)
1493 1516 {
1494 1517 setCalibrationEnable( true );
1495 1518 setCalibrationReload( false );
1496 1519 set_hk_lfr_calib_enable( true );
1497 1520 }
1498 1521 else
1499 1522 {
1500 1523 setCalibrationEnable( false );
1501 1524 setCalibrationReload( true );
1502 1525 set_hk_lfr_calib_enable( false );
1503 1526 }
1504 1527 }
1505 1528
1506 1529 void configureCalibration( bool interleaved )
1507 1530 {
1508 1531 setCalibration( false );
1509 1532 if ( interleaved == true )
1510 1533 {
1511 1534 setCalibrationInterleaved( true );
1512 1535 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1513 1536 setCalibrationDivisor( CAL_F_DIVISOR_INTER ); // => 240 384
1514 1537 setCalibrationDataInterleaved();
1515 1538 }
1516 1539 else
1517 1540 {
1518 1541 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1519 1542 setCalibrationDivisor( CAL_F_DIVISOR ); // => 160 256 (39 - 1)
1520 1543 setCalibrationData();
1521 1544 }
1522 1545 }
1523 1546
1524 1547 //****************
1525 1548 // CLOSING ACTIONS
1526 1549 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC, unsigned char * time )
1527 1550 {
1528 1551 /** This function is used to update the HK packets statistics after a successful TC execution.
1529 1552 *
1530 1553 * @param TC points to the TC being processed
1531 1554 * @param time is the time used to date the TC execution
1532 1555 *
1533 1556 */
1534 1557
1535 1558 unsigned int val;
1536 1559
1537 1560 housekeeping_packet.hk_lfr_last_exe_tc_id[0] = TC->packetID[0];
1538 1561 housekeeping_packet.hk_lfr_last_exe_tc_id[1] = TC->packetID[1];
1539 1562 housekeeping_packet.hk_lfr_last_exe_tc_type[0] = INIT_CHAR;
1540 1563 housekeeping_packet.hk_lfr_last_exe_tc_type[1] = TC->serviceType;
1541 1564 housekeeping_packet.hk_lfr_last_exe_tc_subtype[0] = INIT_CHAR;
1542 1565 housekeeping_packet.hk_lfr_last_exe_tc_subtype[1] = TC->serviceSubType;
1543 1566 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_0] = time[BYTE_0];
1544 1567 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_1] = time[BYTE_1];
1545 1568 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_2] = time[BYTE_2];
1546 1569 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_3] = time[BYTE_3];
1547 1570 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_4] = time[BYTE_4];
1548 1571 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_5] = time[BYTE_5];
1549 1572
1550 1573 val = (housekeeping_packet.hk_lfr_exe_tc_cnt[0] * CONST_256) + housekeeping_packet.hk_lfr_exe_tc_cnt[1];
1551 1574 val++;
1552 1575 housekeeping_packet.hk_lfr_exe_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
1553 1576 housekeeping_packet.hk_lfr_exe_tc_cnt[1] = (unsigned char) (val);
1554 1577 }
1555 1578
1556 1579 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC, unsigned char * time )
1557 1580 {
1558 1581 /** This function is used to update the HK packets statistics after a TC rejection.
1559 1582 *
1560 1583 * @param TC points to the TC being processed
1561 1584 * @param time is the time used to date the TC rejection
1562 1585 *
1563 1586 */
1564 1587
1565 1588 unsigned int val;
1566 1589
1567 1590 housekeeping_packet.hk_lfr_last_rej_tc_id[0] = TC->packetID[0];
1568 1591 housekeeping_packet.hk_lfr_last_rej_tc_id[1] = TC->packetID[1];
1569 1592 housekeeping_packet.hk_lfr_last_rej_tc_type[0] = INIT_CHAR;
1570 1593 housekeeping_packet.hk_lfr_last_rej_tc_type[1] = TC->serviceType;
1571 1594 housekeeping_packet.hk_lfr_last_rej_tc_subtype[0] = INIT_CHAR;
1572 1595 housekeeping_packet.hk_lfr_last_rej_tc_subtype[1] = TC->serviceSubType;
1573 1596 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_0] = time[BYTE_0];
1574 1597 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_1] = time[BYTE_1];
1575 1598 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_2] = time[BYTE_2];
1576 1599 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_3] = time[BYTE_3];
1577 1600 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_4] = time[BYTE_4];
1578 1601 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_5] = time[BYTE_5];
1579 1602
1580 1603 val = (housekeeping_packet.hk_lfr_rej_tc_cnt[0] * CONST_256) + housekeeping_packet.hk_lfr_rej_tc_cnt[1];
1581 1604 val++;
1582 1605 housekeeping_packet.hk_lfr_rej_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
1583 1606 housekeeping_packet.hk_lfr_rej_tc_cnt[1] = (unsigned char) (val);
1584 1607 }
1585 1608
1586 1609 void close_action(ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id )
1587 1610 {
1588 1611 /** This function is the last step of the TC execution workflow.
1589 1612 *
1590 1613 * @param TC points to the TC being processed
1591 1614 * @param result is the result of the TC execution (LFR_SUCCESSFUL / LFR_DEFAULT)
1592 1615 * @param queue_id is the id of the RTEMS message queue used to send TM packets
1593 1616 * @param time is the time used to date the TC execution
1594 1617 *
1595 1618 */
1596 1619
1597 1620 unsigned char requestedMode;
1598 1621
1599 1622 if (result == LFR_SUCCESSFUL)
1600 1623 {
1601 1624 if ( !( (TC->serviceType==TC_TYPE_TIME) & (TC->serviceSubType==TC_SUBTYPE_UPDT_TIME) )
1602 1625 &
1603 1626 !( (TC->serviceType==TC_TYPE_GEN) & (TC->serviceSubType==TC_SUBTYPE_UPDT_INFO))
1604 1627 )
1605 1628 {
1606 1629 send_tm_lfr_tc_exe_success( TC, queue_id );
1607 1630 }
1608 1631 if ( (TC->serviceType == TC_TYPE_GEN) & (TC->serviceSubType == TC_SUBTYPE_ENTER) )
1609 1632 {
1610 1633 //**********************************
1611 1634 // UPDATE THE LFRMODE LOCAL VARIABLE
1612 1635 requestedMode = TC->dataAndCRC[1];
1613 1636 updateLFRCurrentMode( requestedMode );
1614 1637 }
1615 1638 }
1616 1639 else if (result == LFR_EXE_ERROR)
1617 1640 {
1618 1641 send_tm_lfr_tc_exe_error( TC, queue_id );
1619 1642 }
1620 1643 }
1621 1644
1622 1645 //***************************
1623 1646 // Interrupt Service Routines
1624 1647 rtems_isr commutation_isr1( rtems_vector_number vector )
1625 1648 {
1626 1649 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1627 1650 PRINTF("In commutation_isr1 *** Error sending event to DUMB\n")
1628 1651 }
1629 1652 }
1630 1653
1631 1654 rtems_isr commutation_isr2( rtems_vector_number vector )
1632 1655 {
1633 1656 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1634 1657 PRINTF("In commutation_isr2 *** Error sending event to DUMB\n")
1635 1658 }
1636 1659 }
1637 1660
1638 1661 //****************
1639 1662 // OTHER FUNCTIONS
1640 1663 void updateLFRCurrentMode( unsigned char requestedMode )
1641 1664 {
1642 1665 /** This function updates the value of the global variable lfrCurrentMode.
1643 1666 *
1644 1667 * lfrCurrentMode is a parameter used by several functions to know in which mode LFR is running.
1645 1668 *
1646 1669 */
1647 1670
1648 1671 // update the local value of lfrCurrentMode with the value contained in the housekeeping_packet structure
1649 1672 housekeeping_packet.lfr_status_word[0] = (housekeeping_packet.lfr_status_word[0] & STATUS_WORD_LFR_MODE_MASK)
1650 1673 + (unsigned char) ( requestedMode << STATUS_WORD_LFR_MODE_SHIFT );
1651 1674 lfrCurrentMode = requestedMode;
1652 1675 }
1653 1676
1654 1677 void set_lfr_soft_reset( unsigned char value )
1655 1678 {
1656 1679 if (value == 1)
1657 1680 {
1658 1681 time_management_regs->ctrl = time_management_regs->ctrl | BIT_SOFT_RESET; // [0100]
1659 1682 }
1660 1683 else
1661 1684 {
1662 1685 time_management_regs->ctrl = time_management_regs->ctrl & MASK_SOFT_RESET; // [1011]
1663 1686 }
1664 1687 }
1665 1688
1666 1689 void reset_lfr( void )
1667 1690 {
1668 1691 set_lfr_soft_reset( 1 );
1669 1692
1670 1693 set_lfr_soft_reset( 0 );
1671 1694
1672 1695 set_hk_lfr_sc_potential_flag( true );
1673 1696 }
Show file before | Show file after | Hide comments
@@ -1,2064 +1,2088
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** Functions to load and dump parameters in the LFR registers.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * A group of functions to handle TC related to parameter loading and dumping.\n
7 31 * TC_LFR_LOAD_COMMON_PAR\n
8 32 * TC_LFR_LOAD_NORMAL_PAR\n
9 33 * TC_LFR_LOAD_BURST_PAR\n
10 34 * TC_LFR_LOAD_SBM1_PAR\n
11 35 * TC_LFR_LOAD_SBM2_PAR\n
12 36 *
13 37 */
14 38
15 39 #include "tc_load_dump_parameters.h"
16 40
17 41 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_1 = {0};
18 42 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2 = {0};
19 43 ring_node kcoefficient_node_1 = {0};
20 44 ring_node kcoefficient_node_2 = {0};
21 45
22 46 int action_load_common_par(ccsdsTelecommandPacket_t *TC)
23 47 {
24 48 /** This function updates the LFR registers with the incoming common parameters.
25 49 *
26 50 * @param TC points to the TeleCommand packet that is being processed
27 51 *
28 52 *
29 53 */
30 54
31 55 parameter_dump_packet.sy_lfr_common_parameters_spare = TC->dataAndCRC[0];
32 56 parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
33 57 set_wfp_data_shaping( );
34 58 return LFR_SUCCESSFUL;
35 59 }
36 60
37 61 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
38 62 {
39 63 /** This function updates the LFR registers with the incoming normal parameters.
40 64 *
41 65 * @param TC points to the TeleCommand packet that is being processed
42 66 * @param queue_id is the id of the queue which handles TM related to this execution step
43 67 *
44 68 */
45 69
46 70 int result;
47 71 int flag;
48 72 rtems_status_code status;
49 73
50 74 flag = LFR_SUCCESSFUL;
51 75
52 76 if ( (lfrCurrentMode == LFR_MODE_NORMAL) ||
53 77 (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) {
54 78 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
55 79 flag = LFR_DEFAULT;
56 80 }
57 81
58 82 // CHECK THE PARAMETERS SET CONSISTENCY
59 83 if (flag == LFR_SUCCESSFUL)
60 84 {
61 85 flag = check_normal_par_consistency( TC, queue_id );
62 86 }
63 87
64 88 // SET THE PARAMETERS IF THEY ARE CONSISTENT
65 89 if (flag == LFR_SUCCESSFUL)
66 90 {
67 91 result = set_sy_lfr_n_swf_l( TC );
68 92 result = set_sy_lfr_n_swf_p( TC );
69 93 result = set_sy_lfr_n_bp_p0( TC );
70 94 result = set_sy_lfr_n_bp_p1( TC );
71 95 result = set_sy_lfr_n_asm_p( TC );
72 96 result = set_sy_lfr_n_cwf_long_f3( TC );
73 97 }
74 98
75 99 return flag;
76 100 }
77 101
78 102 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
79 103 {
80 104 /** This function updates the LFR registers with the incoming burst parameters.
81 105 *
82 106 * @param TC points to the TeleCommand packet that is being processed
83 107 * @param queue_id is the id of the queue which handles TM related to this execution step
84 108 *
85 109 */
86 110
87 111 int flag;
88 112 rtems_status_code status;
89 113 unsigned char sy_lfr_b_bp_p0;
90 114 unsigned char sy_lfr_b_bp_p1;
91 115 float aux;
92 116
93 117 flag = LFR_SUCCESSFUL;
94 118
95 119 if ( lfrCurrentMode == LFR_MODE_BURST ) {
96 120 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
97 121 flag = LFR_DEFAULT;
98 122 }
99 123
100 124 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
101 125 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
102 126
103 127 // sy_lfr_b_bp_p0 shall not be lower than its default value
104 128 if (flag == LFR_SUCCESSFUL)
105 129 {
106 130 if (sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0 )
107 131 {
108 132 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0 + DATAFIELD_OFFSET, sy_lfr_b_bp_p0 );
109 133 flag = WRONG_APP_DATA;
110 134 }
111 135 }
112 136 // sy_lfr_b_bp_p1 shall not be lower than its default value
113 137 if (flag == LFR_SUCCESSFUL)
114 138 {
115 139 if (sy_lfr_b_bp_p1 < DEFAULT_SY_LFR_B_BP_P1 )
116 140 {
117 141 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P1 + DATAFIELD_OFFSET, sy_lfr_b_bp_p1 );
118 142 flag = WRONG_APP_DATA;
119 143 }
120 144 }
121 145 //****************************************************************
122 146 // check the consistency between sy_lfr_b_bp_p0 and sy_lfr_b_bp_p1
123 147 if (flag == LFR_SUCCESSFUL)
124 148 {
125 149 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
126 150 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
127 151 aux = ( (float ) sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0 ) - floor(sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0);
128 152 if (aux > FLOAT_EQUAL_ZERO)
129 153 {
130 154 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0 + DATAFIELD_OFFSET, sy_lfr_b_bp_p0 );
131 155 flag = LFR_DEFAULT;
132 156 }
133 157 }
134 158
135 159 // SET THE PARAMETERS
136 160 if (flag == LFR_SUCCESSFUL)
137 161 {
138 162 flag = set_sy_lfr_b_bp_p0( TC );
139 163 flag = set_sy_lfr_b_bp_p1( TC );
140 164 }
141 165
142 166 return flag;
143 167 }
144 168
145 169 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
146 170 {
147 171 /** This function updates the LFR registers with the incoming sbm1 parameters.
148 172 *
149 173 * @param TC points to the TeleCommand packet that is being processed
150 174 * @param queue_id is the id of the queue which handles TM related to this execution step
151 175 *
152 176 */
153 177
154 178 int flag;
155 179 rtems_status_code status;
156 180 unsigned char sy_lfr_s1_bp_p0;
157 181 unsigned char sy_lfr_s1_bp_p1;
158 182 float aux;
159 183
160 184 flag = LFR_SUCCESSFUL;
161 185
162 186 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
163 187 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
164 188 flag = LFR_DEFAULT;
165 189 }
166 190
167 191 sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
168 192 sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
169 193
170 194 // sy_lfr_s1_bp_p0
171 195 if (flag == LFR_SUCCESSFUL)
172 196 {
173 197 if (sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0 )
174 198 {
175 199 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p0 );
176 200 flag = WRONG_APP_DATA;
177 201 }
178 202 }
179 203 // sy_lfr_s1_bp_p1
180 204 if (flag == LFR_SUCCESSFUL)
181 205 {
182 206 if (sy_lfr_s1_bp_p1 < DEFAULT_SY_LFR_S1_BP_P1 )
183 207 {
184 208 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P1 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p1 );
185 209 flag = WRONG_APP_DATA;
186 210 }
187 211 }
188 212 //******************************************************************
189 213 // check the consistency between sy_lfr_s1_bp_p0 and sy_lfr_s1_bp_p1
190 214 if (flag == LFR_SUCCESSFUL)
191 215 {
192 216 aux = ( (float ) sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE) )
193 217 - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE));
194 218 if (aux > FLOAT_EQUAL_ZERO)
195 219 {
196 220 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p0 );
197 221 flag = LFR_DEFAULT;
198 222 }
199 223 }
200 224
201 225 // SET THE PARAMETERS
202 226 if (flag == LFR_SUCCESSFUL)
203 227 {
204 228 flag = set_sy_lfr_s1_bp_p0( TC );
205 229 flag = set_sy_lfr_s1_bp_p1( TC );
206 230 }
207 231
208 232 return flag;
209 233 }
210 234
211 235 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
212 236 {
213 237 /** This function updates the LFR registers with the incoming sbm2 parameters.
214 238 *
215 239 * @param TC points to the TeleCommand packet that is being processed
216 240 * @param queue_id is the id of the queue which handles TM related to this execution step
217 241 *
218 242 */
219 243
220 244 int flag;
221 245 rtems_status_code status;
222 246 unsigned char sy_lfr_s2_bp_p0;
223 247 unsigned char sy_lfr_s2_bp_p1;
224 248 float aux;
225 249
226 250 flag = LFR_SUCCESSFUL;
227 251
228 252 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
229 253 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
230 254 flag = LFR_DEFAULT;
231 255 }
232 256
233 257 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
234 258 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
235 259
236 260 // sy_lfr_s2_bp_p0
237 261 if (flag == LFR_SUCCESSFUL)
238 262 {
239 263 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
240 264 {
241 265 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p0 );
242 266 flag = WRONG_APP_DATA;
243 267 }
244 268 }
245 269 // sy_lfr_s2_bp_p1
246 270 if (flag == LFR_SUCCESSFUL)
247 271 {
248 272 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
249 273 {
250 274 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P1 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p1 );
251 275 flag = WRONG_APP_DATA;
252 276 }
253 277 }
254 278 //******************************************************************
255 279 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
256 280 if (flag == LFR_SUCCESSFUL)
257 281 {
258 282 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
259 283 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
260 284 aux = ( (float ) sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0 ) - floor(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
261 285 if (aux > FLOAT_EQUAL_ZERO)
262 286 {
263 287 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p0 );
264 288 flag = LFR_DEFAULT;
265 289 }
266 290 }
267 291
268 292 // SET THE PARAMETERS
269 293 if (flag == LFR_SUCCESSFUL)
270 294 {
271 295 flag = set_sy_lfr_s2_bp_p0( TC );
272 296 flag = set_sy_lfr_s2_bp_p1( TC );
273 297 }
274 298
275 299 return flag;
276 300 }
277 301
278 302 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
279 303 {
280 304 /** This function updates the LFR registers with the incoming sbm2 parameters.
281 305 *
282 306 * @param TC points to the TeleCommand packet that is being processed
283 307 * @param queue_id is the id of the queue which handles TM related to this execution step
284 308 *
285 309 */
286 310
287 311 int flag;
288 312
289 313 flag = LFR_DEFAULT;
290 314
291 315 flag = set_sy_lfr_kcoeff( TC, queue_id );
292 316
293 317 return flag;
294 318 }
295 319
296 320 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
297 321 {
298 322 /** This function updates the LFR registers with the incoming sbm2 parameters.
299 323 *
300 324 * @param TC points to the TeleCommand packet that is being processed
301 325 * @param queue_id is the id of the queue which handles TM related to this execution step
302 326 *
303 327 */
304 328
305 329 int flag;
306 330
307 331 flag = LFR_DEFAULT;
308 332
309 333 flag = set_sy_lfr_fbins( TC );
310 334
311 335 // once the fbins masks have been stored, they have to be merged with the masks which handle the reaction wheels frequencies filtering
312 336 merge_fbins_masks();
313 337
314 338 return flag;
315 339 }
316 340
317 341 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
318 342 {
319 343 /** This function updates the LFR registers with the incoming sbm2 parameters.
320 344 *
321 345 * @param TC points to the TeleCommand packet that is being processed
322 346 * @param queue_id is the id of the queue which handles TM related to this execution step
323 347 *
324 348 */
325 349
326 350 int flag;
327 351 unsigned char k;
328 352
329 353 flag = LFR_DEFAULT;
330 354 k = INIT_CHAR;
331 355
332 356 flag = check_sy_lfr_filter_parameters( TC, queue_id );
333 357
334 358 if (flag == LFR_SUCCESSFUL)
335 359 {
336 360 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ];
337 361 parameter_dump_packet.sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
338 362 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_0 ];
339 363 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_1 ];
340 364 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_2 ];
341 365 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_3 ];
342 366 parameter_dump_packet.sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
343 367 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_0 ];
344 368 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_1 ];
345 369 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_2 ];
346 370 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_3 ];
347 371 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_0 ];
348 372 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_1 ];
349 373 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_2 ];
350 374 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_3 ];
351 375
352 376 //****************************
353 377 // store PAS filter parameters
354 378
355 379 // sy_lfr_pas_filter_enabled
356 380 filterPar.spare_sy_lfr_pas_filter_enabled = parameter_dump_packet.spare_sy_lfr_pas_filter_enabled;
357 381 set_sy_lfr_pas_filter_enabled( parameter_dump_packet.spare_sy_lfr_pas_filter_enabled & BIT_PAS_FILTER_ENABLED );
358 382
359 383 // sy_lfr_pas_filter_modulus
360 384 filterPar.modulus_in_finetime = ((uint64_t) parameter_dump_packet.sy_lfr_pas_filter_modulus) * CONST_65536;
361 385
362 386 // sy_lfr_pas_filter_tbad
363 387 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_tbad,
364 388 parameter_dump_packet.sy_lfr_pas_filter_tbad );
365 389 filterPar.tbad_in_finetime = (uint64_t) (filterPar.sy_lfr_pas_filter_tbad * CONST_65536);
366 390
367 391 // sy_lfr_pas_filter_offset
368 392 filterPar.offset_in_finetime = ((uint64_t) parameter_dump_packet.sy_lfr_pas_filter_offset) * CONST_65536;
369 393
370 394 // sy_lfr_pas_filter_shift
371 395 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_shift,
372 396 parameter_dump_packet.sy_lfr_pas_filter_shift );
373 397 filterPar.shift_in_finetime = (uint64_t) (filterPar.sy_lfr_pas_filter_shift * CONST_65536);
374 398
375 399 //****************************************************
376 400 // store the parameter sy_lfr_sc_rw_delta_f as a float
377 401 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f,
378 402 parameter_dump_packet.sy_lfr_sc_rw_delta_f );
379 403
380 404 // copy rw.._k.. from the incoming TC to the local parameter_dump_packet
381 405 for (k = 0; k < NB_RW_K_COEFFS * NB_BYTES_PER_RW_K_COEFF; k++)
382 406 {
383 407 parameter_dump_packet.sy_lfr_rw1_k1[k] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_RW1_K1 + k ];
384 408 }
385 409
386 410 //***********************************************
387 411 // store the parameter sy_lfr_rw.._k.. as a float
388 412 // rw1_k
389 413 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k1, parameter_dump_packet.sy_lfr_rw1_k1 );
390 414 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k2, parameter_dump_packet.sy_lfr_rw1_k2 );
391 415 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k3, parameter_dump_packet.sy_lfr_rw1_k3 );
392 416 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k4, parameter_dump_packet.sy_lfr_rw1_k4 );
393 417 // rw2_k
394 418 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k1, parameter_dump_packet.sy_lfr_rw2_k1 );
395 419 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k2, parameter_dump_packet.sy_lfr_rw2_k2 );
396 420 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k3, parameter_dump_packet.sy_lfr_rw2_k3 );
397 421 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k4, parameter_dump_packet.sy_lfr_rw2_k4 );
398 422 // rw3_k
399 423 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k1, parameter_dump_packet.sy_lfr_rw3_k1 );
400 424 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k2, parameter_dump_packet.sy_lfr_rw3_k2 );
401 425 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k3, parameter_dump_packet.sy_lfr_rw3_k3 );
402 426 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k4, parameter_dump_packet.sy_lfr_rw3_k4 );
403 427 // rw4_k
404 428 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k1, parameter_dump_packet.sy_lfr_rw4_k1 );
405 429 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k2, parameter_dump_packet.sy_lfr_rw4_k2 );
406 430 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k3, parameter_dump_packet.sy_lfr_rw4_k3 );
407 431 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k4, parameter_dump_packet.sy_lfr_rw4_k4 );
408 432
409 433 }
410 434
411 435 return flag;
412 436 }
413 437
414 438 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
415 439 {
416 440 /** This function updates the LFR registers with the incoming sbm2 parameters.
417 441 *
418 442 * @param TC points to the TeleCommand packet that is being processed
419 443 * @param queue_id is the id of the queue which handles TM related to this execution step
420 444 *
421 445 */
422 446
423 447 unsigned int address;
424 448 rtems_status_code status;
425 449 unsigned int freq;
426 450 unsigned int bin;
427 451 unsigned int coeff;
428 452 unsigned char *kCoeffPtr;
429 453 unsigned char *kCoeffDumpPtr;
430 454
431 455 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
432 456 // F0 => 11 bins
433 457 // F1 => 13 bins
434 458 // F2 => 12 bins
435 459 // 36 bins to dump in two packets (30 bins max per packet)
436 460
437 461 //*********
438 462 // PACKET 1
439 463 // 11 F0 bins, 13 F1 bins and 6 F2 bins
440 464 kcoefficients_dump_1.destinationID = TC->sourceID;
441 465 increment_seq_counter_destination_id_dump( kcoefficients_dump_1.packetSequenceControl, TC->sourceID );
442 466 for( freq = 0;
443 467 freq < NB_BINS_COMPRESSED_SM_F0;
444 468 freq++ )
445 469 {
446 470 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1] = freq;
447 471 bin = freq;
448 472 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
449 473 {
450 474 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
451 475 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
452 476 ]; // 2 for the kcoeff_frequency
453 477 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
454 478 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
455 479 }
456 480 }
457 481 for( freq = NB_BINS_COMPRESSED_SM_F0;
458 482 freq < ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
459 483 freq++ )
460 484 {
461 485 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = freq;
462 486 bin = freq - NB_BINS_COMPRESSED_SM_F0;
463 487 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
464 488 {
465 489 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
466 490 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
467 491 ]; // 2 for the kcoeff_frequency
468 492 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
469 493 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
470 494 }
471 495 }
472 496 for( freq = ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
473 497 freq < KCOEFF_BLK_NR_PKT1 ;
474 498 freq++ )
475 499 {
476 500 kcoefficients_dump_1.kcoeff_blks[ (freq * KCOEFF_BLK_SIZE) + 1 ] = freq;
477 501 bin = freq - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
478 502 for ( coeff = 0; coeff <NB_K_COEFF_PER_BIN; coeff++ )
479 503 {
480 504 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
481 505 (freq * KCOEFF_BLK_SIZE) + (coeff * NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
482 506 ]; // 2 for the kcoeff_frequency
483 507 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
484 508 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
485 509 }
486 510 }
487 511 kcoefficients_dump_1.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
488 512 kcoefficients_dump_1.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
489 513 kcoefficients_dump_1.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
490 514 kcoefficients_dump_1.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
491 515 kcoefficients_dump_1.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
492 516 kcoefficients_dump_1.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
493 517 // SEND DATA
494 518 kcoefficient_node_1.status = 1;
495 519 address = (unsigned int) &kcoefficient_node_1;
496 520 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
497 521 if (status != RTEMS_SUCCESSFUL) {
498 522 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
499 523 }
500 524
501 525 //********
502 526 // PACKET 2
503 527 // 6 F2 bins
504 528 kcoefficients_dump_2.destinationID = TC->sourceID;
505 529 increment_seq_counter_destination_id_dump( kcoefficients_dump_2.packetSequenceControl, TC->sourceID );
506 530 for( freq = 0;
507 531 freq < KCOEFF_BLK_NR_PKT2;
508 532 freq++ )
509 533 {
510 534 kcoefficients_dump_2.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = KCOEFF_BLK_NR_PKT1 + freq;
511 535 bin = freq + KCOEFF_BLK_NR_PKT2;
512 536 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
513 537 {
514 538 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[
515 539 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ ]; // 2 for the kcoeff_frequency
516 540 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
517 541 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
518 542 }
519 543 }
520 544 kcoefficients_dump_2.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
521 545 kcoefficients_dump_2.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
522 546 kcoefficients_dump_2.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
523 547 kcoefficients_dump_2.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
524 548 kcoefficients_dump_2.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
525 549 kcoefficients_dump_2.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
526 550 // SEND DATA
527 551 kcoefficient_node_2.status = 1;
528 552 address = (unsigned int) &kcoefficient_node_2;
529 553 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
530 554 if (status != RTEMS_SUCCESSFUL) {
531 555 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
532 556 }
533 557
534 558 return status;
535 559 }
536 560
537 561 int action_dump_par( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
538 562 {
539 563 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
540 564 *
541 565 * @param queue_id is the id of the queue which handles TM related to this execution step.
542 566 *
543 567 * @return RTEMS directive status codes:
544 568 * - RTEMS_SUCCESSFUL - message sent successfully
545 569 * - RTEMS_INVALID_ID - invalid queue id
546 570 * - RTEMS_INVALID_SIZE - invalid message size
547 571 * - RTEMS_INVALID_ADDRESS - buffer is NULL
548 572 * - RTEMS_UNSATISFIED - out of message buffers
549 573 * - RTEMS_TOO_MANY - queue s limit has been reached
550 574 *
551 575 */
552 576
553 577 int status;
554 578
555 579 increment_seq_counter_destination_id_dump( parameter_dump_packet.packetSequenceControl, TC->sourceID );
556 580 parameter_dump_packet.destinationID = TC->sourceID;
557 581
558 582 // UPDATE TIME
559 583 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
560 584 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
561 585 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
562 586 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
563 587 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
564 588 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
565 589 // SEND DATA
566 590 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
567 591 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
568 592 if (status != RTEMS_SUCCESSFUL) {
569 593 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
570 594 }
571 595
572 596 return status;
573 597 }
574 598
575 599 //***********************
576 600 // NORMAL MODE PARAMETERS
577 601
578 602 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
579 603 {
580 604 unsigned char msb;
581 605 unsigned char lsb;
582 606 int flag;
583 607 float aux;
584 608 rtems_status_code status;
585 609
586 610 unsigned int sy_lfr_n_swf_l;
587 611 unsigned int sy_lfr_n_swf_p;
588 612 unsigned int sy_lfr_n_asm_p;
589 613 unsigned char sy_lfr_n_bp_p0;
590 614 unsigned char sy_lfr_n_bp_p1;
591 615 unsigned char sy_lfr_n_cwf_long_f3;
592 616
593 617 flag = LFR_SUCCESSFUL;
594 618
595 619 //***************
596 620 // get parameters
597 621 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
598 622 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
599 623 sy_lfr_n_swf_l = (msb * CONST_256) + lsb;
600 624
601 625 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
602 626 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
603 627 sy_lfr_n_swf_p = (msb * CONST_256) + lsb;
604 628
605 629 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
606 630 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
607 631 sy_lfr_n_asm_p = (msb * CONST_256) + lsb;
608 632
609 633 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
610 634
611 635 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
612 636
613 637 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
614 638
615 639 //******************
616 640 // check consistency
617 641 // sy_lfr_n_swf_l
618 642 if (sy_lfr_n_swf_l != DFLT_SY_LFR_N_SWF_L)
619 643 {
620 644 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L + DATAFIELD_OFFSET, sy_lfr_n_swf_l );
621 645 flag = WRONG_APP_DATA;
622 646 }
623 647 // sy_lfr_n_swf_p
624 648 if (flag == LFR_SUCCESSFUL)
625 649 {
626 650 if ( sy_lfr_n_swf_p < MIN_SY_LFR_N_SWF_P )
627 651 {
628 652 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P + DATAFIELD_OFFSET, sy_lfr_n_swf_p );
629 653 flag = WRONG_APP_DATA;
630 654 }
631 655 }
632 656 // sy_lfr_n_bp_p0
633 657 if (flag == LFR_SUCCESSFUL)
634 658 {
635 659 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
636 660 {
637 661 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0 + DATAFIELD_OFFSET, sy_lfr_n_bp_p0 );
638 662 flag = WRONG_APP_DATA;
639 663 }
640 664 }
641 665 // sy_lfr_n_asm_p
642 666 if (flag == LFR_SUCCESSFUL)
643 667 {
644 668 if (sy_lfr_n_asm_p == 0)
645 669 {
646 670 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
647 671 flag = WRONG_APP_DATA;
648 672 }
649 673 }
650 674 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
651 675 if (flag == LFR_SUCCESSFUL)
652 676 {
653 677 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
654 678 if (aux > FLOAT_EQUAL_ZERO)
655 679 {
656 680 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
657 681 flag = WRONG_APP_DATA;
658 682 }
659 683 }
660 684 // sy_lfr_n_bp_p1
661 685 if (flag == LFR_SUCCESSFUL)
662 686 {
663 687 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
664 688 {
665 689 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
666 690 flag = WRONG_APP_DATA;
667 691 }
668 692 }
669 693 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
670 694 if (flag == LFR_SUCCESSFUL)
671 695 {
672 696 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
673 697 if (aux > FLOAT_EQUAL_ZERO)
674 698 {
675 699 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
676 700 flag = LFR_DEFAULT;
677 701 }
678 702 }
679 703 // sy_lfr_n_cwf_long_f3
680 704
681 705 return flag;
682 706 }
683 707
684 708 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
685 709 {
686 710 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
687 711 *
688 712 * @param TC points to the TeleCommand packet that is being processed
689 713 * @param queue_id is the id of the queue which handles TM related to this execution step
690 714 *
691 715 */
692 716
693 717 int result;
694 718
695 719 result = LFR_SUCCESSFUL;
696 720
697 721 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
698 722 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
699 723
700 724 return result;
701 725 }
702 726
703 727 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
704 728 {
705 729 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
706 730 *
707 731 * @param TC points to the TeleCommand packet that is being processed
708 732 * @param queue_id is the id of the queue which handles TM related to this execution step
709 733 *
710 734 */
711 735
712 736 int result;
713 737
714 738 result = LFR_SUCCESSFUL;
715 739
716 740 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
717 741 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
718 742
719 743 return result;
720 744 }
721 745
722 746 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
723 747 {
724 748 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
725 749 *
726 750 * @param TC points to the TeleCommand packet that is being processed
727 751 * @param queue_id is the id of the queue which handles TM related to this execution step
728 752 *
729 753 */
730 754
731 755 int result;
732 756
733 757 result = LFR_SUCCESSFUL;
734 758
735 759 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
736 760 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
737 761
738 762 return result;
739 763 }
740 764
741 765 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
742 766 {
743 767 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
744 768 *
745 769 * @param TC points to the TeleCommand packet that is being processed
746 770 * @param queue_id is the id of the queue which handles TM related to this execution step
747 771 *
748 772 */
749 773
750 774 int status;
751 775
752 776 status = LFR_SUCCESSFUL;
753 777
754 778 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
755 779
756 780 return status;
757 781 }
758 782
759 783 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
760 784 {
761 785 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
762 786 *
763 787 * @param TC points to the TeleCommand packet that is being processed
764 788 * @param queue_id is the id of the queue which handles TM related to this execution step
765 789 *
766 790 */
767 791
768 792 int status;
769 793
770 794 status = LFR_SUCCESSFUL;
771 795
772 796 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
773 797
774 798 return status;
775 799 }
776 800
777 801 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
778 802 {
779 803 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
780 804 *
781 805 * @param TC points to the TeleCommand packet that is being processed
782 806 * @param queue_id is the id of the queue which handles TM related to this execution step
783 807 *
784 808 */
785 809
786 810 int status;
787 811
788 812 status = LFR_SUCCESSFUL;
789 813
790 814 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
791 815
792 816 return status;
793 817 }
794 818
795 819 //**********************
796 820 // BURST MODE PARAMETERS
797 821
798 822 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
799 823 {
800 824 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
801 825 *
802 826 * @param TC points to the TeleCommand packet that is being processed
803 827 * @param queue_id is the id of the queue which handles TM related to this execution step
804 828 *
805 829 */
806 830
807 831 int status;
808 832
809 833 status = LFR_SUCCESSFUL;
810 834
811 835 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
812 836
813 837 return status;
814 838 }
815 839
816 840 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
817 841 {
818 842 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
819 843 *
820 844 * @param TC points to the TeleCommand packet that is being processed
821 845 * @param queue_id is the id of the queue which handles TM related to this execution step
822 846 *
823 847 */
824 848
825 849 int status;
826 850
827 851 status = LFR_SUCCESSFUL;
828 852
829 853 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
830 854
831 855 return status;
832 856 }
833 857
834 858 //*********************
835 859 // SBM1 MODE PARAMETERS
836 860
837 861 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
838 862 {
839 863 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
840 864 *
841 865 * @param TC points to the TeleCommand packet that is being processed
842 866 * @param queue_id is the id of the queue which handles TM related to this execution step
843 867 *
844 868 */
845 869
846 870 int status;
847 871
848 872 status = LFR_SUCCESSFUL;
849 873
850 874 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
851 875
852 876 return status;
853 877 }
854 878
855 879 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
856 880 {
857 881 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
858 882 *
859 883 * @param TC points to the TeleCommand packet that is being processed
860 884 * @param queue_id is the id of the queue which handles TM related to this execution step
861 885 *
862 886 */
863 887
864 888 int status;
865 889
866 890 status = LFR_SUCCESSFUL;
867 891
868 892 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
869 893
870 894 return status;
871 895 }
872 896
873 897 //*********************
874 898 // SBM2 MODE PARAMETERS
875 899
876 900 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC )
877 901 {
878 902 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
879 903 *
880 904 * @param TC points to the TeleCommand packet that is being processed
881 905 * @param queue_id is the id of the queue which handles TM related to this execution step
882 906 *
883 907 */
884 908
885 909 int status;
886 910
887 911 status = LFR_SUCCESSFUL;
888 912
889 913 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
890 914
891 915 return status;
892 916 }
893 917
894 918 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
895 919 {
896 920 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
897 921 *
898 922 * @param TC points to the TeleCommand packet that is being processed
899 923 * @param queue_id is the id of the queue which handles TM related to this execution step
900 924 *
901 925 */
902 926
903 927 int status;
904 928
905 929 status = LFR_SUCCESSFUL;
906 930
907 931 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
908 932
909 933 return status;
910 934 }
911 935
912 936 //*******************
913 937 // TC_LFR_UPDATE_INFO
914 938
915 939 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
916 940 {
917 941 unsigned int status;
918 942
919 943 status = LFR_DEFAULT;
920 944
921 945 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
922 946 || (mode == LFR_MODE_BURST)
923 947 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
924 948 {
925 949 status = LFR_SUCCESSFUL;
926 950 }
927 951 else
928 952 {
929 953 status = LFR_DEFAULT;
930 954 }
931 955
932 956 return status;
933 957 }
934 958
935 959 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
936 960 {
937 961 unsigned int status;
938 962
939 963 status = LFR_DEFAULT;
940 964
941 965 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
942 966 || (mode == TDS_MODE_BURST)
943 967 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
944 968 || (mode == TDS_MODE_LFM))
945 969 {
946 970 status = LFR_SUCCESSFUL;
947 971 }
948 972 else
949 973 {
950 974 status = LFR_DEFAULT;
951 975 }
952 976
953 977 return status;
954 978 }
955 979
956 980 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
957 981 {
958 982 unsigned int status;
959 983
960 984 status = LFR_DEFAULT;
961 985
962 986 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
963 987 || (mode == THR_MODE_BURST))
964 988 {
965 989 status = LFR_SUCCESSFUL;
966 990 }
967 991 else
968 992 {
969 993 status = LFR_DEFAULT;
970 994 }
971 995
972 996 return status;
973 997 }
974 998
975 999 void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value )
976 1000 {
977 1001 unsigned char flag;
978 1002 unsigned char flagPosInByte;
979 1003 unsigned char newFlag;
980 1004 unsigned char flagMask;
981 1005
982 1006 // if the frequency value is not a number, the flag is set to 0 and the frequency RWx_Fy is not filtered
983 1007 if (isnan(value))
984 1008 {
985 1009 flag = FLAG_NAN;
986 1010 }
987 1011 else
988 1012 {
989 1013 flag = FLAG_IAN;
990 1014 }
991 1015
992 1016 switch(wheel)
993 1017 {
994 1018 case WHEEL_1:
995 1019 flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
996 1020 flagMask = ~(1 << flagPosInByte);
997 1021 newFlag = flag << flagPosInByte;
998 1022 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
999 1023 break;
1000 1024 case WHEEL_2:
1001 1025 flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
1002 1026 flagMask = ~(1 << flagPosInByte);
1003 1027 newFlag = flag << flagPosInByte;
1004 1028 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
1005 1029 break;
1006 1030 case WHEEL_3:
1007 1031 flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
1008 1032 flagMask = ~(1 << flagPosInByte);
1009 1033 newFlag = flag << flagPosInByte;
1010 1034 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1011 1035 break;
1012 1036 case WHEEL_4:
1013 1037 flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
1014 1038 flagMask = ~(1 << flagPosInByte);
1015 1039 newFlag = flag << flagPosInByte;
1016 1040 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1017 1041 break;
1018 1042 default:
1019 1043 break;
1020 1044 }
1021 1045 }
1022 1046
1023 1047 void set_hk_lfr_sc_rw_f_flags( void )
1024 1048 {
1025 1049 // RW1
1026 1050 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_1, rw_f.cp_rpw_sc_rw1_f1 );
1027 1051 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_2, rw_f.cp_rpw_sc_rw1_f2 );
1028 1052 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_3, rw_f.cp_rpw_sc_rw1_f3 );
1029 1053 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_4, rw_f.cp_rpw_sc_rw1_f4 );
1030 1054
1031 1055 // RW2
1032 1056 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_1, rw_f.cp_rpw_sc_rw2_f1 );
1033 1057 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_2, rw_f.cp_rpw_sc_rw2_f2 );
1034 1058 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_3, rw_f.cp_rpw_sc_rw2_f3 );
1035 1059 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_4, rw_f.cp_rpw_sc_rw2_f4 );
1036 1060
1037 1061 // RW3
1038 1062 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_1, rw_f.cp_rpw_sc_rw3_f1 );
1039 1063 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_2, rw_f.cp_rpw_sc_rw3_f2 );
1040 1064 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_3, rw_f.cp_rpw_sc_rw3_f3 );
1041 1065 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_4, rw_f.cp_rpw_sc_rw3_f4 );
1042 1066
1043 1067 // RW4
1044 1068 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_1, rw_f.cp_rpw_sc_rw4_f1 );
1045 1069 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_2, rw_f.cp_rpw_sc_rw4_f2 );
1046 1070 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_3, rw_f.cp_rpw_sc_rw4_f3 );
1047 1071 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_4, rw_f.cp_rpw_sc_rw4_f4 );
1048 1072 }
1049 1073
1050 1074 int check_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value )
1051 1075 {
1052 1076 float rw_k;
1053 1077 int ret;
1054 1078
1055 1079 ret = LFR_SUCCESSFUL;
1056 1080 rw_k = INIT_FLOAT;
1057 1081
1058 1082 copyFloatByChar( (unsigned char*) &rw_k, (unsigned char*) &TC->packetID[ offset ] );
1059 1083
1060 1084 *pos = offset;
1061 1085 *value = rw_k;
1062 1086
1063 1087 if (rw_k < MIN_SY_LFR_RW_F)
1064 1088 {
1065 1089 ret = WRONG_APP_DATA;
1066 1090 }
1067 1091
1068 1092 return ret;
1069 1093 }
1070 1094
1071 1095 int check_all_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int *pos, float*value )
1072 1096 {
1073 1097 int ret;
1074 1098
1075 1099 ret = LFR_SUCCESSFUL;
1076 1100
1077 1101 //****
1078 1102 //****
1079 1103 // RW1
1080 1104 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1, pos, value ); // F1
1081 1105 if (ret == LFR_SUCCESSFUL) // F2
1082 1106 {
1083 1107 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2, pos, value );
1084 1108 }
1085 1109 if (ret == LFR_SUCCESSFUL) // F3
1086 1110 {
1087 1111 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3, pos, value );
1088 1112 }
1089 1113 if (ret == LFR_SUCCESSFUL) // F4
1090 1114 {
1091 1115 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4, pos, value );
1092 1116 }
1093 1117
1094 1118 //****
1095 1119 //****
1096 1120 // RW2
1097 1121 if (ret == LFR_SUCCESSFUL) // F1
1098 1122 {
1099 1123 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1, pos, value );
1100 1124 }
1101 1125 if (ret == LFR_SUCCESSFUL) // F2
1102 1126 {
1103 1127 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2, pos, value );
1104 1128 }
1105 1129 if (ret == LFR_SUCCESSFUL) // F3
1106 1130 {
1107 1131 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3, pos, value );
1108 1132 }
1109 1133 if (ret == LFR_SUCCESSFUL) // F4
1110 1134 {
1111 1135 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4, pos, value );
1112 1136 }
1113 1137
1114 1138 //****
1115 1139 //****
1116 1140 // RW3
1117 1141 if (ret == LFR_SUCCESSFUL) // F1
1118 1142 {
1119 1143 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1, pos, value );
1120 1144 }
1121 1145 if (ret == LFR_SUCCESSFUL) // F2
1122 1146 {
1123 1147 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2, pos, value );
1124 1148 }
1125 1149 if (ret == LFR_SUCCESSFUL) // F3
1126 1150 {
1127 1151 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3, pos, value );
1128 1152 }
1129 1153 if (ret == LFR_SUCCESSFUL) // F4
1130 1154 {
1131 1155 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4, pos, value );
1132 1156 }
1133 1157
1134 1158 //****
1135 1159 //****
1136 1160 // RW4
1137 1161 if (ret == LFR_SUCCESSFUL) // F1
1138 1162 {
1139 1163 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1, pos, value );
1140 1164 }
1141 1165 if (ret == LFR_SUCCESSFUL) // F2
1142 1166 {
1143 1167 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2, pos, value );
1144 1168 }
1145 1169 if (ret == LFR_SUCCESSFUL) // F3
1146 1170 {
1147 1171 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3, pos, value );
1148 1172 }
1149 1173 if (ret == LFR_SUCCESSFUL) // F4
1150 1174 {
1151 1175 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4, pos, value );
1152 1176 }
1153 1177
1154 1178 return ret;
1155 1179 }
1156 1180
1157 1181 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC )
1158 1182 {
1159 1183 /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally.
1160 1184 *
1161 1185 * @param TC points to the TeleCommand packet that is being processed
1162 1186 *
1163 1187 */
1164 1188
1165 1189 unsigned char * bytePosPtr; // pointer to the beginning of the incoming TC packet
1166 1190
1167 1191 bytePosPtr = (unsigned char *) &TC->packetID;
1168 1192
1169 1193 // rw1_f
1170 1194 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] );
1171 1195 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] );
1172 1196 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3 ] );
1173 1197 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4 ] );
1174 1198
1175 1199 // rw2_f
1176 1200 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] );
1177 1201 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] );
1178 1202 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3 ] );
1179 1203 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4 ] );
1180 1204
1181 1205 // rw3_f
1182 1206 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] );
1183 1207 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] );
1184 1208 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3 ] );
1185 1209 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4 ] );
1186 1210
1187 1211 // rw4_f
1188 1212 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] );
1189 1213 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] );
1190 1214 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3 ] );
1191 1215 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4 ] );
1192 1216
1193 1217 // test each reaction wheel frequency value. NaN means that the frequency is not filtered
1194 1218
1195 1219 }
1196 1220
1197 1221 void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, float sy_lfr_rw_k )
1198 1222 {
1199 1223 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
1200 1224 *
1201 1225 * @param fbins_mask
1202 1226 * @param rw_f is the reaction wheel frequency to filter
1203 1227 * @param delta_f is the frequency step between the frequency bins, it depends on the frequency channel
1204 1228 * @param flag [true] filtering enabled [false] filtering disabled
1205 1229 *
1206 1230 * @return void
1207 1231 *
1208 1232 */
1209 1233
1210 1234 float f_RW_min;
1211 1235 float f_RW_MAX;
1212 1236 float fi_min;
1213 1237 float fi_MAX;
1214 1238 float fi;
1215 1239 float deltaBelow;
1216 1240 float deltaAbove;
1217 1241 float freqToFilterOut;
1218 1242 int binBelow;
1219 1243 int binAbove;
1220 1244 int closestBin;
1221 1245 unsigned int whichByte;
1222 1246 int selectedByte;
1223 1247 int bin;
1224 1248 int binToRemove[NB_BINS_TO_REMOVE];
1225 1249 int k;
1226 1250 bool filteringSet;
1227 1251
1228 1252 closestBin = 0;
1229 1253 whichByte = 0;
1230 1254 bin = 0;
1231 1255 filteringSet = false;
1232 1256
1233 1257 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1234 1258 {
1235 1259 binToRemove[k] = -1;
1236 1260 }
1237 1261
1238 1262 if (!isnan(rw_f))
1239 1263 {
1240 1264 // compute the frequency range to filter [ rw_f - delta_f; rw_f + delta_f ]
1241 1265 f_RW_min = rw_f - ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
1242 1266 f_RW_MAX = rw_f + ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
1243 1267
1244 1268 freqToFilterOut = f_RW_min;
1245 1269 while ( filteringSet == false )
1246 1270 {
1247 1271 // compute the index of the frequency bin immediately below rw_f
1248 1272 binBelow = (int) ( floor( ((double) freqToFilterOut) / ((double) deltaFreq)) );
1249 1273 deltaBelow = freqToFilterOut - binBelow * deltaFreq;
1250 1274
1251 1275 // compute the index of the frequency bin immediately above rw_f
1252 1276 binAbove = (int) ( ceil( ((double) freqToFilterOut) / ((double) deltaFreq)) );
1253 1277 deltaAbove = binAbove * deltaFreq - freqToFilterOut;
1254 1278
1255 1279 // search the closest bin
1256 1280 if (deltaAbove > deltaBelow)
1257 1281 {
1258 1282 closestBin = binBelow;
1259 1283 }
1260 1284 else
1261 1285 {
1262 1286 closestBin = binAbove;
1263 1287 }
1264 1288
1265 1289 // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285]
1266 1290 fi = closestBin * deltaFreq;
1267 1291 fi_min = fi - (deltaFreq * FI_INTERVAL_COEFF);
1268 1292 fi_MAX = fi + (deltaFreq * FI_INTERVAL_COEFF);
1269 1293
1270 1294 //**************************************************************************************
1271 1295 // be careful here, one shall take into account that the bin 0 IS DROPPED in the spectra
1272 1296 // thus, the index 0 in a mask corresponds to the bin 1 of the spectrum
1273 1297 //**************************************************************************************
1274 1298
1275 1299 // 1. IF freqToFilterOut is included in [ fi_min; fi_MAX ]
1276 1300 // => remove f_(i), f_(i-1) and f_(i+1)
1277 1301 if ( ( freqToFilterOut > fi_min ) && ( freqToFilterOut < fi_MAX ) )
1278 1302 {
1279 1303 binToRemove[0] = (closestBin - 1) - 1;
1280 1304 binToRemove[1] = (closestBin) - 1;
1281 1305 binToRemove[2] = (closestBin + 1) - 1;
1282 1306 }
1283 1307 // 2. ELSE
1284 1308 // => remove the two f_(i) which are around f_RW
1285 1309 else
1286 1310 {
1287 1311 binToRemove[0] = (binBelow) - 1;
1288 1312 binToRemove[1] = (binAbove) - 1;
1289 1313 binToRemove[2] = (-1);
1290 1314 }
1291 1315
1292 1316 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1293 1317 {
1294 1318 bin = binToRemove[k];
1295 1319 if ( (bin >= BIN_MIN) && (bin <= BIN_MAX) )
1296 1320 {
1297 1321 whichByte = (bin >> SHIFT_3_BITS); // division by 8
1298 1322 selectedByte = ( 1 << (bin - (whichByte * BITS_PER_BYTE)) );
1299 1323 fbins_mask[BYTES_PER_MASK - 1 - whichByte] =
1300 1324 fbins_mask[BYTES_PER_MASK - 1 - whichByte] & ((unsigned char) (~selectedByte)); // bytes are ordered MSB first in the packets
1301 1325
1302 1326 }
1303 1327 }
1304 1328
1305 1329 // update freqToFilterOut
1306 1330 if ( freqToFilterOut == f_RW_MAX )
1307 1331 {
1308 1332 filteringSet = true; // end of the loop
1309 1333 }
1310 1334 else
1311 1335 {
1312 1336 freqToFilterOut = freqToFilterOut + deltaFreq;
1313 1337 }
1314 1338
1315 1339 if ( freqToFilterOut > f_RW_MAX)
1316 1340 {
1317 1341 freqToFilterOut = f_RW_MAX;
1318 1342 }
1319 1343 }
1320 1344 }
1321 1345 }
1322 1346
1323 1347 void build_sy_lfr_rw_mask( unsigned int channel )
1324 1348 {
1325 1349 unsigned char local_rw_fbins_mask[BYTES_PER_MASK];
1326 1350 unsigned char *maskPtr;
1327 1351 double deltaF;
1328 1352 unsigned k;
1329 1353
1330 1354 maskPtr = NULL;
1331 1355 deltaF = DELTAF_F2;
1332 1356
1333 1357 switch (channel)
1334 1358 {
1335 1359 case CHANNELF0:
1336 1360 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1337 1361 deltaF = DELTAF_F0;
1338 1362 break;
1339 1363 case CHANNELF1:
1340 1364 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1341 1365 deltaF = DELTAF_F1;
1342 1366 break;
1343 1367 case CHANNELF2:
1344 1368 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1345 1369 deltaF = DELTAF_F2;
1346 1370 break;
1347 1371 default:
1348 1372 break;
1349 1373 }
1350 1374
1351 1375 for (k = 0; k < BYTES_PER_MASK; k++)
1352 1376 {
1353 1377 local_rw_fbins_mask[k] = INT8_ALL_F;
1354 1378 }
1355 1379
1356 1380 // RW1
1357 1381 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, filterPar.sy_lfr_rw1_k1 );
1358 1382 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, filterPar.sy_lfr_rw1_k2 );
1359 1383 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f3, deltaF, filterPar.sy_lfr_rw1_k3 );
1360 1384 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f4, deltaF, filterPar.sy_lfr_rw1_k4 );
1361 1385
1362 1386 // RW2
1363 1387 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, filterPar.sy_lfr_rw2_k1 );
1364 1388 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, filterPar.sy_lfr_rw2_k2 );
1365 1389 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f3, deltaF, filterPar.sy_lfr_rw2_k3 );
1366 1390 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f4, deltaF, filterPar.sy_lfr_rw2_k4 );
1367 1391
1368 1392 // RW3
1369 1393 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, filterPar.sy_lfr_rw3_k1 );
1370 1394 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, filterPar.sy_lfr_rw3_k2 );
1371 1395 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f3, deltaF, filterPar.sy_lfr_rw3_k3 );
1372 1396 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f4, deltaF, filterPar.sy_lfr_rw3_k4 );
1373 1397
1374 1398 // RW4
1375 1399 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, filterPar.sy_lfr_rw4_k1 );
1376 1400 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, filterPar.sy_lfr_rw4_k2 );
1377 1401 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f3, deltaF, filterPar.sy_lfr_rw4_k3 );
1378 1402 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f4, deltaF, filterPar.sy_lfr_rw4_k4 );
1379 1403
1380 1404 // update the value of the fbins related to reaction wheels frequency filtering
1381 1405 if (maskPtr != NULL)
1382 1406 {
1383 1407 for (k = 0; k < BYTES_PER_MASK; k++)
1384 1408 {
1385 1409 maskPtr[k] = local_rw_fbins_mask[k];
1386 1410 }
1387 1411 }
1388 1412 }
1389 1413
1390 1414 void build_sy_lfr_rw_masks( void )
1391 1415 {
1392 1416 build_sy_lfr_rw_mask( CHANNELF0 );
1393 1417 build_sy_lfr_rw_mask( CHANNELF1 );
1394 1418 build_sy_lfr_rw_mask( CHANNELF2 );
1395 1419 }
1396 1420
1397 1421 void merge_fbins_masks( void )
1398 1422 {
1399 1423 unsigned char k;
1400 1424
1401 1425 unsigned char *fbins_f0;
1402 1426 unsigned char *fbins_f1;
1403 1427 unsigned char *fbins_f2;
1404 1428 unsigned char *rw_mask_f0;
1405 1429 unsigned char *rw_mask_f1;
1406 1430 unsigned char *rw_mask_f2;
1407 1431
1408 1432 fbins_f0 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1409 1433 fbins_f1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
1410 1434 fbins_f2 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
1411 1435 rw_mask_f0 = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1412 1436 rw_mask_f1 = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1413 1437 rw_mask_f2 = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1414 1438
1415 1439 for( k=0; k < BYTES_PER_MASK; k++ )
1416 1440 {
1417 1441 fbins_masks.merged_fbins_mask_f0[k] = fbins_f0[k] & rw_mask_f0[k];
1418 1442 fbins_masks.merged_fbins_mask_f1[k] = fbins_f1[k] & rw_mask_f1[k];
1419 1443 fbins_masks.merged_fbins_mask_f2[k] = fbins_f2[k] & rw_mask_f2[k];
1420 1444 }
1421 1445 }
1422 1446
1423 1447 //***********
1424 1448 // FBINS MASK
1425 1449
1426 1450 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
1427 1451 {
1428 1452 int status;
1429 1453 unsigned int k;
1430 1454 unsigned char *fbins_mask_dump;
1431 1455 unsigned char *fbins_mask_TC;
1432 1456
1433 1457 status = LFR_SUCCESSFUL;
1434 1458
1435 1459 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1436 1460 fbins_mask_TC = TC->dataAndCRC;
1437 1461
1438 1462 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1439 1463 {
1440 1464 fbins_mask_dump[k] = fbins_mask_TC[k];
1441 1465 }
1442 1466
1443 1467 return status;
1444 1468 }
1445 1469
1446 1470 //***************************
1447 1471 // TC_LFR_LOAD_PAS_FILTER_PAR
1448 1472
1449 1473 int check_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value )
1450 1474 {
1451 1475 float rw_k;
1452 1476 int ret;
1453 1477
1454 1478 ret = LFR_SUCCESSFUL;
1455 1479 rw_k = INIT_FLOAT;
1456 1480
1457 1481 copyFloatByChar( (unsigned char*) &rw_k, (unsigned char*) &TC->dataAndCRC[ offset ] );
1458 1482
1459 1483 *pos = offset;
1460 1484 *value = rw_k;
1461 1485
1462 1486 if (rw_k < MIN_SY_LFR_RW_F)
1463 1487 {
1464 1488 ret = WRONG_APP_DATA;
1465 1489 }
1466 1490
1467 1491 return ret;
1468 1492 }
1469 1493
1470 1494 int check_all_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int *pos, float *value )
1471 1495 {
1472 1496 int ret;
1473 1497
1474 1498 ret = LFR_SUCCESSFUL;
1475 1499
1476 1500 //****
1477 1501 //****
1478 1502 // RW1
1479 1503 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K1, pos, value ); // K1
1480 1504 if (ret == LFR_SUCCESSFUL) // K2
1481 1505 {
1482 1506 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K2, pos, value );
1483 1507 }
1484 1508 if (ret == LFR_SUCCESSFUL) // K3
1485 1509 {
1486 1510 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K3, pos, value );
1487 1511 }
1488 1512 if (ret == LFR_SUCCESSFUL) // K4
1489 1513 {
1490 1514 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K4, pos, value );
1491 1515 }
1492 1516
1493 1517 //****
1494 1518 //****
1495 1519 // RW2
1496 1520 if (ret == LFR_SUCCESSFUL) // K1
1497 1521 {
1498 1522 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K1, pos, value );
1499 1523 }
1500 1524 if (ret == LFR_SUCCESSFUL) // K2
1501 1525 {
1502 1526 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K2, pos, value );
1503 1527 }
1504 1528 if (ret == LFR_SUCCESSFUL) // K3
1505 1529 {
1506 1530 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K3, pos, value );
1507 1531 }
1508 1532 if (ret == LFR_SUCCESSFUL) // K4
1509 1533 {
1510 1534 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K4, pos, value );
1511 1535 }
1512 1536
1513 1537 //****
1514 1538 //****
1515 1539 // RW3
1516 1540 if (ret == LFR_SUCCESSFUL) // K1
1517 1541 {
1518 1542 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K1, pos, value );
1519 1543 }
1520 1544 if (ret == LFR_SUCCESSFUL) // K2
1521 1545 {
1522 1546 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K2, pos, value );
1523 1547 }
1524 1548 if (ret == LFR_SUCCESSFUL) // K3
1525 1549 {
1526 1550 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K3, pos, value );
1527 1551 }
1528 1552 if (ret == LFR_SUCCESSFUL) // K4
1529 1553 {
1530 1554 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K4, pos, value );
1531 1555 }
1532 1556
1533 1557 //****
1534 1558 //****
1535 1559 // RW4
1536 1560 if (ret == LFR_SUCCESSFUL) // K1
1537 1561 {
1538 1562 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K1, pos, value );
1539 1563 }
1540 1564 if (ret == LFR_SUCCESSFUL) // K2
1541 1565 {
1542 1566 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K2, pos, value );
1543 1567 }
1544 1568 if (ret == LFR_SUCCESSFUL) // K3
1545 1569 {
1546 1570 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K3, pos, value );
1547 1571 }
1548 1572 if (ret == LFR_SUCCESSFUL) // K4
1549 1573 {
1550 1574 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K4, pos, value );
1551 1575 }
1552 1576
1553 1577 return ret;
1554 1578 }
1555 1579
1556 1580 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
1557 1581 {
1558 1582 int flag;
1559 1583 rtems_status_code status;
1560 1584
1561 1585 unsigned char sy_lfr_pas_filter_enabled;
1562 1586 unsigned char sy_lfr_pas_filter_modulus;
1563 1587 float sy_lfr_pas_filter_tbad;
1564 1588 unsigned char sy_lfr_pas_filter_offset;
1565 1589 float sy_lfr_pas_filter_shift;
1566 1590 float sy_lfr_sc_rw_delta_f;
1567 1591 char *parPtr;
1568 1592 int datafield_pos;
1569 1593 float rw_k;
1570 1594
1571 1595 flag = LFR_SUCCESSFUL;
1572 1596 sy_lfr_pas_filter_tbad = INIT_FLOAT;
1573 1597 sy_lfr_pas_filter_shift = INIT_FLOAT;
1574 1598 sy_lfr_sc_rw_delta_f = INIT_FLOAT;
1575 1599 parPtr = NULL;
1576 1600 datafield_pos = INIT_INT;
1577 1601 rw_k = INIT_FLOAT;
1578 1602
1579 1603 //***************
1580 1604 // get parameters
1581 1605 sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & BIT_PAS_FILTER_ENABLED; // [0000 0001]
1582 1606 sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
1583 1607 copyFloatByChar(
1584 1608 (unsigned char*) &sy_lfr_pas_filter_tbad,
1585 1609 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD ]
1586 1610 );
1587 1611 sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
1588 1612 copyFloatByChar(
1589 1613 (unsigned char*) &sy_lfr_pas_filter_shift,
1590 1614 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT ]
1591 1615 );
1592 1616 copyFloatByChar(
1593 1617 (unsigned char*) &sy_lfr_sc_rw_delta_f,
1594 1618 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F ]
1595 1619 );
1596 1620
1597 1621 //******************
1598 1622 // CHECK CONSISTENCY
1599 1623
1600 1624 //**************************
1601 1625 // sy_lfr_pas_filter_enabled
1602 1626 // nothing to check, value is 0 or 1
1603 1627
1604 1628 //**************************
1605 1629 // sy_lfr_pas_filter_modulus
1606 1630 if ( (sy_lfr_pas_filter_modulus < MIN_PAS_FILTER_MODULUS) || (sy_lfr_pas_filter_modulus > MAX_PAS_FILTER_MODULUS) )
1607 1631 {
1608 1632 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1609 1633 flag = WRONG_APP_DATA;
1610 1634 }
1611 1635
1612 1636 //***********************
1613 1637 // sy_lfr_pas_filter_tbad
1614 1638 if (flag == LFR_SUCCESSFUL)
1615 1639 {
1616 1640 if ( (sy_lfr_pas_filter_tbad < MIN_PAS_FILTER_TBAD) || (sy_lfr_pas_filter_tbad > MAX_PAS_FILTER_TBAD) )
1617 1641 {
1618 1642 parPtr = (char*) &sy_lfr_pas_filter_tbad;
1619 1643 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1620 1644 flag = WRONG_APP_DATA;
1621 1645 }
1622 1646 }
1623 1647
1624 1648 //*************************
1625 1649 // sy_lfr_pas_filter_offset
1626 1650 if (flag == LFR_SUCCESSFUL)
1627 1651 {
1628 1652 if ( (sy_lfr_pas_filter_offset < MIN_PAS_FILTER_OFFSET) || (sy_lfr_pas_filter_offset > MAX_PAS_FILTER_OFFSET) )
1629 1653 {
1630 1654 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET + DATAFIELD_OFFSET, sy_lfr_pas_filter_offset );
1631 1655 flag = WRONG_APP_DATA;
1632 1656 }
1633 1657 }
1634 1658
1635 1659 //************************
1636 1660 // sy_lfr_pas_filter_shift
1637 1661 if (flag == LFR_SUCCESSFUL)
1638 1662 {
1639 1663 if ( (sy_lfr_pas_filter_shift < MIN_PAS_FILTER_SHIFT) || (sy_lfr_pas_filter_shift > MAX_PAS_FILTER_SHIFT) )
1640 1664 {
1641 1665 parPtr = (char*) &sy_lfr_pas_filter_shift;
1642 1666 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1643 1667 flag = WRONG_APP_DATA;
1644 1668 }
1645 1669 }
1646 1670
1647 1671 //*************************************
1648 1672 // check global coherency of the values
1649 1673 if (flag == LFR_SUCCESSFUL)
1650 1674 {
1651 1675 if ( (sy_lfr_pas_filter_offset + sy_lfr_pas_filter_shift) >= sy_lfr_pas_filter_modulus )
1652 1676 {
1653 1677 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1654 1678 flag = WRONG_APP_DATA;
1655 1679 }
1656 1680 }
1657 1681
1658 1682 //*********************
1659 1683 // sy_lfr_sc_rw_delta_f
1660 1684 if (flag == LFR_SUCCESSFUL)
1661 1685 {
1662 1686 if ( sy_lfr_sc_rw_delta_f < MIN_SY_LFR_SC_RW_DELTA_F )
1663 1687 {
1664 1688 parPtr = (char*) &sy_lfr_sc_rw_delta_f;
1665 1689 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1666 1690 flag = WRONG_APP_DATA;
1667 1691 }
1668 1692 }
1669 1693
1670 1694 //************
1671 1695 // sy_lfr_rw_k
1672 1696 if (flag == LFR_SUCCESSFUL)
1673 1697 {
1674 1698 flag = check_all_sy_lfr_rw_k( TC, &datafield_pos, &rw_k );
1675 1699 if (flag != LFR_SUCCESSFUL)
1676 1700 {
1677 1701 parPtr = (char*) &sy_lfr_pas_filter_shift;
1678 1702 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, datafield_pos + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1679 1703 }
1680 1704 }
1681 1705
1682 1706 return flag;
1683 1707 }
1684 1708
1685 1709 //**************
1686 1710 // KCOEFFICIENTS
1687 1711 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC,rtems_id queue_id )
1688 1712 {
1689 1713 unsigned int kcoeff;
1690 1714 unsigned short sy_lfr_kcoeff_frequency;
1691 1715 unsigned short bin;
1692 1716 float *kcoeffPtr_norm;
1693 1717 float *kcoeffPtr_sbm;
1694 1718 int status;
1695 1719 unsigned char *kcoeffLoadPtr;
1696 1720 unsigned char *kcoeffNormPtr;
1697 1721 unsigned char *kcoeffSbmPtr_a;
1698 1722 unsigned char *kcoeffSbmPtr_b;
1699 1723
1700 1724 sy_lfr_kcoeff_frequency = 0;
1701 1725 bin = 0;
1702 1726 kcoeffPtr_norm = NULL;
1703 1727 kcoeffPtr_sbm = NULL;
1704 1728 status = LFR_SUCCESSFUL;
1705 1729
1706 1730 // copy the value of the frequency byte by byte DO NOT USE A SHORT* POINTER
1707 1731 copyInt16ByChar( (unsigned char*) &sy_lfr_kcoeff_frequency, &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY] );
1708 1732
1709 1733
1710 1734 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
1711 1735 {
1712 1736 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
1713 1737 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + DATAFIELD_OFFSET,
1714 1738 TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 1] ); // +1 to get the LSB instead of the MSB
1715 1739 status = LFR_DEFAULT;
1716 1740 }
1717 1741 else
1718 1742 {
1719 1743 if ( ( sy_lfr_kcoeff_frequency >= 0 )
1720 1744 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
1721 1745 {
1722 1746 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
1723 1747 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
1724 1748 bin = sy_lfr_kcoeff_frequency;
1725 1749 }
1726 1750 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
1727 1751 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
1728 1752 {
1729 1753 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
1730 1754 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
1731 1755 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
1732 1756 }
1733 1757 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
1734 1758 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
1735 1759 {
1736 1760 kcoeffPtr_norm = k_coeff_intercalib_f2;
1737 1761 kcoeffPtr_sbm = NULL;
1738 1762 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
1739 1763 }
1740 1764 }
1741 1765
1742 1766 if (kcoeffPtr_norm != NULL ) // update K coefficient for NORMAL data products
1743 1767 {
1744 1768 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1745 1769 {
1746 1770 // destination
1747 1771 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + kcoeff ];
1748 1772 // source
1749 1773 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1750 1774 // copy source to destination
1751 1775 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
1752 1776 }
1753 1777 }
1754 1778
1755 1779 if (kcoeffPtr_sbm != NULL ) // update K coefficient for SBM data products
1756 1780 {
1757 1781 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1758 1782 {
1759 1783 // destination
1760 1784 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ];
1761 1785 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ (((bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_KCOEFF_PER_NORM_KCOEFF) + 1 ];
1762 1786 // source
1763 1787 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1764 1788 // copy source to destination
1765 1789 copyFloatByChar( kcoeffSbmPtr_a, kcoeffLoadPtr );
1766 1790 copyFloatByChar( kcoeffSbmPtr_b, kcoeffLoadPtr );
1767 1791 }
1768 1792 }
1769 1793
1770 1794 // print_k_coeff();
1771 1795
1772 1796 return status;
1773 1797 }
1774 1798
1775 1799 void copyFloatByChar( unsigned char *destination, unsigned char *source )
1776 1800 {
1777 1801 destination[BYTE_0] = source[BYTE_0];
1778 1802 destination[BYTE_1] = source[BYTE_1];
1779 1803 destination[BYTE_2] = source[BYTE_2];
1780 1804 destination[BYTE_3] = source[BYTE_3];
1781 1805 }
1782 1806
1783 1807 void copyInt32ByChar( unsigned char *destination, unsigned char *source )
1784 1808 {
1785 1809 destination[BYTE_0] = source[BYTE_0];
1786 1810 destination[BYTE_1] = source[BYTE_1];
1787 1811 destination[BYTE_2] = source[BYTE_2];
1788 1812 destination[BYTE_3] = source[BYTE_3];
1789 1813 }
1790 1814
1791 1815 void copyInt16ByChar( unsigned char *destination, unsigned char *source )
1792 1816 {
1793 1817 destination[BYTE_0] = source[BYTE_0];
1794 1818 destination[BYTE_1] = source[BYTE_1];
1795 1819 }
1796 1820
1797 1821 void floatToChar( float value, unsigned char* ptr)
1798 1822 {
1799 1823 unsigned char* valuePtr;
1800 1824
1801 1825 valuePtr = (unsigned char*) &value;
1802 1826
1803 1827 ptr[BYTE_0] = valuePtr[BYTE_0];
1804 1828 ptr[BYTE_1] = valuePtr[BYTE_1];
1805 1829 ptr[BYTE_2] = valuePtr[BYTE_2];
1806 1830 ptr[BYTE_3] = valuePtr[BYTE_3];
1807 1831 }
1808 1832
1809 1833 //**********
1810 1834 // init dump
1811 1835
1812 1836 void init_parameter_dump( void )
1813 1837 {
1814 1838 /** This function initialize the parameter_dump_packet global variable with default values.
1815 1839 *
1816 1840 */
1817 1841
1818 1842 unsigned int k;
1819 1843
1820 1844 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
1821 1845 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
1822 1846 parameter_dump_packet.reserved = CCSDS_RESERVED;
1823 1847 parameter_dump_packet.userApplication = CCSDS_USER_APP;
1824 1848 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1825 1849 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1826 1850 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1827 1851 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1828 1852 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> SHIFT_1_BYTE);
1829 1853 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
1830 1854 // DATA FIELD HEADER
1831 1855 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1832 1856 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
1833 1857 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
1834 1858 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
1835 1859 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
1836 1860 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
1837 1861 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
1838 1862 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
1839 1863 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
1840 1864 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
1841 1865 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
1842 1866
1843 1867 //******************
1844 1868 // COMMON PARAMETERS
1845 1869 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
1846 1870 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
1847 1871
1848 1872 //******************
1849 1873 // NORMAL PARAMETERS
1850 1874 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> SHIFT_1_BYTE);
1851 1875 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1852 1876 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> SHIFT_1_BYTE);
1853 1877 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1854 1878 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> SHIFT_1_BYTE);
1855 1879 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1856 1880 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1857 1881 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1858 1882 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1859 1883
1860 1884 //*****************
1861 1885 // BURST PARAMETERS
1862 1886 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1863 1887 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1864 1888
1865 1889 //****************
1866 1890 // SBM1 PARAMETERS
1867 1891 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
1868 1892 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1869 1893
1870 1894 //****************
1871 1895 // SBM2 PARAMETERS
1872 1896 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1873 1897 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1874 1898
1875 1899 //************
1876 1900 // FBINS MASKS
1877 1901 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1878 1902 {
1879 1903 parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = INT8_ALL_F;
1880 1904 }
1881 1905
1882 1906 // PAS FILTER PARAMETERS
1883 1907 parameter_dump_packet.pa_rpw_spare8_2 = INIT_CHAR;
1884 1908 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = INIT_CHAR;
1885 1909 parameter_dump_packet.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
1886 1910 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_TBAD, parameter_dump_packet.sy_lfr_pas_filter_tbad );
1887 1911 parameter_dump_packet.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
1888 1912 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift );
1889 1913 floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f );
1890 1914
1891 1915 // RW1_K
1892 1916 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw1_k1);
1893 1917 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw1_k2);
1894 1918 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw1_k3);
1895 1919 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw1_k4);
1896 1920 // RW2_K
1897 1921 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw2_k1);
1898 1922 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw2_k2);
1899 1923 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw2_k3);
1900 1924 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw2_k4);
1901 1925 // RW3_K
1902 1926 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw3_k1);
1903 1927 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw3_k2);
1904 1928 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw3_k3);
1905 1929 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw3_k4);
1906 1930 // RW4_K
1907 1931 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw4_k1);
1908 1932 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw4_k2);
1909 1933 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw4_k3);
1910 1934 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw4_k4);
1911 1935
1912 1936 // LFR_RW_MASK
1913 1937 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1914 1938 {
1915 1939 parameter_dump_packet.sy_lfr_rw_mask_f0_word1[k] = INT8_ALL_F;
1916 1940 }
1917 1941
1918 1942 // once the reaction wheels masks have been initialized, they have to be merged with the fbins masks
1919 1943 merge_fbins_masks();
1920 1944 }
1921 1945
1922 1946 void init_kcoefficients_dump( void )
1923 1947 {
1924 1948 init_kcoefficients_dump_packet( &kcoefficients_dump_1, PKTNR_1, KCOEFF_BLK_NR_PKT1 );
1925 1949 init_kcoefficients_dump_packet( &kcoefficients_dump_2, PKTNR_2, KCOEFF_BLK_NR_PKT2 );
1926 1950
1927 1951 kcoefficient_node_1.previous = NULL;
1928 1952 kcoefficient_node_1.next = NULL;
1929 1953 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1930 1954 kcoefficient_node_1.coarseTime = INIT_CHAR;
1931 1955 kcoefficient_node_1.fineTime = INIT_CHAR;
1932 1956 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1933 1957 kcoefficient_node_1.status = INIT_CHAR;
1934 1958
1935 1959 kcoefficient_node_2.previous = NULL;
1936 1960 kcoefficient_node_2.next = NULL;
1937 1961 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1938 1962 kcoefficient_node_2.coarseTime = INIT_CHAR;
1939 1963 kcoefficient_node_2.fineTime = INIT_CHAR;
1940 1964 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1941 1965 kcoefficient_node_2.status = INIT_CHAR;
1942 1966 }
1943 1967
1944 1968 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1945 1969 {
1946 1970 unsigned int k;
1947 1971 unsigned int packetLength;
1948 1972
1949 1973 packetLength =
1950 1974 ((blk_nr * KCOEFF_BLK_SIZE) + BYTE_POS_KCOEFFICIENTS_PARAMETES) - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1951 1975
1952 1976 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1953 1977 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1954 1978 kcoefficients_dump->reserved = CCSDS_RESERVED;
1955 1979 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1956 1980 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1957 1981 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1958 1982 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1959 1983 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1960 1984 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
1961 1985 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1962 1986 // DATA FIELD HEADER
1963 1987 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1964 1988 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1965 1989 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1966 1990 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1967 1991 kcoefficients_dump->time[BYTE_0] = INIT_CHAR;
1968 1992 kcoefficients_dump->time[BYTE_1] = INIT_CHAR;
1969 1993 kcoefficients_dump->time[BYTE_2] = INIT_CHAR;
1970 1994 kcoefficients_dump->time[BYTE_3] = INIT_CHAR;
1971 1995 kcoefficients_dump->time[BYTE_4] = INIT_CHAR;
1972 1996 kcoefficients_dump->time[BYTE_5] = INIT_CHAR;
1973 1997 kcoefficients_dump->sid = SID_K_DUMP;
1974 1998
1975 1999 kcoefficients_dump->pkt_cnt = KCOEFF_PKTCNT;
1976 2000 kcoefficients_dump->pkt_nr = PKTNR_1;
1977 2001 kcoefficients_dump->blk_nr = blk_nr;
1978 2002
1979 2003 //******************
1980 2004 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1981 2005 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1982 2006 for (k=0; k<(KCOEFF_BLK_NR_PKT1 * KCOEFF_BLK_SIZE); k++)
1983 2007 {
1984 2008 kcoefficients_dump->kcoeff_blks[k] = INIT_CHAR;
1985 2009 }
1986 2010 }
1987 2011
1988 2012 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id )
1989 2013 {
1990 2014 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
1991 2015 *
1992 2016 * @param packet_sequence_control points to the packet sequence control which will be incremented
1993 2017 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
1994 2018 *
1995 2019 * If the destination ID is not known, a dedicated counter is incremented.
1996 2020 *
1997 2021 */
1998 2022
1999 2023 unsigned short sequence_cnt;
2000 2024 unsigned short segmentation_grouping_flag;
2001 2025 unsigned short new_packet_sequence_control;
2002 2026 unsigned char i;
2003 2027
2004 2028 switch (destination_id)
2005 2029 {
2006 2030 case SID_TC_GROUND:
2007 2031 i = GROUND;
2008 2032 break;
2009 2033 case SID_TC_MISSION_TIMELINE:
2010 2034 i = MISSION_TIMELINE;
2011 2035 break;
2012 2036 case SID_TC_TC_SEQUENCES:
2013 2037 i = TC_SEQUENCES;
2014 2038 break;
2015 2039 case SID_TC_RECOVERY_ACTION_CMD:
2016 2040 i = RECOVERY_ACTION_CMD;
2017 2041 break;
2018 2042 case SID_TC_BACKUP_MISSION_TIMELINE:
2019 2043 i = BACKUP_MISSION_TIMELINE;
2020 2044 break;
2021 2045 case SID_TC_DIRECT_CMD:
2022 2046 i = DIRECT_CMD;
2023 2047 break;
2024 2048 case SID_TC_SPARE_GRD_SRC1:
2025 2049 i = SPARE_GRD_SRC1;
2026 2050 break;
2027 2051 case SID_TC_SPARE_GRD_SRC2:
2028 2052 i = SPARE_GRD_SRC2;
2029 2053 break;
2030 2054 case SID_TC_OBCP:
2031 2055 i = OBCP;
2032 2056 break;
2033 2057 case SID_TC_SYSTEM_CONTROL:
2034 2058 i = SYSTEM_CONTROL;
2035 2059 break;
2036 2060 case SID_TC_AOCS:
2037 2061 i = AOCS;
2038 2062 break;
2039 2063 case SID_TC_RPW_INTERNAL:
2040 2064 i = RPW_INTERNAL;
2041 2065 break;
2042 2066 default:
2043 2067 i = GROUND;
2044 2068 break;
2045 2069 }
2046 2070
2047 2071 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
2048 2072 sequence_cnt = sequenceCounters_TM_DUMP[ i ] & SEQ_CNT_MASK;
2049 2073
2050 2074 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
2051 2075
2052 2076 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> SHIFT_1_BYTE);
2053 2077 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
2054 2078
2055 2079 // increment the sequence counter
2056 2080 if ( sequenceCounters_TM_DUMP[ i ] < SEQ_CNT_MAX )
2057 2081 {
2058 2082 sequenceCounters_TM_DUMP[ i ] = sequenceCounters_TM_DUMP[ i ] + 1;
2059 2083 }
2060 2084 else
2061 2085 {
2062 2086 sequenceCounters_TM_DUMP[ i ] = 0;
2063 2087 }
2064 2088 }
Show file before | Show file after | Hide comments
@@ -1,512 +1,536
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** Functions to send TM packets related to TC parsing and execution.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * A group of functions to send appropriate TM packets after parsing and execution:
7 31 * - TM_LFR_TC_EXE_SUCCESS
8 32 * - TM_LFR_TC_EXE_INCONSISTENT
9 33 * - TM_LFR_TC_EXE_NOT_EXECUTABLE
10 34 * - TM_LFR_TC_EXE_NOT_IMPLEMENTED
11 35 * - TM_LFR_TC_EXE_ERROR
12 36 * - TM_LFR_TC_EXE_CORRUPTED
13 37 *
14 38 */
15 39
16 40 #include "tm_lfr_tc_exe.h"
17 41
18 42 int send_tm_lfr_tc_exe_success( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
19 43 {
20 44 /** This function sends a TM_LFR_TC_EXE_SUCCESS packet in the dedicated RTEMS message queue.
21 45 *
22 46 * @param TC points to the TeleCommand packet that is being processed
23 47 * @param queue_id is the id of the queue which handles TM
24 48 *
25 49 * @return RTEMS directive status code:
26 50 * - RTEMS_SUCCESSFUL - message sent successfully
27 51 * - RTEMS_INVALID_ID - invalid queue id
28 52 * - RTEMS_INVALID_SIZE - invalid message size
29 53 * - RTEMS_INVALID_ADDRESS - buffer is NULL
30 54 * - RTEMS_UNSATISFIED - out of message buffers
31 55 * - RTEMS_TOO_MANY - queue s limit has been reached
32 56 *
33 57 */
34 58
35 59 rtems_status_code status;
36 60 Packet_TM_LFR_TC_EXE_SUCCESS_t TM;
37 61 unsigned char messageSize;
38 62
39 63 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
40 64 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
41 65 TM.reserved = DEFAULT_RESERVED;
42 66 TM.userApplication = CCSDS_USER_APP;
43 67 // PACKET HEADER
44 68 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
45 69 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
46 70 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
47 71 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_SUCCESS >> SHIFT_1_BYTE);
48 72 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_SUCCESS );
49 73 // DATA FIELD HEADER
50 74 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
51 75 TM.serviceType = TM_TYPE_TC_EXE;
52 76 TM.serviceSubType = TM_SUBTYPE_EXE_OK;
53 77 TM.destinationID = TC->sourceID;
54 78 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
55 79 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
56 80 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
57 81 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
58 82 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
59 83 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
60 84 //
61 85 TM.telecommand_pkt_id[0] = TC->packetID[0];
62 86 TM.telecommand_pkt_id[1] = TC->packetID[1];
63 87 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
64 88 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
65 89
66 90 messageSize = PACKET_LENGTH_TC_EXE_SUCCESS + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
67 91
68 92 // SEND DATA
69 93 status = rtems_message_queue_send( queue_id, &TM, messageSize);
70 94 if (status != RTEMS_SUCCESSFUL) {
71 95 PRINTF("in send_tm_lfr_tc_exe_success *** ERR\n")
72 96 }
73 97
74 98 // UPDATE HK FIELDS
75 99 update_last_TC_exe( TC, TM.time );
76 100
77 101 return status;
78 102 }
79 103
80 104 int send_tm_lfr_tc_exe_inconsistent( ccsdsTelecommandPacket_t *TC, rtems_id queue_id,
81 105 unsigned char byte_position, unsigned char rcv_value )
82 106 {
83 107 /** This function sends a TM_LFR_TC_EXE_INCONSISTENT packet in the dedicated RTEMS message queue.
84 108 *
85 109 * @param TC points to the TeleCommand packet that is being processed
86 110 * @param queue_id is the id of the queue which handles TM
87 111 * @param byte_position is the byte position of the MSB of the parameter that has been seen as inconsistent
88 112 * @param rcv_value is the value of the LSB of the parameter that has been detected as inconsistent
89 113 *
90 114 * @return RTEMS directive status code:
91 115 * - RTEMS_SUCCESSFUL - message sent successfully
92 116 * - RTEMS_INVALID_ID - invalid queue id
93 117 * - RTEMS_INVALID_SIZE - invalid message size
94 118 * - RTEMS_INVALID_ADDRESS - buffer is NULL
95 119 * - RTEMS_UNSATISFIED - out of message buffers
96 120 * - RTEMS_TOO_MANY - queue s limit has been reached
97 121 *
98 122 */
99 123
100 124 rtems_status_code status;
101 125 Packet_TM_LFR_TC_EXE_INCONSISTENT_t TM;
102 126 unsigned char messageSize;
103 127
104 128 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
105 129 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
106 130 TM.reserved = DEFAULT_RESERVED;
107 131 TM.userApplication = CCSDS_USER_APP;
108 132 // PACKET HEADER
109 133 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
110 134 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
111 135 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
112 136 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_INCONSISTENT >> SHIFT_1_BYTE);
113 137 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_INCONSISTENT );
114 138 // DATA FIELD HEADER
115 139 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
116 140 TM.serviceType = TM_TYPE_TC_EXE;
117 141 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
118 142 TM.destinationID = TC->sourceID;
119 143 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
120 144 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
121 145 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
122 146 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
123 147 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
124 148 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
125 149 //
126 150 TM.tc_failure_code[0] = (char) (WRONG_APP_DATA >> SHIFT_1_BYTE);
127 151 TM.tc_failure_code[1] = (char) (WRONG_APP_DATA );
128 152 TM.telecommand_pkt_id[0] = TC->packetID[0];
129 153 TM.telecommand_pkt_id[1] = TC->packetID[1];
130 154 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
131 155 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
132 156 TM.tc_service = TC->serviceType; // type of the rejected TC
133 157 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
134 158 TM.byte_position = byte_position;
135 159 TM.rcv_value = (unsigned char) rcv_value;
136 160
137 161 messageSize = PACKET_LENGTH_TC_EXE_INCONSISTENT + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
138 162
139 163 // SEND DATA
140 164 status = rtems_message_queue_send( queue_id, &TM, messageSize);
141 165 if (status != RTEMS_SUCCESSFUL) {
142 166 PRINTF("in send_tm_lfr_tc_exe_inconsistent *** ERR\n")
143 167 }
144 168
145 169 // UPDATE HK FIELDS
146 170 update_last_TC_rej( TC, TM.time );
147 171
148 172 return status;
149 173 }
150 174
151 175 int send_tm_lfr_tc_exe_not_executable( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
152 176 {
153 177 /** This function sends a TM_LFR_TC_EXE_NOT_EXECUTABLE packet in the dedicated RTEMS message queue.
154 178 *
155 179 * @param TC points to the TeleCommand packet that is being processed
156 180 * @param queue_id is the id of the queue which handles TM
157 181 *
158 182 * @return RTEMS directive status code:
159 183 * - RTEMS_SUCCESSFUL - message sent successfully
160 184 * - RTEMS_INVALID_ID - invalid queue id
161 185 * - RTEMS_INVALID_SIZE - invalid message size
162 186 * - RTEMS_INVALID_ADDRESS - buffer is NULL
163 187 * - RTEMS_UNSATISFIED - out of message buffers
164 188 * - RTEMS_TOO_MANY - queue s limit has been reached
165 189 *
166 190 */
167 191
168 192 rtems_status_code status;
169 193 Packet_TM_LFR_TC_EXE_NOT_EXECUTABLE_t TM;
170 194 unsigned char messageSize;
171 195
172 196 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
173 197 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
174 198 TM.reserved = DEFAULT_RESERVED;
175 199 TM.userApplication = CCSDS_USER_APP;
176 200 // PACKET HEADER
177 201 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
178 202 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
179 203 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
180 204 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE >> SHIFT_1_BYTE);
181 205 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE );
182 206 // DATA FIELD HEADER
183 207 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
184 208 TM.serviceType = TM_TYPE_TC_EXE;
185 209 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
186 210 TM.destinationID = TC->sourceID; // default destination id
187 211 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
188 212 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
189 213 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
190 214 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
191 215 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
192 216 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
193 217 //
194 218 TM.tc_failure_code[0] = (char) (TC_NOT_EXE >> SHIFT_1_BYTE);
195 219 TM.tc_failure_code[1] = (char) (TC_NOT_EXE );
196 220 TM.telecommand_pkt_id[0] = TC->packetID[0];
197 221 TM.telecommand_pkt_id[1] = TC->packetID[1];
198 222 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
199 223 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
200 224 TM.tc_service = TC->serviceType; // type of the rejected TC
201 225 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
202 226 TM.lfr_status_word[0] = housekeeping_packet.lfr_status_word[0];
203 227 TM.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1];
204 228
205 229 messageSize = PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
206 230
207 231 // SEND DATA
208 232 status = rtems_message_queue_send( queue_id, &TM, messageSize);
209 233 if (status != RTEMS_SUCCESSFUL) {
210 234 PRINTF("in send_tm_lfr_tc_exe_not_executable *** ERR\n")
211 235 }
212 236
213 237 // UPDATE HK FIELDS
214 238 update_last_TC_rej( TC, TM.time );
215 239
216 240 return status;
217 241 }
218 242
219 243 int send_tm_lfr_tc_exe_not_implemented( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time )
220 244 {
221 245 /** This function sends a TM_LFR_TC_EXE_NOT_IMPLEMENTED packet in the dedicated RTEMS message queue.
222 246 *
223 247 * @param TC points to the TeleCommand packet that is being processed
224 248 * @param queue_id is the id of the queue which handles TM
225 249 *
226 250 * @return RTEMS directive status code:
227 251 * - RTEMS_SUCCESSFUL - message sent successfully
228 252 * - RTEMS_INVALID_ID - invalid queue id
229 253 * - RTEMS_INVALID_SIZE - invalid message size
230 254 * - RTEMS_INVALID_ADDRESS - buffer is NULL
231 255 * - RTEMS_UNSATISFIED - out of message buffers
232 256 * - RTEMS_TOO_MANY - queue s limit has been reached
233 257 *
234 258 */
235 259
236 260 rtems_status_code status;
237 261 Packet_TM_LFR_TC_EXE_NOT_IMPLEMENTED_t TM;
238 262 unsigned char messageSize;
239 263
240 264 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
241 265 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
242 266 TM.reserved = DEFAULT_RESERVED;
243 267 TM.userApplication = CCSDS_USER_APP;
244 268 // PACKET HEADER
245 269 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
246 270 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
247 271 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
248 272 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED >> SHIFT_1_BYTE);
249 273 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED );
250 274 // DATA FIELD HEADER
251 275 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
252 276 TM.serviceType = TM_TYPE_TC_EXE;
253 277 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
254 278 TM.destinationID = TC->sourceID; // default destination id
255 279 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
256 280 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
257 281 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
258 282 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
259 283 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
260 284 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
261 285 //
262 286 TM.tc_failure_code[0] = (char) (FUNCT_NOT_IMPL >> SHIFT_1_BYTE);
263 287 TM.tc_failure_code[1] = (char) (FUNCT_NOT_IMPL );
264 288 TM.telecommand_pkt_id[0] = TC->packetID[0];
265 289 TM.telecommand_pkt_id[1] = TC->packetID[1];
266 290 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
267 291 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
268 292 TM.tc_service = TC->serviceType; // type of the rejected TC
269 293 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
270 294
271 295 messageSize = PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
272 296
273 297 // SEND DATA
274 298 status = rtems_message_queue_send( queue_id, &TM, messageSize);
275 299 if (status != RTEMS_SUCCESSFUL) {
276 300 PRINTF("in send_tm_lfr_tc_exe_not_implemented *** ERR\n")
277 301 }
278 302
279 303 // UPDATE HK FIELDS
280 304 update_last_TC_rej( TC, TM.time );
281 305
282 306 return status;
283 307 }
284 308
285 309 int send_tm_lfr_tc_exe_error( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
286 310 {
287 311 /** This function sends a TM_LFR_TC_EXE_ERROR packet in the dedicated RTEMS message queue.
288 312 *
289 313 * @param TC points to the TeleCommand packet that is being processed
290 314 * @param queue_id is the id of the queue which handles TM
291 315 *
292 316 * @return RTEMS directive status code:
293 317 * - RTEMS_SUCCESSFUL - message sent successfully
294 318 * - RTEMS_INVALID_ID - invalid queue id
295 319 * - RTEMS_INVALID_SIZE - invalid message size
296 320 * - RTEMS_INVALID_ADDRESS - buffer is NULL
297 321 * - RTEMS_UNSATISFIED - out of message buffers
298 322 * - RTEMS_TOO_MANY - queue s limit has been reached
299 323 *
300 324 */
301 325
302 326 rtems_status_code status;
303 327 Packet_TM_LFR_TC_EXE_ERROR_t TM;
304 328 unsigned char messageSize;
305 329
306 330 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
307 331 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
308 332 TM.reserved = DEFAULT_RESERVED;
309 333 TM.userApplication = CCSDS_USER_APP;
310 334 // PACKET HEADER
311 335 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
312 336 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
313 337 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
314 338 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_ERROR >> SHIFT_1_BYTE);
315 339 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_ERROR );
316 340 // DATA FIELD HEADER
317 341 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
318 342 TM.serviceType = TM_TYPE_TC_EXE;
319 343 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
320 344 TM.destinationID = TC->sourceID; // default destination id
321 345 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
322 346 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
323 347 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
324 348 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
325 349 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
326 350 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
327 351 //
328 352 TM.tc_failure_code[0] = (char) (FAIL_DETECTED >> SHIFT_1_BYTE);
329 353 TM.tc_failure_code[1] = (char) (FAIL_DETECTED );
330 354 TM.telecommand_pkt_id[0] = TC->packetID[0];
331 355 TM.telecommand_pkt_id[1] = TC->packetID[1];
332 356 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
333 357 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
334 358 TM.tc_service = TC->serviceType; // type of the rejected TC
335 359 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
336 360
337 361 messageSize = PACKET_LENGTH_TC_EXE_ERROR + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
338 362
339 363 // SEND DATA
340 364 status = rtems_message_queue_send( queue_id, &TM, messageSize);
341 365 if (status != RTEMS_SUCCESSFUL) {
342 366 PRINTF("in send_tm_lfr_tc_exe_error *** ERR\n")
343 367 }
344 368
345 369 // UPDATE HK FIELDS
346 370 update_last_TC_rej( TC, TM.time );
347 371
348 372 return status;
349 373 }
350 374
351 375 int send_tm_lfr_tc_exe_corrupted(ccsdsTelecommandPacket_t *TC, rtems_id queue_id,
352 376 unsigned char *computed_CRC, unsigned char *currentTC_LEN_RCV,
353 377 unsigned char destinationID )
354 378 {
355 379 /** This function sends a TM_LFR_TC_EXE_CORRUPTED packet in the dedicated RTEMS message queue.
356 380 *
357 381 * @param TC points to the TeleCommand packet that is being processed
358 382 * @param queue_id is the id of the queue which handles TM
359 383 * @param computed_CRC points to a buffer of two bytes containing the CRC computed during the parsing of the TeleCommand
360 384 * @param currentTC_LEN_RCV points to a buffer of two bytes containing a packet size field computed on the received data
361 385 *
362 386 * @return RTEMS directive status code:
363 387 * - RTEMS_SUCCESSFUL - message sent successfully
364 388 * - RTEMS_INVALID_ID - invalid queue id
365 389 * - RTEMS_INVALID_SIZE - invalid message size
366 390 * - RTEMS_INVALID_ADDRESS - buffer is NULL
367 391 * - RTEMS_UNSATISFIED - out of message buffers
368 392 * - RTEMS_TOO_MANY - queue s limit has been reached
369 393 *
370 394 */
371 395
372 396 rtems_status_code status;
373 397 Packet_TM_LFR_TC_EXE_CORRUPTED_t TM;
374 398 unsigned char messageSize;
375 399 unsigned int packetLength;
376 400 unsigned int estimatedPacketLength;
377 401 unsigned char *packetDataField;
378 402
379 403 packetLength = (TC->packetLength[0] * CONST_256) + TC->packetLength[1]; // compute the packet length parameter written in the TC
380 404 estimatedPacketLength = (unsigned int) ((currentTC_LEN_RCV[0] * CONST_256) + currentTC_LEN_RCV[1]);
381 405 packetDataField = (unsigned char *) &TC->headerFlag_pusVersion_Ack; // get the beginning of the data field
382 406
383 407 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
384 408 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
385 409 TM.reserved = DEFAULT_RESERVED;
386 410 TM.userApplication = CCSDS_USER_APP;
387 411 // PACKET HEADER
388 412 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
389 413 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
390 414 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
391 415 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_CORRUPTED >> SHIFT_1_BYTE);
392 416 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_CORRUPTED );
393 417 // DATA FIELD HEADER
394 418 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
395 419 TM.serviceType = TM_TYPE_TC_EXE;
396 420 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
397 421 TM.destinationID = destinationID;
398 422 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
399 423 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
400 424 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
401 425 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
402 426 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
403 427 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
404 428 //
405 429 TM.tc_failure_code[0] = (unsigned char) (CORRUPTED >> SHIFT_1_BYTE);
406 430 TM.tc_failure_code[1] = (unsigned char) (CORRUPTED );
407 431 TM.telecommand_pkt_id[0] = TC->packetID[0];
408 432 TM.telecommand_pkt_id[1] = TC->packetID[1];
409 433 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
410 434 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
411 435 TM.tc_service = TC->serviceType; // type of the rejected TC
412 436 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
413 437 TM.pkt_len_rcv_value[0] = TC->packetLength[0];
414 438 TM.pkt_len_rcv_value[1] = TC->packetLength[1];
415 439 TM.pkt_datafieldsize_cnt[0] = currentTC_LEN_RCV[0];
416 440 TM.pkt_datafieldsize_cnt[1] = currentTC_LEN_RCV[1];
417 441 TM.rcv_crc[0] = packetDataField[ estimatedPacketLength - 1 ];
418 442 TM.rcv_crc[1] = packetDataField[ estimatedPacketLength ];
419 443 TM.computed_crc[0] = computed_CRC[0];
420 444 TM.computed_crc[1] = computed_CRC[1];
421 445
422 446 messageSize = PACKET_LENGTH_TC_EXE_CORRUPTED + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
423 447
424 448 // SEND DATA
425 449 status = rtems_message_queue_send( queue_id, &TM, messageSize);
426 450 if (status != RTEMS_SUCCESSFUL) {
427 451 PRINTF("in send_tm_lfr_tc_exe_error *** ERR\n")
428 452 }
429 453
430 454 // UPDATE HK FIELDS
431 455 update_last_TC_rej( TC, TM.time );
432 456
433 457 return status;
434 458 }
435 459
436 460 void increment_seq_counter_destination_id( unsigned char *packet_sequence_control, unsigned char destination_id )
437 461 {
438 462 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
439 463 *
440 464 * @param packet_sequence_control points to the packet sequence control which will be incremented
441 465 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
442 466 *
443 467 * If the destination ID is not known, a dedicated counter is incremented.
444 468 *
445 469 */
446 470
447 471 unsigned short sequence_cnt;
448 472 unsigned short segmentation_grouping_flag;
449 473 unsigned short new_packet_sequence_control;
450 474 unsigned char i;
451 475
452 476 switch (destination_id)
453 477 {
454 478 case SID_TC_GROUND:
455 479 i = GROUND;
456 480 break;
457 481 case SID_TC_MISSION_TIMELINE:
458 482 i = MISSION_TIMELINE;
459 483 break;
460 484 case SID_TC_TC_SEQUENCES:
461 485 i = TC_SEQUENCES;
462 486 break;
463 487 case SID_TC_RECOVERY_ACTION_CMD:
464 488 i = RECOVERY_ACTION_CMD;
465 489 break;
466 490 case SID_TC_BACKUP_MISSION_TIMELINE:
467 491 i = BACKUP_MISSION_TIMELINE;
468 492 break;
469 493 case SID_TC_DIRECT_CMD:
470 494 i = DIRECT_CMD;
471 495 break;
472 496 case SID_TC_SPARE_GRD_SRC1:
473 497 i = SPARE_GRD_SRC1;
474 498 break;
475 499 case SID_TC_SPARE_GRD_SRC2:
476 500 i = SPARE_GRD_SRC2;
477 501 break;
478 502 case SID_TC_OBCP:
479 503 i = OBCP;
480 504 break;
481 505 case SID_TC_SYSTEM_CONTROL:
482 506 i = SYSTEM_CONTROL;
483 507 break;
484 508 case SID_TC_AOCS:
485 509 i = AOCS;
486 510 break;
487 511 case SID_TC_RPW_INTERNAL:
488 512 i = RPW_INTERNAL;
489 513 break;
490 514 default:
491 515 i = GROUND;
492 516 break;
493 517 }
494 518
495 519 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
496 520 sequence_cnt = sequenceCounters_TC_EXE[ i ] & SEQ_CNT_MASK;
497 521
498 522 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
499 523
500 524 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> SHIFT_1_BYTE);
501 525 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
502 526
503 527 // increment the sequence counter
504 528 if ( sequenceCounters_TC_EXE[ i ] < SEQ_CNT_MAX )
505 529 {
506 530 sequenceCounters_TC_EXE[ i ] = sequenceCounters_TC_EXE[ i ] + 1;
507 531 }
508 532 else
509 533 {
510 534 sequenceCounters_TC_EXE[ i ] = 0;
511 535 }
512 536 }
Show file before | Show file after | Hide comments
@@ -1,1342 +1,1366
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** Functions and tasks related to waveform packet generation.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * A group of functions to handle waveforms, in snapshot or continuous format.\n
7 31 *
8 32 */
9 33
10 34 #include "wf_handler.h"
11 35
12 36 //***************
13 37 // waveform rings
14 38 // F0
15 39 ring_node waveform_ring_f0[NB_RING_NODES_F0]= {0};
16 40 ring_node *current_ring_node_f0 = NULL;
17 41 ring_node *ring_node_to_send_swf_f0 = NULL;
18 42 // F1
19 43 ring_node waveform_ring_f1[NB_RING_NODES_F1] = {0};
20 44 ring_node *current_ring_node_f1 = NULL;
21 45 ring_node *ring_node_to_send_swf_f1 = NULL;
22 46 ring_node *ring_node_to_send_cwf_f1 = NULL;
23 47 // F2
24 48 ring_node waveform_ring_f2[NB_RING_NODES_F2] = {0};
25 49 ring_node *current_ring_node_f2 = NULL;
26 50 ring_node *ring_node_to_send_swf_f2 = NULL;
27 51 ring_node *ring_node_to_send_cwf_f2 = NULL;
28 52 // F3
29 53 ring_node waveform_ring_f3[NB_RING_NODES_F3] = {0};
30 54 ring_node *current_ring_node_f3 = NULL;
31 55 ring_node *ring_node_to_send_cwf_f3 = NULL;
32 56 char wf_cont_f3_light[ (NB_SAMPLES_PER_SNAPSHOT) * NB_BYTES_CWF3_LIGHT_BLK ] = {0};
33 57
34 58 bool extractSWF1 = false;
35 59 bool extractSWF2 = false;
36 60 bool swf0_ready_flag_f1 = false;
37 61 bool swf0_ready_flag_f2 = false;
38 62 bool swf1_ready = false;
39 63 bool swf2_ready = false;
40 64
41 65 int swf1_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ] = {0};
42 66 int swf2_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ] = {0};
43 67 ring_node ring_node_swf1_extracted = {0};
44 68 ring_node ring_node_swf2_extracted = {0};
45 69
46 70 typedef enum resynchro_state_t
47 71 {
48 72 MEASURE,
49 73 CORRECTION
50 74 } resynchro_state;
51 75
52 76 //*********************
53 77 // Interrupt SubRoutine
54 78
55 79 ring_node * getRingNodeToSendCWF( unsigned char frequencyChannel)
56 80 {
57 81 ring_node *node;
58 82
59 83 node = NULL;
60 84 switch ( frequencyChannel ) {
61 85 case CHANNELF1:
62 86 node = ring_node_to_send_cwf_f1;
63 87 break;
64 88 case CHANNELF2:
65 89 node = ring_node_to_send_cwf_f2;
66 90 break;
67 91 case CHANNELF3:
68 92 node = ring_node_to_send_cwf_f3;
69 93 break;
70 94 default:
71 95 break;
72 96 }
73 97
74 98 return node;
75 99 }
76 100
77 101 ring_node * getRingNodeToSendSWF( unsigned char frequencyChannel)
78 102 {
79 103 ring_node *node;
80 104
81 105 node = NULL;
82 106 switch ( frequencyChannel ) {
83 107 case CHANNELF0:
84 108 node = ring_node_to_send_swf_f0;
85 109 break;
86 110 case CHANNELF1:
87 111 node = ring_node_to_send_swf_f1;
88 112 break;
89 113 case CHANNELF2:
90 114 node = ring_node_to_send_swf_f2;
91 115 break;
92 116 default:
93 117 break;
94 118 }
95 119
96 120 return node;
97 121 }
98 122
99 123 void reset_extractSWF( void )
100 124 {
101 125 extractSWF1 = false;
102 126 extractSWF2 = false;
103 127 swf0_ready_flag_f1 = false;
104 128 swf0_ready_flag_f2 = false;
105 129 swf1_ready = false;
106 130 swf2_ready = false;
107 131 }
108 132
109 133 inline void waveforms_isr_f3( void )
110 134 {
111 135 rtems_status_code spare_status;
112 136
113 137 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
114 138 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
115 139 { // in modes other than STANDBY and BURST, send the CWF_F3 data
116 140 //***
117 141 // F3
118 142 if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F3) != INIT_CHAR ) { // [1100 0000] check the f3 full bits
119 143 ring_node_to_send_cwf_f3 = current_ring_node_f3->previous;
120 144 current_ring_node_f3 = current_ring_node_f3->next;
121 145 if ((waveform_picker_regs->status & BIT_WFP_BUF_F3_0) == BIT_WFP_BUF_F3_0){ // [0100 0000] f3 buffer 0 is full
122 146 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_0_coarse_time;
123 147 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_0_fine_time;
124 148 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->buffer_address;
125 149 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F3_0; // [1000 1000 0100 0000]
126 150 }
127 151 else if ((waveform_picker_regs->status & BIT_WFP_BUF_F3_1) == BIT_WFP_BUF_F3_1){ // [1000 0000] f3 buffer 1 is full
128 152 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_1_coarse_time;
129 153 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_1_fine_time;
130 154 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address;
131 155 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F3_1; // [1000 1000 1000 0000]
132 156 }
133 157 if (rtems_event_send( Task_id[TASKID_CWF3], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
134 158 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
135 159 }
136 160 }
137 161 }
138 162 }
139 163
140 164 inline void waveforms_isr_burst( void )
141 165 {
142 166 unsigned char status;
143 167 rtems_status_code spare_status;
144 168
145 169 status = (waveform_picker_regs->status & BITS_WFP_STATUS_F2) >> SHIFT_WFP_STATUS_F2; // [0011 0000] get the status bits for f2
146 170
147 171 switch(status)
148 172 {
149 173 case BIT_WFP_BUFFER_0:
150 174 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
151 175 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
152 176 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
153 177 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
154 178 current_ring_node_f2 = current_ring_node_f2->next;
155 179 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
156 180 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
157 181 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
158 182 }
159 183 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_0; // [0100 0100 0001 0000]
160 184 break;
161 185 case BIT_WFP_BUFFER_1:
162 186 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
163 187 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
164 188 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
165 189 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
166 190 current_ring_node_f2 = current_ring_node_f2->next;
167 191 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
168 192 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
169 193 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
170 194 }
171 195 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_1; // [0100 0100 0010 0000]
172 196 break;
173 197 default:
174 198 break;
175 199 }
176 200 }
177 201
178 202 inline void waveform_isr_normal_sbm1_sbm2( void )
179 203 {
180 204 rtems_status_code status;
181 205
182 206 //***
183 207 // F0
184 208 if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F0) != INIT_CHAR ) // [0000 0011] check the f0 full bits
185 209 {
186 210 swf0_ready_flag_f1 = true;
187 211 swf0_ready_flag_f2 = true;
188 212 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
189 213 current_ring_node_f0 = current_ring_node_f0->next;
190 214 if ( (waveform_picker_regs->status & BIT_WFP_BUFFER_0) == BIT_WFP_BUFFER_0)
191 215 {
192 216
193 217 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
194 218 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
195 219 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
196 220 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F0_0; // [0001 0001 0000 0001]
197 221 }
198 222 else if ( (waveform_picker_regs->status & BIT_WFP_BUFFER_1) == BIT_WFP_BUFFER_1)
199 223 {
200 224 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
201 225 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
202 226 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
203 227 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F0_1; // [0001 0001 0000 0010]
204 228 }
205 229 // send an event to the WFRM task for resynchro activities
206 230 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_SWF_RESYNCH );
207 231 status = rtems_event_send( Task_id[TASKID_CALI], RTEMS_EVENT_CAL_SWEEP_WAKE );
208 232 }
209 233
210 234 //***
211 235 // F1
212 236 if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F1) != INIT_CHAR ) { // [0000 1100] check the f1 full bits
213 237 // (1) change the receiving buffer for the waveform picker
214 238 ring_node_to_send_cwf_f1 = current_ring_node_f1->previous;
215 239 current_ring_node_f1 = current_ring_node_f1->next;
216 240 if ( (waveform_picker_regs->status & BIT_WFP_BUF_F1_0) == BIT_WFP_BUF_F1_0)
217 241 {
218 242 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
219 243 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
220 244 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
221 245 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F1_0; // [0010 0010 0000 0100] f1 bits = 0
222 246 }
223 247 else if ( (waveform_picker_regs->status & BIT_WFP_BUF_F1_1) == BIT_WFP_BUF_F1_1)
224 248 {
225 249 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
226 250 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
227 251 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
228 252 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F1_1; // [0010 0010 0000 1000] f1 bits = 0
229 253 }
230 254 // (2) send an event for the the CWF1 task for transmission (and snapshot extraction if needed)
231 255 status = rtems_event_send( Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_NORM_S1_S2 );
232 256 }
233 257
234 258 //***
235 259 // F2
236 260 if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F2) != INIT_CHAR ) { // [0011 0000] check the f2 full bit
237 261 // (1) change the receiving buffer for the waveform picker
238 262 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
239 263 ring_node_to_send_cwf_f2->sid = SID_SBM2_CWF_F2;
240 264 current_ring_node_f2 = current_ring_node_f2->next;
241 265 if ( (waveform_picker_regs->status & BIT_WFP_BUF_F2_0) == BIT_WFP_BUF_F2_0)
242 266 {
243 267 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
244 268 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
245 269 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
246 270 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_0; // [0100 0100 0001 0000]
247 271 }
248 272 else if ( (waveform_picker_regs->status & BIT_WFP_BUF_F2_1) == BIT_WFP_BUF_F2_1)
249 273 {
250 274 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
251 275 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
252 276 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
253 277 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_1; // [0100 0100 0010 0000]
254 278 }
255 279 // (2) send an event for the waveforms transmission
256 280 status = rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_NORM_S1_S2 );
257 281 }
258 282 }
259 283
260 284 rtems_isr waveforms_isr( rtems_vector_number vector )
261 285 {
262 286 /** This is the interrupt sub routine called by the waveform picker core.
263 287 *
264 288 * This ISR launch different actions depending mainly on two pieces of information:
265 289 * 1. the values read in the registers of the waveform picker.
266 290 * 2. the current LFR mode.
267 291 *
268 292 */
269 293
270 294 // STATUS
271 295 // new error error buffer full
272 296 // 15 14 13 12 11 10 9 8
273 297 // f3 f2 f1 f0 f3 f2 f1 f0
274 298 //
275 299 // ready buffer
276 300 // 7 6 5 4 3 2 1 0
277 301 // f3_1 f3_0 f2_1 f2_0 f1_1 f1_0 f0_1 f0_0
278 302
279 303 rtems_status_code spare_status;
280 304
281 305 waveforms_isr_f3();
282 306
283 307 //*************************************************
284 308 // copy the status bits in the housekeeping packets
285 309 housekeeping_packet.hk_lfr_vhdl_iir_cal =
286 310 (unsigned char) ((waveform_picker_regs->status & BYTE0_MASK) >> SHIFT_1_BYTE);
287 311
288 312 if ( (waveform_picker_regs->status & BYTE0_MASK) != INIT_CHAR) // [1111 1111 0000 0000] check the error bits
289 313 {
290 314 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_10 );
291 315 }
292 316
293 317 switch(lfrCurrentMode)
294 318 {
295 319 //********
296 320 // STANDBY
297 321 case LFR_MODE_STANDBY:
298 322 break;
299 323 //**************************
300 324 // LFR NORMAL, SBM1 and SBM2
301 325 case LFR_MODE_NORMAL:
302 326 case LFR_MODE_SBM1:
303 327 case LFR_MODE_SBM2:
304 328 waveform_isr_normal_sbm1_sbm2();
305 329 break;
306 330 //******
307 331 // BURST
308 332 case LFR_MODE_BURST:
309 333 waveforms_isr_burst();
310 334 break;
311 335 //********
312 336 // DEFAULT
313 337 default:
314 338 break;
315 339 }
316 340 }
317 341
318 342 //************
319 343 // RTEMS TASKS
320 344
321 345 rtems_task wfrm_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
322 346 {
323 347 /** This RTEMS task is dedicated to the transmission of snapshots of the NORMAL mode.
324 348 *
325 349 * @param unused is the starting argument of the RTEMS task
326 350 *
327 351 * The following data packets are sent by this task:
328 352 * - TM_LFR_SCIENCE_NORMAL_SWF_F0
329 353 * - TM_LFR_SCIENCE_NORMAL_SWF_F1
330 354 * - TM_LFR_SCIENCE_NORMAL_SWF_F2
331 355 *
332 356 */
333 357
334 358 rtems_event_set event_out;
335 359 rtems_id queue_id;
336 360 rtems_status_code status;
337 361 ring_node *ring_node_swf1_extracted_ptr;
338 362 ring_node *ring_node_swf2_extracted_ptr;
339 363
340 364 event_out = EVENT_SETS_NONE_PENDING;
341 365 queue_id = RTEMS_ID_NONE;
342 366
343 367 ring_node_swf1_extracted_ptr = (ring_node *) &ring_node_swf1_extracted;
344 368 ring_node_swf2_extracted_ptr = (ring_node *) &ring_node_swf2_extracted;
345 369
346 370 status = get_message_queue_id_send( &queue_id );
347 371 if (status != RTEMS_SUCCESSFUL)
348 372 {
349 373 PRINTF1("in WFRM *** ERR get_message_queue_id_send %d\n", status);
350 374 }
351 375
352 376 BOOT_PRINTF("in WFRM ***\n");
353 377
354 378 while(1){
355 379 // wait for an RTEMS_EVENT
356 380 rtems_event_receive(RTEMS_EVENT_MODE_NORMAL | RTEMS_EVENT_SWF_RESYNCH,
357 381 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
358 382
359 383 if (event_out == RTEMS_EVENT_MODE_NORMAL)
360 384 {
361 385 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM2\n");
362 386 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
363 387 ring_node_swf1_extracted_ptr->sid = SID_NORM_SWF_F1;
364 388 ring_node_swf2_extracted_ptr->sid = SID_NORM_SWF_F2;
365 389 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
366 390 status = rtems_message_queue_send( queue_id, &ring_node_swf1_extracted_ptr, sizeof( ring_node* ) );
367 391 status = rtems_message_queue_send( queue_id, &ring_node_swf2_extracted_ptr, sizeof( ring_node* ) );
368 392 }
369 393 if (event_out == RTEMS_EVENT_SWF_RESYNCH)
370 394 {
371 395 snapshot_resynchronization( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
372 396 }
373 397 }
374 398 }
375 399
376 400 rtems_task cwf3_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
377 401 {
378 402 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f3.
379 403 *
380 404 * @param unused is the starting argument of the RTEMS task
381 405 *
382 406 * The following data packet is sent by this task:
383 407 * - TM_LFR_SCIENCE_NORMAL_CWF_F3
384 408 *
385 409 */
386 410
387 411 rtems_event_set event_out;
388 412 rtems_id queue_id;
389 413 rtems_status_code status;
390 414 ring_node ring_node_cwf3_light;
391 415 ring_node *ring_node_to_send_cwf;
392 416
393 417 event_out = EVENT_SETS_NONE_PENDING;
394 418 queue_id = RTEMS_ID_NONE;
395 419
396 420 status = get_message_queue_id_send( &queue_id );
397 421 if (status != RTEMS_SUCCESSFUL)
398 422 {
399 423 PRINTF1("in CWF3 *** ERR get_message_queue_id_send %d\n", status)
400 424 }
401 425
402 426 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
403 427
404 428 // init the ring_node_cwf3_light structure
405 429 ring_node_cwf3_light.buffer_address = (int) wf_cont_f3_light;
406 430 ring_node_cwf3_light.coarseTime = INIT_CHAR;
407 431 ring_node_cwf3_light.fineTime = INIT_CHAR;
408 432 ring_node_cwf3_light.next = NULL;
409 433 ring_node_cwf3_light.previous = NULL;
410 434 ring_node_cwf3_light.sid = SID_NORM_CWF_F3;
411 435 ring_node_cwf3_light.status = INIT_CHAR;
412 436
413 437 BOOT_PRINTF("in CWF3 ***\n");
414 438
415 439 while(1){
416 440 // wait for an RTEMS_EVENT
417 441 rtems_event_receive( RTEMS_EVENT_0,
418 442 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
419 443 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
420 444 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode==LFR_MODE_SBM2) )
421 445 {
422 446 ring_node_to_send_cwf = getRingNodeToSendCWF( CHANNELF3 );
423 447 if ( (parameter_dump_packet.sy_lfr_n_cwf_long_f3 & BIT_CWF_LONG_F3) == BIT_CWF_LONG_F3)
424 448 {
425 449 PRINTF("send CWF_LONG_F3\n");
426 450 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
427 451 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
428 452 }
429 453 else
430 454 {
431 455 PRINTF("send CWF_F3 (light)\n");
432 456 send_waveform_CWF3_light( ring_node_to_send_cwf, &ring_node_cwf3_light, queue_id );
433 457 }
434 458
435 459 }
436 460 else
437 461 {
438 462 PRINTF1("in CWF3 *** lfrCurrentMode is %d, no data will be sent\n", lfrCurrentMode)
439 463 }
440 464 }
441 465 }
442 466
443 467 rtems_task cwf2_task(rtems_task_argument argument) // ONLY USED IN BURST AND SBM2
444 468 {
445 469 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f2.
446 470 *
447 471 * @param unused is the starting argument of the RTEMS task
448 472 *
449 473 * The following data packet is sent by this function:
450 474 * - TM_LFR_SCIENCE_BURST_CWF_F2
451 475 * - TM_LFR_SCIENCE_SBM2_CWF_F2
452 476 *
453 477 */
454 478
455 479 rtems_event_set event_out;
456 480 rtems_id queue_id;
457 481 rtems_status_code status;
458 482 ring_node *ring_node_to_send;
459 483 unsigned long long int acquisitionTimeF0_asLong;
460 484
461 485 event_out = EVENT_SETS_NONE_PENDING;
462 486 queue_id = RTEMS_ID_NONE;
463 487
464 488 acquisitionTimeF0_asLong = INIT_CHAR;
465 489
466 490 status = get_message_queue_id_send( &queue_id );
467 491 if (status != RTEMS_SUCCESSFUL)
468 492 {
469 493 PRINTF1("in CWF2 *** ERR get_message_queue_id_send %d\n", status)
470 494 }
471 495
472 496 BOOT_PRINTF("in CWF2 ***\n");
473 497
474 498 while(1){
475 499 // wait for an RTEMS_EVENT// send the snapshot when built
476 500 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM2 );
477 501 rtems_event_receive( RTEMS_EVENT_MODE_NORM_S1_S2 | RTEMS_EVENT_MODE_BURST,
478 502 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
479 503 ring_node_to_send = getRingNodeToSendCWF( CHANNELF2 );
480 504 if (event_out == RTEMS_EVENT_MODE_BURST)
481 505 { // data are sent whatever the transition time
482 506 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
483 507 }
484 508 else if (event_out == RTEMS_EVENT_MODE_NORM_S1_S2)
485 509 {
486 510 if ( lfrCurrentMode == LFR_MODE_SBM2 )
487 511 {
488 512 // data are sent depending on the transition time
489 513 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
490 514 {
491 515 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
492 516 }
493 517 }
494 518 // launch snapshot extraction if needed
495 519 if (extractSWF2 == true)
496 520 {
497 521 ring_node_to_send_swf_f2 = ring_node_to_send_cwf_f2;
498 522 // extract the snapshot
499 523 build_snapshot_from_ring( ring_node_to_send_swf_f2, CHANNELF2, acquisitionTimeF0_asLong,
500 524 &ring_node_swf2_extracted, swf2_extracted );
501 525 extractSWF2 = false;
502 526 swf2_ready = true; // once the snapshot at f2 is ready the CWF1 task will send an event to WFRM
503 527 }
504 528 if (swf0_ready_flag_f2 == true)
505 529 {
506 530 extractSWF2 = true;
507 531 // record the acquition time of the f0 snapshot to use to build the snapshot at f2
508 532 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
509 533 swf0_ready_flag_f2 = false;
510 534 }
511 535 }
512 536 }
513 537 }
514 538
515 539 rtems_task cwf1_task(rtems_task_argument argument) // ONLY USED IN SBM1
516 540 {
517 541 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f1.
518 542 *
519 543 * @param unused is the starting argument of the RTEMS task
520 544 *
521 545 * The following data packet is sent by this function:
522 546 * - TM_LFR_SCIENCE_SBM1_CWF_F1
523 547 *
524 548 */
525 549
526 550 rtems_event_set event_out;
527 551 rtems_id queue_id;
528 552 rtems_status_code status;
529 553
530 554 ring_node *ring_node_to_send_cwf;
531 555
532 556 event_out = EVENT_SETS_NONE_PENDING;
533 557 queue_id = RTEMS_ID_NONE;
534 558
535 559 status = get_message_queue_id_send( &queue_id );
536 560 if (status != RTEMS_SUCCESSFUL)
537 561 {
538 562 PRINTF1("in CWF1 *** ERR get_message_queue_id_send %d\n", status)
539 563 }
540 564
541 565 BOOT_PRINTF("in CWF1 ***\n");
542 566
543 567 while(1){
544 568 // wait for an RTEMS_EVENT
545 569 rtems_event_receive( RTEMS_EVENT_MODE_NORM_S1_S2,
546 570 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
547 571 ring_node_to_send_cwf = getRingNodeToSendCWF( 1 );
548 572 ring_node_to_send_cwf_f1->sid = SID_SBM1_CWF_F1;
549 573 if (lfrCurrentMode == LFR_MODE_SBM1)
550 574 {
551 575 // data are sent depending on the transition time
552 576 if ( time_management_regs->coarse_time >= lastValidEnterModeTime )
553 577 {
554 578 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
555 579 }
556 580 }
557 581 // launch snapshot extraction if needed
558 582 if (extractSWF1 == true)
559 583 {
560 584 ring_node_to_send_swf_f1 = ring_node_to_send_cwf;
561 585 // launch the snapshot extraction
562 586 status = rtems_event_send( Task_id[TASKID_SWBD], RTEMS_EVENT_MODE_NORM_S1_S2 );
563 587 extractSWF1 = false;
564 588 }
565 589 if (swf0_ready_flag_f1 == true)
566 590 {
567 591 extractSWF1 = true;
568 592 swf0_ready_flag_f1 = false; // this step shall be executed only one time
569 593 }
570 594 if ((swf1_ready == true) && (swf2_ready == true)) // swf_f1 is ready after the extraction
571 595 {
572 596 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_NORMAL );
573 597 swf1_ready = false;
574 598 swf2_ready = false;
575 599 }
576 600 }
577 601 }
578 602
579 603 rtems_task swbd_task(rtems_task_argument argument)
580 604 {
581 605 /** This RTEMS task is dedicated to the building of snapshots from different continuous waveforms buffers.
582 606 *
583 607 * @param unused is the starting argument of the RTEMS task
584 608 *
585 609 */
586 610
587 611 rtems_event_set event_out;
588 612 unsigned long long int acquisitionTimeF0_asLong;
589 613
590 614 event_out = EVENT_SETS_NONE_PENDING;
591 615 acquisitionTimeF0_asLong = INIT_CHAR;
592 616
593 617 BOOT_PRINTF("in SWBD ***\n")
594 618
595 619 while(1){
596 620 // wait for an RTEMS_EVENT
597 621 rtems_event_receive( RTEMS_EVENT_MODE_NORM_S1_S2,
598 622 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
599 623 if (event_out == RTEMS_EVENT_MODE_NORM_S1_S2)
600 624 {
601 625 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
602 626 build_snapshot_from_ring( ring_node_to_send_swf_f1, CHANNELF1, acquisitionTimeF0_asLong,
603 627 &ring_node_swf1_extracted, swf1_extracted );
604 628 swf1_ready = true; // the snapshot has been extracted and is ready to be sent
605 629 }
606 630 else
607 631 {
608 632 PRINTF1("in SWBD *** unexpected rtems event received %x\n", (int) event_out)
609 633 }
610 634 }
611 635 }
612 636
613 637 //******************
614 638 // general functions
615 639
616 640 void WFP_init_rings( void )
617 641 {
618 642 // F0 RING
619 643 init_ring( waveform_ring_f0, NB_RING_NODES_F0, wf_buffer_f0, WFRM_BUFFER );
620 644 // F1 RING
621 645 init_ring( waveform_ring_f1, NB_RING_NODES_F1, wf_buffer_f1, WFRM_BUFFER );
622 646 // F2 RING
623 647 init_ring( waveform_ring_f2, NB_RING_NODES_F2, wf_buffer_f2, WFRM_BUFFER );
624 648 // F3 RING
625 649 init_ring( waveform_ring_f3, NB_RING_NODES_F3, wf_buffer_f3, WFRM_BUFFER );
626 650
627 651 ring_node_swf1_extracted.buffer_address = (int) swf1_extracted;
628 652 ring_node_swf2_extracted.buffer_address = (int) swf2_extracted;
629 653
630 654 DEBUG_PRINTF1("waveform_ring_f0 @%x\n", (unsigned int) waveform_ring_f0)
631 655 DEBUG_PRINTF1("waveform_ring_f1 @%x\n", (unsigned int) waveform_ring_f1)
632 656 DEBUG_PRINTF1("waveform_ring_f2 @%x\n", (unsigned int) waveform_ring_f2)
633 657 DEBUG_PRINTF1("waveform_ring_f3 @%x\n", (unsigned int) waveform_ring_f3)
634 658 DEBUG_PRINTF1("wf_buffer_f0 @%x\n", (unsigned int) wf_buffer_f0)
635 659 DEBUG_PRINTF1("wf_buffer_f1 @%x\n", (unsigned int) wf_buffer_f1)
636 660 DEBUG_PRINTF1("wf_buffer_f2 @%x\n", (unsigned int) wf_buffer_f2)
637 661 DEBUG_PRINTF1("wf_buffer_f3 @%x\n", (unsigned int) wf_buffer_f3)
638 662
639 663 }
640 664
641 665 void WFP_reset_current_ring_nodes( void )
642 666 {
643 667 current_ring_node_f0 = waveform_ring_f0[0].next;
644 668 current_ring_node_f1 = waveform_ring_f1[0].next;
645 669 current_ring_node_f2 = waveform_ring_f2[0].next;
646 670 current_ring_node_f3 = waveform_ring_f3[0].next;
647 671
648 672 ring_node_to_send_swf_f0 = waveform_ring_f0;
649 673 ring_node_to_send_swf_f1 = waveform_ring_f1;
650 674 ring_node_to_send_swf_f2 = waveform_ring_f2;
651 675
652 676 ring_node_to_send_cwf_f1 = waveform_ring_f1;
653 677 ring_node_to_send_cwf_f2 = waveform_ring_f2;
654 678 ring_node_to_send_cwf_f3 = waveform_ring_f3;
655 679 }
656 680
657 681 int send_waveform_CWF3_light( ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id )
658 682 {
659 683 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
660 684 *
661 685 * @param waveform points to the buffer containing the data that will be send.
662 686 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
663 687 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
664 688 * contain information to setup the transmission of the data packets.
665 689 *
666 690 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
667 691 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
668 692 *
669 693 */
670 694
671 695 unsigned int i;
672 696 unsigned int j;
673 697 int ret;
674 698 rtems_status_code status;
675 699
676 700 char *sample;
677 701 int *dataPtr;
678 702
679 703 ret = LFR_DEFAULT;
680 704
681 705 dataPtr = (int*) ring_node_to_send->buffer_address;
682 706
683 707 ring_node_cwf3_light->coarseTime = ring_node_to_send->coarseTime;
684 708 ring_node_cwf3_light->fineTime = ring_node_to_send->fineTime;
685 709
686 710 //**********************
687 711 // BUILD CWF3_light DATA
688 712 for ( i=0; i< NB_SAMPLES_PER_SNAPSHOT; i++)
689 713 {
690 714 sample = (char*) &dataPtr[ (i * NB_WORDS_SWF_BLK) ];
691 715 for (j=0; j < CWF_BLK_SIZE; j++)
692 716 {
693 717 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + j] = sample[ j ];
694 718 }
695 719 }
696 720
697 721 // SEND PACKET
698 722 status = rtems_message_queue_send( queue_id, &ring_node_cwf3_light, sizeof( ring_node* ) );
699 723 if (status != RTEMS_SUCCESSFUL) {
700 724 ret = LFR_DEFAULT;
701 725 }
702 726
703 727 return ret;
704 728 }
705 729
706 730 void compute_acquisition_time( unsigned int coarseTime, unsigned int fineTime,
707 731 unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char * acquisitionTime )
708 732 {
709 733 unsigned long long int acquisitionTimeAsLong;
710 734 unsigned char localAcquisitionTime[BYTES_PER_TIME];
711 735 double deltaT;
712 736
713 737 deltaT = INIT_FLOAT;
714 738
715 739 localAcquisitionTime[BYTE_0] = (unsigned char) ( coarseTime >> SHIFT_3_BYTES );
716 740 localAcquisitionTime[BYTE_1] = (unsigned char) ( coarseTime >> SHIFT_2_BYTES );
717 741 localAcquisitionTime[BYTE_2] = (unsigned char) ( coarseTime >> SHIFT_1_BYTE );
718 742 localAcquisitionTime[BYTE_3] = (unsigned char) ( coarseTime );
719 743 localAcquisitionTime[BYTE_4] = (unsigned char) ( fineTime >> SHIFT_1_BYTE );
720 744 localAcquisitionTime[BYTE_5] = (unsigned char) ( fineTime );
721 745
722 746 acquisitionTimeAsLong = ( (unsigned long long int) localAcquisitionTime[BYTE_0] << SHIFT_5_BYTES )
723 747 + ( (unsigned long long int) localAcquisitionTime[BYTE_1] << SHIFT_4_BYTES )
724 748 + ( (unsigned long long int) localAcquisitionTime[BYTE_2] << SHIFT_3_BYTES )
725 749 + ( (unsigned long long int) localAcquisitionTime[BYTE_3] << SHIFT_2_BYTES )
726 750 + ( (unsigned long long int) localAcquisitionTime[BYTE_4] << SHIFT_1_BYTE )
727 751 + ( (unsigned long long int) localAcquisitionTime[BYTE_5] );
728 752
729 753 switch( sid )
730 754 {
731 755 case SID_NORM_SWF_F0:
732 756 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * T0_IN_FINETIME ;
733 757 break;
734 758
735 759 case SID_NORM_SWF_F1:
736 760 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * T1_IN_FINETIME ;
737 761 break;
738 762
739 763 case SID_NORM_SWF_F2:
740 764 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * T2_IN_FINETIME ;
741 765 break;
742 766
743 767 case SID_SBM1_CWF_F1:
744 768 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * T1_IN_FINETIME ;
745 769 break;
746 770
747 771 case SID_SBM2_CWF_F2:
748 772 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * T2_IN_FINETIME ;
749 773 break;
750 774
751 775 case SID_BURST_CWF_F2:
752 776 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * T2_IN_FINETIME ;
753 777 break;
754 778
755 779 case SID_NORM_CWF_F3:
756 780 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF_SHORT_F3 * T3_IN_FINETIME ;
757 781 break;
758 782
759 783 case SID_NORM_CWF_LONG_F3:
760 784 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * T3_IN_FINETIME ;
761 785 break;
762 786
763 787 default:
764 788 PRINTF1("in compute_acquisition_time *** ERR unexpected sid %d\n", sid)
765 789 deltaT = 0.;
766 790 break;
767 791 }
768 792
769 793 acquisitionTimeAsLong = acquisitionTimeAsLong + (unsigned long long int) deltaT;
770 794 //
771 795 acquisitionTime[BYTE_0] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_5_BYTES);
772 796 acquisitionTime[BYTE_1] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_4_BYTES);
773 797 acquisitionTime[BYTE_2] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_3_BYTES);
774 798 acquisitionTime[BYTE_3] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_2_BYTES);
775 799 acquisitionTime[BYTE_4] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_1_BYTE );
776 800 acquisitionTime[BYTE_5] = (unsigned char) (acquisitionTimeAsLong );
777 801
778 802 }
779 803
780 804 void build_snapshot_from_ring( ring_node *ring_node_to_send,
781 805 unsigned char frequencyChannel,
782 806 unsigned long long int acquisitionTimeF0_asLong,
783 807 ring_node *ring_node_swf_extracted,
784 808 int *swf_extracted)
785 809 {
786 810 unsigned int i;
787 811 unsigned int node;
788 812 unsigned long long int centerTime_asLong;
789 813 unsigned long long int acquisitionTime_asLong;
790 814 unsigned long long int bufferAcquisitionTime_asLong;
791 815 unsigned char *ptr1;
792 816 unsigned char *ptr2;
793 817 unsigned char *timeCharPtr;
794 818 unsigned char nb_ring_nodes;
795 819 unsigned long long int frequency_asLong;
796 820 unsigned long long int nbTicksPerSample_asLong;
797 821 unsigned long long int nbSamplesPart1_asLong;
798 822 unsigned long long int sampleOffset_asLong;
799 823
800 824 unsigned int deltaT_F0;
801 825 unsigned int deltaT_F1;
802 826 unsigned long long int deltaT_F2;
803 827
804 828 deltaT_F0 = DELTAT_F0;
805 829 deltaT_F1 = DELTAT_F1;
806 830 deltaT_F2 = DELTAT_F2;
807 831 sampleOffset_asLong = INIT_CHAR;
808 832
809 833 // (1) get the f0 acquisition time => the value is passed in argument
810 834
811 835 // (2) compute the central reference time
812 836 centerTime_asLong = acquisitionTimeF0_asLong + deltaT_F0;
813 837 acquisitionTime_asLong = centerTime_asLong; //set to default value (Don_Initialisation_P2)
814 838 bufferAcquisitionTime_asLong = centerTime_asLong; //set to default value (Don_Initialisation_P2)
815 839 nbTicksPerSample_asLong = TICKS_PER_T2; //set to default value (Don_Initialisation_P2)
816 840
817 841 // (3) compute the acquisition time of the current snapshot
818 842 switch(frequencyChannel)
819 843 {
820 844 case CHANNELF1: // 1 is for F1 = 4096 Hz
821 845 acquisitionTime_asLong = centerTime_asLong - deltaT_F1;
822 846 nb_ring_nodes = NB_RING_NODES_F1;
823 847 frequency_asLong = FREQ_F1;
824 848 nbTicksPerSample_asLong = TICKS_PER_T1; // 65536 / 4096;
825 849 break;
826 850 case CHANNELF2: // 2 is for F2 = 256 Hz
827 851 acquisitionTime_asLong = centerTime_asLong - deltaT_F2;
828 852 nb_ring_nodes = NB_RING_NODES_F2;
829 853 frequency_asLong = FREQ_F2;
830 854 nbTicksPerSample_asLong = TICKS_PER_T2; // 65536 / 256;
831 855 break;
832 856 default:
833 857 acquisitionTime_asLong = centerTime_asLong;
834 858 nb_ring_nodes = 0;
835 859 frequency_asLong = FREQ_F2;
836 860 nbTicksPerSample_asLong = TICKS_PER_T2;
837 861 break;
838 862 }
839 863
840 864 //*****************************************************************************
841 865 // (4) search the ring_node with the acquisition time <= acquisitionTime_asLong
842 866 node = 0;
843 867 while ( node < nb_ring_nodes)
844 868 {
845 869 bufferAcquisitionTime_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send->coarseTime );
846 870 if (bufferAcquisitionTime_asLong <= acquisitionTime_asLong)
847 871 {
848 872 node = nb_ring_nodes;
849 873 }
850 874 else
851 875 {
852 876 node = node + 1;
853 877 ring_node_to_send = ring_node_to_send->previous;
854 878 }
855 879 }
856 880
857 881 // (5) compute the number of samples to take in the current buffer
858 882 sampleOffset_asLong = ((acquisitionTime_asLong - bufferAcquisitionTime_asLong) * frequency_asLong ) >> SHIFT_2_BYTES;
859 883 nbSamplesPart1_asLong = NB_SAMPLES_PER_SNAPSHOT - sampleOffset_asLong;
860 884
861 885 // (6) compute the final acquisition time
862 886 acquisitionTime_asLong = bufferAcquisitionTime_asLong +
863 887 (sampleOffset_asLong * nbTicksPerSample_asLong);
864 888
865 889 // (7) copy the acquisition time at the beginning of the extrated snapshot
866 890 ptr1 = (unsigned char*) &acquisitionTime_asLong;
867 891 // fine time
868 892 ptr2 = (unsigned char*) &ring_node_swf_extracted->fineTime;
869 893 ptr2[BYTE_2] = ptr1[ BYTE_4 + OFFSET_2_BYTES ];
870 894 ptr2[BYTE_3] = ptr1[ BYTE_5 + OFFSET_2_BYTES ];
871 895 // coarse time
872 896 ptr2 = (unsigned char*) &ring_node_swf_extracted->coarseTime;
873 897 ptr2[BYTE_0] = ptr1[ BYTE_0 + OFFSET_2_BYTES ];
874 898 ptr2[BYTE_1] = ptr1[ BYTE_1 + OFFSET_2_BYTES ];
875 899 ptr2[BYTE_2] = ptr1[ BYTE_2 + OFFSET_2_BYTES ];
876 900 ptr2[BYTE_3] = ptr1[ BYTE_3 + OFFSET_2_BYTES ];
877 901
878 902 // re set the synchronization bit
879 903 timeCharPtr = (unsigned char*) &ring_node_to_send->coarseTime;
880 904 ptr2[0] = ptr2[0] | (timeCharPtr[0] & SYNC_BIT); // [1000 0000]
881 905
882 906 if ( (nbSamplesPart1_asLong > NB_SAMPLES_PER_SNAPSHOT) | (nbSamplesPart1_asLong < 0) )
883 907 {
884 908 nbSamplesPart1_asLong = 0;
885 909 }
886 910 // copy the part 1 of the snapshot in the extracted buffer
887 911 for ( i = 0; i < (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i++ )
888 912 {
889 913 swf_extracted[i] =
890 914 ((int*) ring_node_to_send->buffer_address)[ i + (sampleOffset_asLong * NB_WORDS_SWF_BLK) ];
891 915 }
892 916 // copy the part 2 of the snapshot in the extracted buffer
893 917 ring_node_to_send = ring_node_to_send->next;
894 918 for ( i = (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i < (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK); i++ )
895 919 {
896 920 swf_extracted[i] =
897 921 ((int*) ring_node_to_send->buffer_address)[ (i-(nbSamplesPart1_asLong * NB_WORDS_SWF_BLK)) ];
898 922 }
899 923 }
900 924
901 925 double computeCorrection( unsigned char *timePtr )
902 926 {
903 927 unsigned long long int acquisitionTime;
904 928 unsigned long long int centerTime;
905 929 unsigned long long int previousTick;
906 930 unsigned long long int nextTick;
907 931 unsigned long long int deltaPreviousTick;
908 932 unsigned long long int deltaNextTick;
909 933 double deltaPrevious_ms;
910 934 double deltaNext_ms;
911 935 double correctionInF2;
912 936
913 937 correctionInF2 = 0; //set to default value (Don_Initialisation_P2)
914 938
915 939 // get acquisition time in fine time ticks
916 940 acquisitionTime = get_acquisition_time( timePtr );
917 941
918 942 // compute center time
919 943 centerTime = acquisitionTime + DELTAT_F0; // (2048. / 24576. / 2.) * 65536. = 2730.667;
920 944 previousTick = centerTime - (centerTime & INT16_ALL_F);
921 945 nextTick = previousTick + TICKS_PER_S;
922 946
923 947 deltaPreviousTick = centerTime - previousTick;
924 948 deltaNextTick = nextTick - centerTime;
925 949
926 950 deltaPrevious_ms = (((double) deltaPreviousTick) / TICKS_PER_S) * MS_PER_S;
927 951 deltaNext_ms = (((double) deltaNextTick) / TICKS_PER_S) * MS_PER_S;
928 952
929 953 PRINTF2(" delta previous = %.3f ms, delta next = %.2f ms\n", deltaPrevious_ms, deltaNext_ms);
930 954
931 955 // which tick is the closest?
932 956 if (deltaPreviousTick > deltaNextTick)
933 957 {
934 958 // the snapshot center is just before the second => increase delta_snapshot
935 959 correctionInF2 = + (deltaNext_ms * FREQ_F2 / MS_PER_S );
936 960 }
937 961 else
938 962 {
939 963 // the snapshot center is just after the second => decrease delta_snapshot
940 964 correctionInF2 = - (deltaPrevious_ms * FREQ_F2 / MS_PER_S );
941 965 }
942 966
943 967 PRINTF1(" correctionInF2 = %.2f\n", correctionInF2);
944 968
945 969 return correctionInF2;
946 970 }
947 971
948 972 void applyCorrection( double correction )
949 973 {
950 974 int correctionInt;
951 975
952 976 correctionInt = 0;
953 977
954 978 if (correction >= 0.)
955 979 {
956 980 if ( (ONE_TICK_CORR_INTERVAL_0_MIN < correction) && (correction < ONE_TICK_CORR_INTERVAL_0_MAX) )
957 981 {
958 982 correctionInt = ONE_TICK_CORR;
959 983 }
960 984 else
961 985 {
962 986 correctionInt = CORR_MULT * floor(correction);
963 987 }
964 988 }
965 989 else
966 990 {
967 991 if ( (ONE_TICK_CORR_INTERVAL_1_MIN < correction) && (correction < ONE_TICK_CORR_INTERVAL_1_MAX) )
968 992 {
969 993 correctionInt = -ONE_TICK_CORR;
970 994 }
971 995 else
972 996 {
973 997 correctionInt = CORR_MULT * ceil(correction);
974 998 }
975 999 }
976 1000 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot + correctionInt;
977 1001 }
978 1002
979 1003 void snapshot_resynchronization( unsigned char *timePtr )
980 1004 {
981 1005 /** This function compute a correction to apply on delta_snapshot.
982 1006 *
983 1007 *
984 1008 * @param timePtr is a pointer to the acquisition time of the snapshot being considered.
985 1009 *
986 1010 * @return void
987 1011 *
988 1012 */
989 1013
990 1014 static double correction = INIT_FLOAT;
991 1015 static resynchro_state state = MEASURE;
992 1016 static unsigned int nbSnapshots = 0;
993 1017
994 1018 int correctionInt;
995 1019
996 1020 correctionInt = 0;
997 1021
998 1022 switch (state)
999 1023 {
1000 1024
1001 1025 case MEASURE:
1002 1026 // ********
1003 1027 PRINTF1("MEASURE === %d\n", nbSnapshots);
1004 1028 state = CORRECTION;
1005 1029 correction = computeCorrection( timePtr );
1006 1030 PRINTF1("MEASURE === correction = %.2f\n", correction );
1007 1031 applyCorrection( correction );
1008 1032 PRINTF1("MEASURE === delta_snapshot = %d\n", waveform_picker_regs->delta_snapshot);
1009 1033 //****
1010 1034 break;
1011 1035
1012 1036 case CORRECTION:
1013 1037 //************
1014 1038 PRINTF1("CORRECTION === %d\n", nbSnapshots);
1015 1039 state = MEASURE;
1016 1040 computeCorrection( timePtr );
1017 1041 set_wfp_delta_snapshot();
1018 1042 PRINTF1("CORRECTION === delta_snapshot = %d\n", waveform_picker_regs->delta_snapshot);
1019 1043 //****
1020 1044 break;
1021 1045
1022 1046 default:
1023 1047 break;
1024 1048
1025 1049 }
1026 1050
1027 1051 nbSnapshots++;
1028 1052 }
1029 1053
1030 1054 //**************
1031 1055 // wfp registers
1032 1056 void reset_wfp_burst_enable( void )
1033 1057 {
1034 1058 /** This function resets the waveform picker burst_enable register.
1035 1059 *
1036 1060 * The burst bits [f2 f1 f0] and the enable bits [f3 f2 f1 f0] are set to 0.
1037 1061 *
1038 1062 */
1039 1063
1040 1064 // [1000 000] burst f2, f1, f0 enable f3, f2, f1, f0
1041 1065 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable & RST_BITS_RUN_BURST_EN;
1042 1066 }
1043 1067
1044 1068 void reset_wfp_status( void )
1045 1069 {
1046 1070 /** This function resets the waveform picker status register.
1047 1071 *
1048 1072 * All status bits are set to 0 [new_err full_err full].
1049 1073 *
1050 1074 */
1051 1075
1052 1076 waveform_picker_regs->status = INT16_ALL_F;
1053 1077 }
1054 1078
1055 1079 void reset_wfp_buffer_addresses( void )
1056 1080 {
1057 1081 // F0
1058 1082 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->previous->buffer_address; // 0x08
1059 1083 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address; // 0x0c
1060 1084 // F1
1061 1085 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->previous->buffer_address; // 0x10
1062 1086 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address; // 0x14
1063 1087 // F2
1064 1088 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->previous->buffer_address; // 0x18
1065 1089 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; // 0x1c
1066 1090 // F3
1067 1091 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->previous->buffer_address; // 0x20
1068 1092 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address; // 0x24
1069 1093 }
1070 1094
1071 1095 void reset_waveform_picker_regs( void )
1072 1096 {
1073 1097 /** This function resets the waveform picker module registers.
1074 1098 *
1075 1099 * The registers affected by this function are located at the following offset addresses:
1076 1100 * - 0x00 data_shaping
1077 1101 * - 0x04 run_burst_enable
1078 1102 * - 0x08 addr_data_f0
1079 1103 * - 0x0C addr_data_f1
1080 1104 * - 0x10 addr_data_f2
1081 1105 * - 0x14 addr_data_f3
1082 1106 * - 0x18 status
1083 1107 * - 0x1C delta_snapshot
1084 1108 * - 0x20 delta_f0
1085 1109 * - 0x24 delta_f0_2
1086 1110 * - 0x28 delta_f1 (obsolet parameter)
1087 1111 * - 0x2c delta_f2
1088 1112 * - 0x30 nb_data_by_buffer
1089 1113 * - 0x34 nb_snapshot_param
1090 1114 * - 0x38 start_date
1091 1115 * - 0x3c nb_word_in_buffer
1092 1116 *
1093 1117 */
1094 1118
1095 1119 set_wfp_data_shaping(); // 0x00 *** R1 R0 SP1 SP0 BW
1096 1120
1097 1121 reset_wfp_burst_enable(); // 0x04 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
1098 1122
1099 1123 reset_wfp_buffer_addresses();
1100 1124
1101 1125 reset_wfp_status(); // 0x18
1102 1126
1103 1127 set_wfp_delta_snapshot(); // 0x1c *** 300 s => 0x12bff
1104 1128
1105 1129 set_wfp_delta_f0_f0_2(); // 0x20, 0x24
1106 1130
1107 1131 //the parameter delta_f1 [0x28] is not used anymore
1108 1132
1109 1133 set_wfp_delta_f2(); // 0x2c
1110 1134
1111 1135 DEBUG_PRINTF1("delta_snapshot %x\n", waveform_picker_regs->delta_snapshot);
1112 1136 DEBUG_PRINTF1("delta_f0 %x\n", waveform_picker_regs->delta_f0);
1113 1137 DEBUG_PRINTF1("delta_f0_2 %x\n", waveform_picker_regs->delta_f0_2);
1114 1138 DEBUG_PRINTF1("delta_f1 %x\n", waveform_picker_regs->delta_f1);
1115 1139 DEBUG_PRINTF1("delta_f2 %x\n", waveform_picker_regs->delta_f2);
1116 1140 // 2688 = 8 * 336
1117 1141 waveform_picker_regs->nb_data_by_buffer = DFLT_WFP_NB_DATA_BY_BUFFER; // 0x30 *** 2688 - 1 => nb samples -1
1118 1142 waveform_picker_regs->snapshot_param = DFLT_WFP_SNAPSHOT_PARAM; // 0x34 *** 2688 => nb samples
1119 1143 waveform_picker_regs->start_date = COARSE_TIME_MASK;
1120 1144 //
1121 1145 // coarse time and fine time registers are not initialized, they are volatile
1122 1146 //
1123 1147 waveform_picker_regs->buffer_length = DFLT_WFP_BUFFER_LENGTH; // buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
1124 1148 }
1125 1149
1126 1150 void set_wfp_data_shaping( void )
1127 1151 {
1128 1152 /** This function sets the data_shaping register of the waveform picker module.
1129 1153 *
1130 1154 * The value is read from one field of the parameter_dump_packet structure:\n
1131 1155 * bw_sp0_sp1_r0_r1
1132 1156 *
1133 1157 */
1134 1158
1135 1159 unsigned char data_shaping;
1136 1160
1137 1161 // get the parameters for the data shaping [BW SP0 SP1 R0 R1] in sy_lfr_common1 and configure the register
1138 1162 // waveform picker : [R1 R0 SP1 SP0 BW]
1139 1163
1140 1164 data_shaping = parameter_dump_packet.sy_lfr_common_parameters;
1141 1165
1142 1166 waveform_picker_regs->data_shaping =
1143 1167 ( (data_shaping & BIT_5) >> SHIFT_5_BITS ) // BW
1144 1168 + ( (data_shaping & BIT_4) >> SHIFT_3_BITS ) // SP0
1145 1169 + ( (data_shaping & BIT_3) >> 1 ) // SP1
1146 1170 + ( (data_shaping & BIT_2) << 1 ) // R0
1147 1171 + ( (data_shaping & BIT_1) << SHIFT_3_BITS ) // R1
1148 1172 + ( (data_shaping & BIT_0) << SHIFT_5_BITS ); // R2
1149 1173 }
1150 1174
1151 1175 void set_wfp_burst_enable_register( unsigned char mode )
1152 1176 {
1153 1177 /** This function sets the waveform picker burst_enable register depending on the mode.
1154 1178 *
1155 1179 * @param mode is the LFR mode to launch.
1156 1180 *
1157 1181 * The burst bits shall be before the enable bits.
1158 1182 *
1159 1183 */
1160 1184
1161 1185 // [0000 0000] burst f2, f1, f0 enable f3 f2 f1 f0
1162 1186 // the burst bits shall be set first, before the enable bits
1163 1187 switch(mode) {
1164 1188 case LFR_MODE_NORMAL:
1165 1189 case LFR_MODE_SBM1:
1166 1190 case LFR_MODE_SBM2:
1167 1191 waveform_picker_regs->run_burst_enable = RUN_BURST_ENABLE_SBM2; // [0110 0000] enable f2 and f1 burst
1168 1192 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | BITS_WFP_ENABLE_ALL; // [1111] enable f3 f2 f1 f0
1169 1193 break;
1170 1194 case LFR_MODE_BURST:
1171 1195 waveform_picker_regs->run_burst_enable = RUN_BURST_ENABLE_BURST; // [0100 0000] f2 burst enabled
1172 1196 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | BITS_WFP_ENABLE_BURST; // [1100] enable f3 and f2
1173 1197 break;
1174 1198 default:
1175 1199 waveform_picker_regs->run_burst_enable = INIT_CHAR; // [0000 0000] no burst enabled, no waveform enabled
1176 1200 break;
1177 1201 }
1178 1202 }
1179 1203
1180 1204 void set_wfp_delta_snapshot( void )
1181 1205 {
1182 1206 /** This function sets the delta_snapshot register of the waveform picker module.
1183 1207 *
1184 1208 * The value is read from two (unsigned char) of the parameter_dump_packet structure:
1185 1209 * - sy_lfr_n_swf_p[0]
1186 1210 * - sy_lfr_n_swf_p[1]
1187 1211 *
1188 1212 */
1189 1213
1190 1214 unsigned int delta_snapshot;
1191 1215 unsigned int delta_snapshot_in_T2;
1192 1216
1193 1217 delta_snapshot = (parameter_dump_packet.sy_lfr_n_swf_p[0] * CONST_256)
1194 1218 + parameter_dump_packet.sy_lfr_n_swf_p[1];
1195 1219
1196 1220 delta_snapshot_in_T2 = delta_snapshot * FREQ_F2;
1197 1221 waveform_picker_regs->delta_snapshot = delta_snapshot_in_T2 - 1; // max 4 bytes
1198 1222 }
1199 1223
1200 1224 void set_wfp_delta_f0_f0_2( void )
1201 1225 {
1202 1226 unsigned int delta_snapshot;
1203 1227 unsigned int nb_samples_per_snapshot;
1204 1228 float delta_f0_in_float;
1205 1229
1206 1230 delta_snapshot = waveform_picker_regs->delta_snapshot;
1207 1231 nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_swf_l[1];
1208 1232 delta_f0_in_float = (nb_samples_per_snapshot / 2.) * ( (1. / FREQ_F2) - (1. / FREQ_F0) ) * FREQ_F2;
1209 1233
1210 1234 waveform_picker_regs->delta_f0 = delta_snapshot - floor( delta_f0_in_float );
1211 1235 waveform_picker_regs->delta_f0_2 = DFLT_WFP_DELTA_F0_2;
1212 1236 }
1213 1237
1214 1238 void set_wfp_delta_f1( void )
1215 1239 {
1216 1240 /** Sets the value of the delta_f1 parameter
1217 1241 *
1218 1242 * @param void
1219 1243 *
1220 1244 * @return void
1221 1245 *
1222 1246 * delta_f1 is not used, the snapshots are extracted from CWF_F1 waveforms.
1223 1247 *
1224 1248 */
1225 1249
1226 1250 unsigned int delta_snapshot;
1227 1251 unsigned int nb_samples_per_snapshot;
1228 1252 float delta_f1_in_float;
1229 1253
1230 1254 delta_snapshot = waveform_picker_regs->delta_snapshot;
1231 1255 nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_swf_l[1];
1232 1256 delta_f1_in_float = (nb_samples_per_snapshot / 2.) * ( (1. / FREQ_F2) - (1. / FREQ_F1) ) * FREQ_F2;
1233 1257
1234 1258 waveform_picker_regs->delta_f1 = delta_snapshot - floor( delta_f1_in_float );
1235 1259 }
1236 1260
1237 1261 void set_wfp_delta_f2( void ) // parameter not used, only delta_f0 and delta_f0_2 are used
1238 1262 {
1239 1263 /** Sets the value of the delta_f2 parameter
1240 1264 *
1241 1265 * @param void
1242 1266 *
1243 1267 * @return void
1244 1268 *
1245 1269 * delta_f2 is used only for the first snapshot generation, even when the snapshots are extracted from CWF_F2
1246 1270 * waveforms (see lpp_waveform_snapshot_controler.vhd for details).
1247 1271 *
1248 1272 */
1249 1273
1250 1274 unsigned int delta_snapshot;
1251 1275 unsigned int nb_samples_per_snapshot;
1252 1276
1253 1277 delta_snapshot = waveform_picker_regs->delta_snapshot;
1254 1278 nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_swf_l[1];
1255 1279
1256 1280 waveform_picker_regs->delta_f2 = delta_snapshot - (nb_samples_per_snapshot / 2) - 1;
1257 1281 }
1258 1282
1259 1283 //*****************
1260 1284 // local parameters
1261 1285
1262 1286 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid )
1263 1287 {
1264 1288 /** This function increments the parameter "sequence_cnt" depending on the sid passed in argument.
1265 1289 *
1266 1290 * @param packet_sequence_control is a pointer toward the parameter sequence_cnt to update.
1267 1291 * @param sid is the source identifier of the packet being updated.
1268 1292 *
1269 1293 * REQ-LFR-SRS-5240 / SSS-CP-FS-590
1270 1294 * The sequence counters shall wrap around from 2^14 to zero.
1271 1295 * The sequence counter shall start at zero at startup.
1272 1296 *
1273 1297 * REQ-LFR-SRS-5239 / SSS-CP-FS-580
1274 1298 * All TM_LFR_SCIENCE_ packets are sent to ground, i.e. destination id = 0
1275 1299 *
1276 1300 */
1277 1301
1278 1302 unsigned short *sequence_cnt;
1279 1303 unsigned short segmentation_grouping_flag;
1280 1304 unsigned short new_packet_sequence_control;
1281 1305 rtems_mode initial_mode_set;
1282 1306 rtems_mode current_mode_set;
1283 1307 rtems_status_code status;
1284 1308
1285 1309 initial_mode_set = RTEMS_DEFAULT_MODES;
1286 1310 current_mode_set = RTEMS_DEFAULT_MODES;
1287 1311 sequence_cnt = NULL;
1288 1312
1289 1313 //******************************************
1290 1314 // CHANGE THE MODE OF THE CALLING RTEMS TASK
1291 1315 status = rtems_task_mode( RTEMS_NO_PREEMPT, RTEMS_PREEMPT_MASK, &initial_mode_set );
1292 1316
1293 1317 if ( (sid == SID_NORM_SWF_F0) || (sid == SID_NORM_SWF_F1) || (sid == SID_NORM_SWF_F2)
1294 1318 || (sid == SID_NORM_CWF_F3) || (sid == SID_NORM_CWF_LONG_F3)
1295 1319 || (sid == SID_BURST_CWF_F2)
1296 1320 || (sid == SID_NORM_ASM_F0) || (sid == SID_NORM_ASM_F1) || (sid == SID_NORM_ASM_F2)
1297 1321 || (sid == SID_NORM_BP1_F0) || (sid == SID_NORM_BP1_F1) || (sid == SID_NORM_BP1_F2)
1298 1322 || (sid == SID_NORM_BP2_F0) || (sid == SID_NORM_BP2_F1) || (sid == SID_NORM_BP2_F2)
1299 1323 || (sid == SID_BURST_BP1_F0) || (sid == SID_BURST_BP2_F0)
1300 1324 || (sid == SID_BURST_BP1_F1) || (sid == SID_BURST_BP2_F1) )
1301 1325 {
1302 1326 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_NORMAL_BURST;
1303 1327 }
1304 1328 else if ( (sid ==SID_SBM1_CWF_F1) || (sid ==SID_SBM2_CWF_F2)
1305 1329 || (sid == SID_SBM1_BP1_F0) || (sid == SID_SBM1_BP2_F0)
1306 1330 || (sid == SID_SBM2_BP1_F0) || (sid == SID_SBM2_BP2_F0)
1307 1331 || (sid == SID_SBM2_BP1_F1) || (sid == SID_SBM2_BP2_F1) )
1308 1332 {
1309 1333 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_SBM1_SBM2;
1310 1334 }
1311 1335 else
1312 1336 {
1313 1337 sequence_cnt = (unsigned short *) NULL;
1314 1338 PRINTF1("in increment_seq_counter_source_id *** ERR apid_destid %d not known\n", sid)
1315 1339 }
1316 1340
1317 1341 if (sequence_cnt != NULL)
1318 1342 {
1319 1343 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
1320 1344 *sequence_cnt = (*sequence_cnt) & SEQ_CNT_MASK;
1321 1345
1322 1346 new_packet_sequence_control = segmentation_grouping_flag | (*sequence_cnt) ;
1323 1347
1324 1348 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> SHIFT_1_BYTE);
1325 1349 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1326 1350
1327 1351 // increment the sequence counter
1328 1352 if ( *sequence_cnt < SEQ_CNT_MAX)
1329 1353 {
1330 1354 *sequence_cnt = *sequence_cnt + 1;
1331 1355 }
1332 1356 else
1333 1357 {
1334 1358 *sequence_cnt = 0;
1335 1359 }
1336 1360 }
1337 1361
1338 1362 //*************************************
1339 1363 // RESTORE THE MODE OF THE CALLING TASK
1340 1364 status = rtems_task_mode( initial_mode_set, RTEMS_PREEMPT_MASK, &current_mode_set );
1341 1365 }
1342 1366
Show file before | Show file after | Hide comments
@@ -1,93 +1,117
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 #define NB_VALUES_PER_SM 25
2 26 #define NB_BINS_PER_SM 128
3 27
4 28 #define NB_BINS_COMPRESSED_SM_F0 11
5 29 #define ASM_F0_INDICE_START 17 // 88 bins
6 30 #define NB_BINS_TO_AVERAGE_ASM_F0 8
7 31
8 32 void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
9 33 {
10 34 int frequencyBin;
11 35 int asmComponent;
12 36 unsigned int offsetASM;
13 37 unsigned int offsetASMReorganized;
14 38
15 39 // BUILD DATA
16 40 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
17 41 {
18 42 for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
19 43 {
20 44 offsetASMReorganized =
21 45 frequencyBin * NB_VALUES_PER_SM
22 46 + asmComponent;
23 47 offsetASM =
24 48 asmComponent * NB_BINS_PER_SM
25 49 + frequencyBin;
26 50 averaged_spec_mat_reorganized[offsetASMReorganized ] =
27 51 averaged_spec_mat[ offsetASM ] / divider;
28 52 }
29 53 }
30 54 }
31 55
32 56 void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
33 57 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
34 58 {
35 59 int frequencyBin;
36 60 int asmComponent;
37 61 int offsetASM;
38 62 int offsetCompressed;
39 63 int k;
40 64
41 65 // BUILD DATA
42 66 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
43 67 {
44 68 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
45 69 {
46 70 offsetCompressed = // NO TIME OFFSET
47 71 frequencyBin * NB_VALUES_PER_SM
48 72 + asmComponent;
49 73 offsetASM = // NO TIME OFFSET
50 74 asmComponent * NB_BINS_PER_SM
51 75 + ASMIndexStart
52 76 + frequencyBin * nbBinsToAverage;
53 77 compressed_spec_mat[ offsetCompressed ] = 0;
54 78 for ( k = 0; k < nbBinsToAverage; k++ )
55 79 {
56 80 compressed_spec_mat[offsetCompressed ] =
57 81 ( compressed_spec_mat[ offsetCompressed ]
58 82 + averaged_spec_mat[ offsetASM + k ] );
59 83 }
60 84 compressed_spec_mat[ offsetCompressed ] =
61 85 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
62 86 }
63 87 }
64 88 }
65 89
66 90 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
67 91 {
68 92 unsigned int i;
69 93 float re;
70 94 float im;
71 95
72 96 for (i=0; i<NB_BINS_PER_SM; i++){
73 97 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 ];
74 98 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 + 1];
75 99 outputASM[ (asmComponent *NB_BINS_PER_SM) + i] = re;
76 100 outputASM[ (asmComponent+1)*NB_BINS_PER_SM + i] = im;
77 101 }
78 102
79 103 }
80 104
81 105 void ASM_patch( float *inputASM, float *outputASM )
82 106 {
83 107 extractReImVectors( inputASM, outputASM, 1); // b1b2
84 108 extractReImVectors( inputASM, outputASM, 3 ); // b1b3
85 109 extractReImVectors( inputASM, outputASM, 5 ); // b1e1
86 110 extractReImVectors( inputASM, outputASM, 7 ); // b1e2
87 111 extractReImVectors( inputASM, outputASM, 10 ); // b2b3
88 112 extractReImVectors( inputASM, outputASM, 12 ); // b2e1
89 113 extractReImVectors( inputASM, outputASM, 14 ); // b2e2
90 114 extractReImVectors( inputASM, outputASM, 17 ); // b3e1
91 115 extractReImVectors( inputASM, outputASM, 19 ); // b3e2
92 116 extractReImVectors( inputASM, outputASM, 22 ); // e1e2
93 117 }
General Comments 0
You need to be logged in to leave comments. Login now