##// END OF EJS Templates
modulus, tbad, offset, shift are stored in finetime (uint64_t)...
paul -
r355:c502a62ca98f R3++ draft
parent child
Show More
@@ -1,2 +1,2
1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
2 26659466eb11170e587645c796142ac8a7fd0add header/lfr_common_headers
2 e904b329ff977514bf36af92617afefd22fd06ab header/lfr_common_headers
@@ -1,972 +1,974
1 /** This is the RTEMS initialization module.
1 /** This is the RTEMS initialization module.
2 *
2 *
3 * @file
3 * @file
4 * @author P. LEROY
4 * @author P. LEROY
5 *
5 *
6 * This module contains two very different information:
6 * This module contains two very different information:
7 * - specific instructions to configure the compilation of the RTEMS executive
7 * - specific instructions to configure the compilation of the RTEMS executive
8 * - functions related to the fligth softwre initialization, especially the INIT RTEMS task
8 * - functions related to the fligth softwre initialization, especially the INIT RTEMS task
9 *
9 *
10 */
10 */
11
11
12 //*************************
12 //*************************
13 // GPL reminder to be added
13 // GPL reminder to be added
14 //*************************
14 //*************************
15
15
16 #include <rtems.h>
16 #include <rtems.h>
17
17
18 /* configuration information */
18 /* configuration information */
19
19
20 #define CONFIGURE_INIT
20 #define CONFIGURE_INIT
21
21
22 #include <bsp.h> /* for device driver prototypes */
22 #include <bsp.h> /* for device driver prototypes */
23
23
24 /* configuration information */
24 /* configuration information */
25
25
26 #define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
26 #define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
27 #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
27 #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
28
28
29 #define CONFIGURE_MAXIMUM_TASKS 21 // number of tasks concurrently active including INIT
29 #define CONFIGURE_MAXIMUM_TASKS 21 // number of tasks concurrently active including INIT
30 #define CONFIGURE_RTEMS_INIT_TASKS_TABLE
30 #define CONFIGURE_RTEMS_INIT_TASKS_TABLE
31 #define CONFIGURE_EXTRA_TASK_STACKS (3 * RTEMS_MINIMUM_STACK_SIZE)
31 #define CONFIGURE_EXTRA_TASK_STACKS (3 * RTEMS_MINIMUM_STACK_SIZE)
32 #define CONFIGURE_LIBIO_MAXIMUM_FILE_DESCRIPTORS 32
32 #define CONFIGURE_LIBIO_MAXIMUM_FILE_DESCRIPTORS 32
33 #define CONFIGURE_INIT_TASK_PRIORITY 1 // instead of 100
33 #define CONFIGURE_INIT_TASK_PRIORITY 1 // instead of 100
34 #define CONFIGURE_INIT_TASK_MODE (RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT)
34 #define CONFIGURE_INIT_TASK_MODE (RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT)
35 #define CONFIGURE_INIT_TASK_ATTRIBUTES (RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT)
35 #define CONFIGURE_INIT_TASK_ATTRIBUTES (RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT)
36 #define CONFIGURE_MAXIMUM_DRIVERS 16
36 #define CONFIGURE_MAXIMUM_DRIVERS 16
37 #define CONFIGURE_MAXIMUM_PERIODS 6 // [hous] [load] [avgv]
37 #define CONFIGURE_MAXIMUM_PERIODS 6 // [hous] [load] [avgv]
38 #define CONFIGURE_MAXIMUM_TIMERS 6 // [spiq] [link] [spacewire_reset_link]
38 #define CONFIGURE_MAXIMUM_TIMERS 6 // [spiq] [link] [spacewire_reset_link]
39 #define CONFIGURE_MAXIMUM_MESSAGE_QUEUES 5
39 #define CONFIGURE_MAXIMUM_MESSAGE_QUEUES 5
40 #ifdef PRINT_STACK_REPORT
40 #ifdef PRINT_STACK_REPORT
41 #define CONFIGURE_STACK_CHECKER_ENABLED
41 #define CONFIGURE_STACK_CHECKER_ENABLED
42 #endif
42 #endif
43
43
44 #include <rtems/confdefs.h>
44 #include <rtems/confdefs.h>
45
45
46 /* If --drvmgr was enabled during the configuration of the RTEMS kernel */
46 /* If --drvmgr was enabled during the configuration of the RTEMS kernel */
47 #ifdef RTEMS_DRVMGR_STARTUP
47 #ifdef RTEMS_DRVMGR_STARTUP
48 #ifdef LEON3
48 #ifdef LEON3
49 /* Add Timer and UART Driver */
49 /* Add Timer and UART Driver */
50
50
51 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
51 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
52 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
52 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
53 #endif
53 #endif
54
54
55 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
55 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
56 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
56 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
57 #endif
57 #endif
58
58
59 #endif
59 #endif
60 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
60 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
61
61
62 #include <drvmgr/drvmgr_confdefs.h>
62 #include <drvmgr/drvmgr_confdefs.h>
63 #endif
63 #endif
64
64
65 #include "fsw_init.h"
65 #include "fsw_init.h"
66 #include "fsw_config.c"
66 #include "fsw_config.c"
67 #include "GscMemoryLPP.hpp"
67 #include "GscMemoryLPP.hpp"
68
68
69 void initCache()
69 void initCache()
70 {
70 {
71 // ASI 2 contains a few control registers that have not been assigned as ancillary state registers.
71 // ASI 2 contains a few control registers that have not been assigned as ancillary state registers.
72 // These should only be read and written using 32-bit LDA/STA instructions.
72 // These should only be read and written using 32-bit LDA/STA instructions.
73 // All cache registers are accessed through load/store operations to the alternate address space (LDA/STA), using ASI = 2.
73 // All cache registers are accessed through load/store operations to the alternate address space (LDA/STA), using ASI = 2.
74 // The table below shows the register addresses:
74 // The table below shows the register addresses:
75 // 0x00 Cache control register
75 // 0x00 Cache control register
76 // 0x04 Reserved
76 // 0x04 Reserved
77 // 0x08 Instruction cache configuration register
77 // 0x08 Instruction cache configuration register
78 // 0x0C Data cache configuration register
78 // 0x0C Data cache configuration register
79
79
80 // Cache Control Register Leon3 / Leon3FT
80 // Cache Control Register Leon3 / Leon3FT
81 // 31..30 29 28 27..24 23 22 21 20..19 18 17 16
81 // 31..30 29 28 27..24 23 22 21 20..19 18 17 16
82 // RFT PS TB DS FD FI FT ST IB
82 // RFT PS TB DS FD FI FT ST IB
83 // 15 14 13..12 11..10 9..8 7..6 5 4 3..2 1..0
83 // 15 14 13..12 11..10 9..8 7..6 5 4 3..2 1..0
84 // IP DP ITE IDE DTE DDE DF IF DCS ICS
84 // IP DP ITE IDE DTE DDE DF IF DCS ICS
85
85
86 unsigned int cacheControlRegister;
86 unsigned int cacheControlRegister;
87
87
88 CCR_resetCacheControlRegister();
88 CCR_resetCacheControlRegister();
89 ASR16_resetRegisterProtectionControlRegister();
89 ASR16_resetRegisterProtectionControlRegister();
90
90
91 cacheControlRegister = CCR_getValue();
91 cacheControlRegister = CCR_getValue();
92 PRINTF1("(0) CCR - Cache Control Register = %x\n", cacheControlRegister);
92 PRINTF1("(0) CCR - Cache Control Register = %x\n", cacheControlRegister);
93 PRINTF1("(0) ASR16 = %x\n", *asr16Ptr);
93 PRINTF1("(0) ASR16 = %x\n", *asr16Ptr);
94
94
95 CCR_enableInstructionCache(); // ICS bits
95 CCR_enableInstructionCache(); // ICS bits
96 CCR_enableDataCache(); // DCS bits
96 CCR_enableDataCache(); // DCS bits
97 CCR_enableInstructionBurstFetch(); // IB bit
97 CCR_enableInstructionBurstFetch(); // IB bit
98
98
99 faultTolerantScheme();
99 faultTolerantScheme();
100
100
101 cacheControlRegister = CCR_getValue();
101 cacheControlRegister = CCR_getValue();
102 PRINTF1("(1) CCR - Cache Control Register = %x\n", cacheControlRegister);
102 PRINTF1("(1) CCR - Cache Control Register = %x\n", cacheControlRegister);
103 PRINTF1("(1) ASR16 Register protection control register = %x\n", *asr16Ptr);
103 PRINTF1("(1) ASR16 Register protection control register = %x\n", *asr16Ptr);
104
104
105 PRINTF("\n");
105 PRINTF("\n");
106 }
106 }
107
107
108 rtems_task Init( rtems_task_argument ignored )
108 rtems_task Init( rtems_task_argument ignored )
109 {
109 {
110 /** This is the RTEMS INIT taks, it is the first task launched by the system.
110 /** This is the RTEMS INIT taks, it is the first task launched by the system.
111 *
111 *
112 * @param unused is the starting argument of the RTEMS task
112 * @param unused is the starting argument of the RTEMS task
113 *
113 *
114 * The INIT task create and run all other RTEMS tasks.
114 * The INIT task create and run all other RTEMS tasks.
115 *
115 *
116 */
116 */
117
117
118 //***********
118 //***********
119 // INIT CACHE
119 // INIT CACHE
120
120
121 unsigned char *vhdlVersion;
121 unsigned char *vhdlVersion;
122
122
123 reset_lfr();
123 reset_lfr();
124
124
125 reset_local_time();
125 reset_local_time();
126
126
127 rtems_cpu_usage_reset();
127 rtems_cpu_usage_reset();
128
128
129 rtems_status_code status;
129 rtems_status_code status;
130 rtems_status_code status_spw;
130 rtems_status_code status_spw;
131 rtems_isr_entry old_isr_handler;
131 rtems_isr_entry old_isr_handler;
132
132
133 old_isr_handler = NULL;
133 old_isr_handler = NULL;
134
134
135 // UART settings
135 // UART settings
136 enable_apbuart_transmitter();
136 enable_apbuart_transmitter();
137 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
137 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
138
138
139 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
139 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
140
140
141
141
142 PRINTF("\n\n\n\n\n")
142 PRINTF("\n\n\n\n\n")
143
143
144 initCache();
144 initCache();
145
145
146 PRINTF("*************************\n")
146 PRINTF("*************************\n")
147 PRINTF("** LFR Flight Software **\n")
147 PRINTF("** LFR Flight Software **\n")
148
148
149 PRINTF1("** %d-", SW_VERSION_N1)
149 PRINTF1("** %d-", SW_VERSION_N1)
150 PRINTF1("%d-" , SW_VERSION_N2)
150 PRINTF1("%d-" , SW_VERSION_N2)
151 PRINTF1("%d-" , SW_VERSION_N3)
151 PRINTF1("%d-" , SW_VERSION_N3)
152 PRINTF1("%d **\n", SW_VERSION_N4)
152 PRINTF1("%d **\n", SW_VERSION_N4)
153
153
154 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
154 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
155 PRINTF("** VHDL **\n")
155 PRINTF("** VHDL **\n")
156 PRINTF1("** %d-", vhdlVersion[1])
156 PRINTF1("** %d-", vhdlVersion[1])
157 PRINTF1("%d-" , vhdlVersion[2])
157 PRINTF1("%d-" , vhdlVersion[2])
158 PRINTF1("%d **\n", vhdlVersion[3])
158 PRINTF1("%d **\n", vhdlVersion[3])
159 PRINTF("*************************\n")
159 PRINTF("*************************\n")
160 PRINTF("\n\n")
160 PRINTF("\n\n")
161
161
162 init_parameter_dump();
162 init_parameter_dump();
163 init_kcoefficients_dump();
163 init_kcoefficients_dump();
164 init_local_mode_parameters();
164 init_local_mode_parameters();
165 init_housekeeping_parameters();
165 init_housekeeping_parameters();
166 init_k_coefficients_prc0();
166 init_k_coefficients_prc0();
167 init_k_coefficients_prc1();
167 init_k_coefficients_prc1();
168 init_k_coefficients_prc2();
168 init_k_coefficients_prc2();
169 pa_bia_status_info = INIT_CHAR;
169 pa_bia_status_info = INIT_CHAR;
170
170
171 // initialize all reaction wheels frequencies to NaN
171 // initialize all reaction wheels frequencies to NaN
172 rw_f.cp_rpw_sc_rw1_f1 = NAN;
172 rw_f.cp_rpw_sc_rw1_f1 = NAN;
173 rw_f.cp_rpw_sc_rw1_f2 = NAN;
173 rw_f.cp_rpw_sc_rw1_f2 = NAN;
174 rw_f.cp_rpw_sc_rw1_f3 = NAN;
174 rw_f.cp_rpw_sc_rw1_f3 = NAN;
175 rw_f.cp_rpw_sc_rw1_f4 = NAN;
175 rw_f.cp_rpw_sc_rw1_f4 = NAN;
176 rw_f.cp_rpw_sc_rw2_f1 = NAN;
176 rw_f.cp_rpw_sc_rw2_f1 = NAN;
177 rw_f.cp_rpw_sc_rw2_f2 = NAN;
177 rw_f.cp_rpw_sc_rw2_f2 = NAN;
178 rw_f.cp_rpw_sc_rw2_f3 = NAN;
178 rw_f.cp_rpw_sc_rw2_f3 = NAN;
179 rw_f.cp_rpw_sc_rw2_f4 = NAN;
179 rw_f.cp_rpw_sc_rw2_f4 = NAN;
180 rw_f.cp_rpw_sc_rw3_f1 = NAN;
180 rw_f.cp_rpw_sc_rw3_f1 = NAN;
181 rw_f.cp_rpw_sc_rw3_f2 = NAN;
181 rw_f.cp_rpw_sc_rw3_f2 = NAN;
182 rw_f.cp_rpw_sc_rw3_f3 = NAN;
182 rw_f.cp_rpw_sc_rw3_f3 = NAN;
183 rw_f.cp_rpw_sc_rw3_f4 = NAN;
183 rw_f.cp_rpw_sc_rw3_f4 = NAN;
184 rw_f.cp_rpw_sc_rw4_f1 = NAN;
184 rw_f.cp_rpw_sc_rw4_f1 = NAN;
185 rw_f.cp_rpw_sc_rw4_f2 = NAN;
185 rw_f.cp_rpw_sc_rw4_f2 = NAN;
186 rw_f.cp_rpw_sc_rw4_f3 = NAN;
186 rw_f.cp_rpw_sc_rw4_f3 = NAN;
187 rw_f.cp_rpw_sc_rw4_f4 = NAN;
187 rw_f.cp_rpw_sc_rw4_f4 = NAN;
188
188
189 // initialize filtering parameters
189 // initialize filtering parameters
190 filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED;
190 filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED;
191 filterPar.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
191 filterPar.sy_lfr_sc_rw_delta_f = DEFAULT_SY_LFR_SC_RW_DELTA_F;
192 filterPar.sy_lfr_pas_filter_tbad = DEFAULT_SY_LFR_PAS_FILTER_TBAD;
192 filterPar.sy_lfr_pas_filter_tbad = DEFAULT_SY_LFR_PAS_FILTER_TBAD;
193 filterPar.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
194 filterPar.sy_lfr_pas_filter_shift = DEFAULT_SY_LFR_PAS_FILTER_SHIFT;
193 filterPar.sy_lfr_pas_filter_shift = DEFAULT_SY_LFR_PAS_FILTER_SHIFT;
195 filterPar.sy_lfr_sc_rw_delta_f = DEFAULT_SY_LFR_SC_RW_DELTA_F;
194 filterPar.modulus_in_finetime = DEFAULT_MODULUS;
195 filterPar.tbad_in_finetime = DEFAULT_TBAD;
196 filterPar.offset_in_finetime = DEFAULT_OFFSET;
197 filterPar.shift_in_finetime = DEFAULT_SHIFT;
196 update_last_valid_transition_date( DEFAULT_LAST_VALID_TRANSITION_DATE );
198 update_last_valid_transition_date( DEFAULT_LAST_VALID_TRANSITION_DATE );
197
199
198 // waveform picker initialization
200 // waveform picker initialization
199 WFP_init_rings();
201 WFP_init_rings();
200 LEON_Clear_interrupt( IRQ_SPARC_GPTIMER_WATCHDOG ); // initialize the waveform rings
202 LEON_Clear_interrupt( IRQ_SPARC_GPTIMER_WATCHDOG ); // initialize the waveform rings
201 WFP_reset_current_ring_nodes();
203 WFP_reset_current_ring_nodes();
202 reset_waveform_picker_regs();
204 reset_waveform_picker_regs();
203
205
204 // spectral matrices initialization
206 // spectral matrices initialization
205 SM_init_rings(); // initialize spectral matrices rings
207 SM_init_rings(); // initialize spectral matrices rings
206 SM_reset_current_ring_nodes();
208 SM_reset_current_ring_nodes();
207 reset_spectral_matrix_regs();
209 reset_spectral_matrix_regs();
208
210
209 // configure calibration
211 // configure calibration
210 configureCalibration( false ); // true means interleaved mode, false is for normal mode
212 configureCalibration( false ); // true means interleaved mode, false is for normal mode
211
213
212 updateLFRCurrentMode( LFR_MODE_STANDBY );
214 updateLFRCurrentMode( LFR_MODE_STANDBY );
213
215
214 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
216 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
215
217
216 create_names(); // create all names
218 create_names(); // create all names
217
219
218 status = create_timecode_timer(); // create the timer used by timecode_irq_handler
220 status = create_timecode_timer(); // create the timer used by timecode_irq_handler
219 if (status != RTEMS_SUCCESSFUL)
221 if (status != RTEMS_SUCCESSFUL)
220 {
222 {
221 PRINTF1("in INIT *** ERR in create_timer_timecode, code %d", status)
223 PRINTF1("in INIT *** ERR in create_timer_timecode, code %d", status)
222 }
224 }
223
225
224 status = create_message_queues(); // create message queues
226 status = create_message_queues(); // create message queues
225 if (status != RTEMS_SUCCESSFUL)
227 if (status != RTEMS_SUCCESSFUL)
226 {
228 {
227 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
229 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
228 }
230 }
229
231
230 status = create_all_tasks(); // create all tasks
232 status = create_all_tasks(); // create all tasks
231 if (status != RTEMS_SUCCESSFUL)
233 if (status != RTEMS_SUCCESSFUL)
232 {
234 {
233 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
235 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
234 }
236 }
235
237
236 // **************************
238 // **************************
237 // <SPACEWIRE INITIALIZATION>
239 // <SPACEWIRE INITIALIZATION>
238 status_spw = spacewire_open_link(); // (1) open the link
240 status_spw = spacewire_open_link(); // (1) open the link
239 if ( status_spw != RTEMS_SUCCESSFUL )
241 if ( status_spw != RTEMS_SUCCESSFUL )
240 {
242 {
241 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
243 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
242 }
244 }
243
245
244 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
246 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
245 {
247 {
246 status_spw = spacewire_configure_link( fdSPW );
248 status_spw = spacewire_configure_link( fdSPW );
247 if ( status_spw != RTEMS_SUCCESSFUL )
249 if ( status_spw != RTEMS_SUCCESSFUL )
248 {
250 {
249 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
251 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
250 }
252 }
251 }
253 }
252
254
253 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
255 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
254 {
256 {
255 status_spw = spacewire_start_link( fdSPW );
257 status_spw = spacewire_start_link( fdSPW );
256 if ( status_spw != RTEMS_SUCCESSFUL )
258 if ( status_spw != RTEMS_SUCCESSFUL )
257 {
259 {
258 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
260 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
259 }
261 }
260 }
262 }
261 // </SPACEWIRE INITIALIZATION>
263 // </SPACEWIRE INITIALIZATION>
262 // ***************************
264 // ***************************
263
265
264 status = start_all_tasks(); // start all tasks
266 status = start_all_tasks(); // start all tasks
265 if (status != RTEMS_SUCCESSFUL)
267 if (status != RTEMS_SUCCESSFUL)
266 {
268 {
267 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
269 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
268 }
270 }
269
271
270 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
272 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
271 status = start_recv_send_tasks();
273 status = start_recv_send_tasks();
272 if ( status != RTEMS_SUCCESSFUL )
274 if ( status != RTEMS_SUCCESSFUL )
273 {
275 {
274 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
276 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
275 }
277 }
276
278
277 // suspend science tasks, they will be restarted later depending on the mode
279 // suspend science tasks, they will be restarted later depending on the mode
278 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
280 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
279 if (status != RTEMS_SUCCESSFUL)
281 if (status != RTEMS_SUCCESSFUL)
280 {
282 {
281 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
283 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
282 }
284 }
283
285
284 // configure IRQ handling for the waveform picker unit
286 // configure IRQ handling for the waveform picker unit
285 status = rtems_interrupt_catch( waveforms_isr,
287 status = rtems_interrupt_catch( waveforms_isr,
286 IRQ_SPARC_WAVEFORM_PICKER,
288 IRQ_SPARC_WAVEFORM_PICKER,
287 &old_isr_handler) ;
289 &old_isr_handler) ;
288 // configure IRQ handling for the spectral matrices unit
290 // configure IRQ handling for the spectral matrices unit
289 status = rtems_interrupt_catch( spectral_matrices_isr,
291 status = rtems_interrupt_catch( spectral_matrices_isr,
290 IRQ_SPARC_SPECTRAL_MATRIX,
292 IRQ_SPARC_SPECTRAL_MATRIX,
291 &old_isr_handler) ;
293 &old_isr_handler) ;
292
294
293 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
295 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
294 if ( status_spw != RTEMS_SUCCESSFUL )
296 if ( status_spw != RTEMS_SUCCESSFUL )
295 {
297 {
296 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
298 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
297 if ( status != RTEMS_SUCCESSFUL ) {
299 if ( status != RTEMS_SUCCESSFUL ) {
298 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
300 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
299 }
301 }
300 }
302 }
301
303
302 BOOT_PRINTF("delete INIT\n")
304 BOOT_PRINTF("delete INIT\n")
303
305
304 set_hk_lfr_sc_potential_flag( true );
306 set_hk_lfr_sc_potential_flag( true );
305
307
306 // start the timer to detect a missing spacewire timecode
308 // start the timer to detect a missing spacewire timecode
307 // the timeout is larger because the spw IP needs to receive several valid timecodes before generating a tickout
309 // the timeout is larger because the spw IP needs to receive several valid timecodes before generating a tickout
308 // if a tickout is generated, the timer is restarted
310 // if a tickout is generated, the timer is restarted
309 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT_INIT, timecode_timer_routine, NULL );
311 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT_INIT, timecode_timer_routine, NULL );
310
312
311 grspw_timecode_callback = &timecode_irq_handler;
313 grspw_timecode_callback = &timecode_irq_handler;
312
314
313 status = rtems_task_delete(RTEMS_SELF);
315 status = rtems_task_delete(RTEMS_SELF);
314
316
315 }
317 }
316
318
317 void init_local_mode_parameters( void )
319 void init_local_mode_parameters( void )
318 {
320 {
319 /** This function initialize the param_local global variable with default values.
321 /** This function initialize the param_local global variable with default values.
320 *
322 *
321 */
323 */
322
324
323 unsigned int i;
325 unsigned int i;
324
326
325 // LOCAL PARAMETERS
327 // LOCAL PARAMETERS
326
328
327 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
329 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
328 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
330 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
329
331
330 // init sequence counters
332 // init sequence counters
331
333
332 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
334 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
333 {
335 {
334 sequenceCounters_TC_EXE[i] = INIT_CHAR;
336 sequenceCounters_TC_EXE[i] = INIT_CHAR;
335 sequenceCounters_TM_DUMP[i] = INIT_CHAR;
337 sequenceCounters_TM_DUMP[i] = INIT_CHAR;
336 }
338 }
337 sequenceCounters_SCIENCE_NORMAL_BURST = INIT_CHAR;
339 sequenceCounters_SCIENCE_NORMAL_BURST = INIT_CHAR;
338 sequenceCounters_SCIENCE_SBM1_SBM2 = INIT_CHAR;
340 sequenceCounters_SCIENCE_SBM1_SBM2 = INIT_CHAR;
339 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << TM_PACKET_SEQ_SHIFT;
341 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << TM_PACKET_SEQ_SHIFT;
340 }
342 }
341
343
342 void reset_local_time( void )
344 void reset_local_time( void )
343 {
345 {
344 time_management_regs->ctrl = time_management_regs->ctrl | VAL_SOFTWARE_RESET; // [0010] software reset, coarse time = 0x80000000
346 time_management_regs->ctrl = time_management_regs->ctrl | VAL_SOFTWARE_RESET; // [0010] software reset, coarse time = 0x80000000
345 }
347 }
346
348
347 void create_names( void ) // create all names for tasks and queues
349 void create_names( void ) // create all names for tasks and queues
348 {
350 {
349 /** This function creates all RTEMS names used in the software for tasks and queues.
351 /** This function creates all RTEMS names used in the software for tasks and queues.
350 *
352 *
351 * @return RTEMS directive status codes:
353 * @return RTEMS directive status codes:
352 * - RTEMS_SUCCESSFUL - successful completion
354 * - RTEMS_SUCCESSFUL - successful completion
353 *
355 *
354 */
356 */
355
357
356 // task names
358 // task names
357 Task_name[TASKID_AVGV] = rtems_build_name( 'A', 'V', 'G', 'V' );
359 Task_name[TASKID_AVGV] = rtems_build_name( 'A', 'V', 'G', 'V' );
358 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
360 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
359 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
361 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
360 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
362 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
361 Task_name[TASKID_LOAD] = rtems_build_name( 'L', 'O', 'A', 'D' );
363 Task_name[TASKID_LOAD] = rtems_build_name( 'L', 'O', 'A', 'D' );
362 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
364 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
363 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
365 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
364 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
366 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
365 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
367 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
366 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
368 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
367 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
369 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
368 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
370 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
369 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
371 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
370 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
372 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
371 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
373 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
372 Task_name[TASKID_LINK] = rtems_build_name( 'L', 'I', 'N', 'K' );
374 Task_name[TASKID_LINK] = rtems_build_name( 'L', 'I', 'N', 'K' );
373 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
375 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
374 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
376 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
375 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
377 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
376 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
378 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
377
379
378 // rate monotonic period names
380 // rate monotonic period names
379 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
381 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
380 name_avgv_rate_monotonic = rtems_build_name( 'A', 'V', 'G', 'V' );
382 name_avgv_rate_monotonic = rtems_build_name( 'A', 'V', 'G', 'V' );
381
383
382 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
384 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
383 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
385 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
384 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
386 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
385 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
387 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
386 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
388 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
387
389
388 timecode_timer_name = rtems_build_name( 'S', 'P', 'T', 'C' );
390 timecode_timer_name = rtems_build_name( 'S', 'P', 'T', 'C' );
389 }
391 }
390
392
391 int create_all_tasks( void ) // create all tasks which run in the software
393 int create_all_tasks( void ) // create all tasks which run in the software
392 {
394 {
393 /** This function creates all RTEMS tasks used in the software.
395 /** This function creates all RTEMS tasks used in the software.
394 *
396 *
395 * @return RTEMS directive status codes:
397 * @return RTEMS directive status codes:
396 * - RTEMS_SUCCESSFUL - task created successfully
398 * - RTEMS_SUCCESSFUL - task created successfully
397 * - RTEMS_INVALID_ADDRESS - id is NULL
399 * - RTEMS_INVALID_ADDRESS - id is NULL
398 * - RTEMS_INVALID_NAME - invalid task name
400 * - RTEMS_INVALID_NAME - invalid task name
399 * - RTEMS_INVALID_PRIORITY - invalid task priority
401 * - RTEMS_INVALID_PRIORITY - invalid task priority
400 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
402 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
401 * - RTEMS_TOO_MANY - too many tasks created
403 * - RTEMS_TOO_MANY - too many tasks created
402 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
404 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
403 * - RTEMS_TOO_MANY - too many global objects
405 * - RTEMS_TOO_MANY - too many global objects
404 *
406 *
405 */
407 */
406
408
407 rtems_status_code status;
409 rtems_status_code status;
408
410
409 //**********
411 //**********
410 // SPACEWIRE
412 // SPACEWIRE
411 // RECV
413 // RECV
412 status = rtems_task_create(
414 status = rtems_task_create(
413 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
415 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
414 RTEMS_DEFAULT_MODES,
416 RTEMS_DEFAULT_MODES,
415 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
417 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
416 );
418 );
417 if (status == RTEMS_SUCCESSFUL) // SEND
419 if (status == RTEMS_SUCCESSFUL) // SEND
418 {
420 {
419 status = rtems_task_create(
421 status = rtems_task_create(
420 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
422 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
421 RTEMS_DEFAULT_MODES,
423 RTEMS_DEFAULT_MODES,
422 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
424 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
423 );
425 );
424 }
426 }
425 if (status == RTEMS_SUCCESSFUL) // LINK
427 if (status == RTEMS_SUCCESSFUL) // LINK
426 {
428 {
427 status = rtems_task_create(
429 status = rtems_task_create(
428 Task_name[TASKID_LINK], TASK_PRIORITY_LINK, RTEMS_MINIMUM_STACK_SIZE,
430 Task_name[TASKID_LINK], TASK_PRIORITY_LINK, RTEMS_MINIMUM_STACK_SIZE,
429 RTEMS_DEFAULT_MODES,
431 RTEMS_DEFAULT_MODES,
430 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LINK]
432 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LINK]
431 );
433 );
432 }
434 }
433 if (status == RTEMS_SUCCESSFUL) // ACTN
435 if (status == RTEMS_SUCCESSFUL) // ACTN
434 {
436 {
435 status = rtems_task_create(
437 status = rtems_task_create(
436 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
438 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
437 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
439 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
438 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
440 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
439 );
441 );
440 }
442 }
441 if (status == RTEMS_SUCCESSFUL) // SPIQ
443 if (status == RTEMS_SUCCESSFUL) // SPIQ
442 {
444 {
443 status = rtems_task_create(
445 status = rtems_task_create(
444 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
446 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
445 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
447 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
446 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
448 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
447 );
449 );
448 }
450 }
449
451
450 //******************
452 //******************
451 // SPECTRAL MATRICES
453 // SPECTRAL MATRICES
452 if (status == RTEMS_SUCCESSFUL) // AVF0
454 if (status == RTEMS_SUCCESSFUL) // AVF0
453 {
455 {
454 status = rtems_task_create(
456 status = rtems_task_create(
455 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
457 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
456 RTEMS_DEFAULT_MODES,
458 RTEMS_DEFAULT_MODES,
457 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
459 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
458 );
460 );
459 }
461 }
460 if (status == RTEMS_SUCCESSFUL) // PRC0
462 if (status == RTEMS_SUCCESSFUL) // PRC0
461 {
463 {
462 status = rtems_task_create(
464 status = rtems_task_create(
463 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
465 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
464 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
466 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
465 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
467 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
466 );
468 );
467 }
469 }
468 if (status == RTEMS_SUCCESSFUL) // AVF1
470 if (status == RTEMS_SUCCESSFUL) // AVF1
469 {
471 {
470 status = rtems_task_create(
472 status = rtems_task_create(
471 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
473 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
472 RTEMS_DEFAULT_MODES,
474 RTEMS_DEFAULT_MODES,
473 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
475 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
474 );
476 );
475 }
477 }
476 if (status == RTEMS_SUCCESSFUL) // PRC1
478 if (status == RTEMS_SUCCESSFUL) // PRC1
477 {
479 {
478 status = rtems_task_create(
480 status = rtems_task_create(
479 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
481 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
480 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
482 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
481 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
483 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
482 );
484 );
483 }
485 }
484 if (status == RTEMS_SUCCESSFUL) // AVF2
486 if (status == RTEMS_SUCCESSFUL) // AVF2
485 {
487 {
486 status = rtems_task_create(
488 status = rtems_task_create(
487 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
489 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
488 RTEMS_DEFAULT_MODES,
490 RTEMS_DEFAULT_MODES,
489 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
491 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
490 );
492 );
491 }
493 }
492 if (status == RTEMS_SUCCESSFUL) // PRC2
494 if (status == RTEMS_SUCCESSFUL) // PRC2
493 {
495 {
494 status = rtems_task_create(
496 status = rtems_task_create(
495 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
497 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
496 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
498 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
497 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
499 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
498 );
500 );
499 }
501 }
500
502
501 //****************
503 //****************
502 // WAVEFORM PICKER
504 // WAVEFORM PICKER
503 if (status == RTEMS_SUCCESSFUL) // WFRM
505 if (status == RTEMS_SUCCESSFUL) // WFRM
504 {
506 {
505 status = rtems_task_create(
507 status = rtems_task_create(
506 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
508 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
507 RTEMS_DEFAULT_MODES,
509 RTEMS_DEFAULT_MODES,
508 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
510 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
509 );
511 );
510 }
512 }
511 if (status == RTEMS_SUCCESSFUL) // CWF3
513 if (status == RTEMS_SUCCESSFUL) // CWF3
512 {
514 {
513 status = rtems_task_create(
515 status = rtems_task_create(
514 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
516 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
515 RTEMS_DEFAULT_MODES,
517 RTEMS_DEFAULT_MODES,
516 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
518 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
517 );
519 );
518 }
520 }
519 if (status == RTEMS_SUCCESSFUL) // CWF2
521 if (status == RTEMS_SUCCESSFUL) // CWF2
520 {
522 {
521 status = rtems_task_create(
523 status = rtems_task_create(
522 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
524 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
523 RTEMS_DEFAULT_MODES,
525 RTEMS_DEFAULT_MODES,
524 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
526 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
525 );
527 );
526 }
528 }
527 if (status == RTEMS_SUCCESSFUL) // CWF1
529 if (status == RTEMS_SUCCESSFUL) // CWF1
528 {
530 {
529 status = rtems_task_create(
531 status = rtems_task_create(
530 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
532 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
531 RTEMS_DEFAULT_MODES,
533 RTEMS_DEFAULT_MODES,
532 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
534 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
533 );
535 );
534 }
536 }
535 if (status == RTEMS_SUCCESSFUL) // SWBD
537 if (status == RTEMS_SUCCESSFUL) // SWBD
536 {
538 {
537 status = rtems_task_create(
539 status = rtems_task_create(
538 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
540 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
539 RTEMS_DEFAULT_MODES,
541 RTEMS_DEFAULT_MODES,
540 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
542 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
541 );
543 );
542 }
544 }
543
545
544 //*****
546 //*****
545 // MISC
547 // MISC
546 if (status == RTEMS_SUCCESSFUL) // LOAD
548 if (status == RTEMS_SUCCESSFUL) // LOAD
547 {
549 {
548 status = rtems_task_create(
550 status = rtems_task_create(
549 Task_name[TASKID_LOAD], TASK_PRIORITY_LOAD, RTEMS_MINIMUM_STACK_SIZE,
551 Task_name[TASKID_LOAD], TASK_PRIORITY_LOAD, RTEMS_MINIMUM_STACK_SIZE,
550 RTEMS_DEFAULT_MODES,
552 RTEMS_DEFAULT_MODES,
551 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LOAD]
553 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LOAD]
552 );
554 );
553 }
555 }
554 if (status == RTEMS_SUCCESSFUL) // DUMB
556 if (status == RTEMS_SUCCESSFUL) // DUMB
555 {
557 {
556 status = rtems_task_create(
558 status = rtems_task_create(
557 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
559 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
558 RTEMS_DEFAULT_MODES,
560 RTEMS_DEFAULT_MODES,
559 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
561 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
560 );
562 );
561 }
563 }
562 if (status == RTEMS_SUCCESSFUL) // HOUS
564 if (status == RTEMS_SUCCESSFUL) // HOUS
563 {
565 {
564 status = rtems_task_create(
566 status = rtems_task_create(
565 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
567 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
566 RTEMS_DEFAULT_MODES,
568 RTEMS_DEFAULT_MODES,
567 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
569 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
568 );
570 );
569 }
571 }
570 if (status == RTEMS_SUCCESSFUL) // AVGV
572 if (status == RTEMS_SUCCESSFUL) // AVGV
571 {
573 {
572 status = rtems_task_create(
574 status = rtems_task_create(
573 Task_name[TASKID_AVGV], TASK_PRIORITY_AVGV, RTEMS_MINIMUM_STACK_SIZE,
575 Task_name[TASKID_AVGV], TASK_PRIORITY_AVGV, RTEMS_MINIMUM_STACK_SIZE,
574 RTEMS_DEFAULT_MODES,
576 RTEMS_DEFAULT_MODES,
575 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVGV]
577 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVGV]
576 );
578 );
577 }
579 }
578
580
579 return status;
581 return status;
580 }
582 }
581
583
582 int start_recv_send_tasks( void )
584 int start_recv_send_tasks( void )
583 {
585 {
584 rtems_status_code status;
586 rtems_status_code status;
585
587
586 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
588 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
587 if (status!=RTEMS_SUCCESSFUL) {
589 if (status!=RTEMS_SUCCESSFUL) {
588 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
590 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
589 }
591 }
590
592
591 if (status == RTEMS_SUCCESSFUL) // SEND
593 if (status == RTEMS_SUCCESSFUL) // SEND
592 {
594 {
593 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
595 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
594 if (status!=RTEMS_SUCCESSFUL) {
596 if (status!=RTEMS_SUCCESSFUL) {
595 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
597 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
596 }
598 }
597 }
599 }
598
600
599 return status;
601 return status;
600 }
602 }
601
603
602 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
604 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
603 {
605 {
604 /** This function starts all RTEMS tasks used in the software.
606 /** This function starts all RTEMS tasks used in the software.
605 *
607 *
606 * @return RTEMS directive status codes:
608 * @return RTEMS directive status codes:
607 * - RTEMS_SUCCESSFUL - ask started successfully
609 * - RTEMS_SUCCESSFUL - ask started successfully
608 * - RTEMS_INVALID_ADDRESS - invalid task entry point
610 * - RTEMS_INVALID_ADDRESS - invalid task entry point
609 * - RTEMS_INVALID_ID - invalid task id
611 * - RTEMS_INVALID_ID - invalid task id
610 * - RTEMS_INCORRECT_STATE - task not in the dormant state
612 * - RTEMS_INCORRECT_STATE - task not in the dormant state
611 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
613 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
612 *
614 *
613 */
615 */
614 // starts all the tasks fot eh flight software
616 // starts all the tasks fot eh flight software
615
617
616 rtems_status_code status;
618 rtems_status_code status;
617
619
618 //**********
620 //**********
619 // SPACEWIRE
621 // SPACEWIRE
620 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
622 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
621 if (status!=RTEMS_SUCCESSFUL) {
623 if (status!=RTEMS_SUCCESSFUL) {
622 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
624 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
623 }
625 }
624
626
625 if (status == RTEMS_SUCCESSFUL) // LINK
627 if (status == RTEMS_SUCCESSFUL) // LINK
626 {
628 {
627 status = rtems_task_start( Task_id[TASKID_LINK], link_task, 1 );
629 status = rtems_task_start( Task_id[TASKID_LINK], link_task, 1 );
628 if (status!=RTEMS_SUCCESSFUL) {
630 if (status!=RTEMS_SUCCESSFUL) {
629 BOOT_PRINTF("in INIT *** Error starting TASK_LINK\n")
631 BOOT_PRINTF("in INIT *** Error starting TASK_LINK\n")
630 }
632 }
631 }
633 }
632
634
633 if (status == RTEMS_SUCCESSFUL) // ACTN
635 if (status == RTEMS_SUCCESSFUL) // ACTN
634 {
636 {
635 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
637 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
636 if (status!=RTEMS_SUCCESSFUL) {
638 if (status!=RTEMS_SUCCESSFUL) {
637 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
639 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
638 }
640 }
639 }
641 }
640
642
641 //******************
643 //******************
642 // SPECTRAL MATRICES
644 // SPECTRAL MATRICES
643 if (status == RTEMS_SUCCESSFUL) // AVF0
645 if (status == RTEMS_SUCCESSFUL) // AVF0
644 {
646 {
645 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
647 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
646 if (status!=RTEMS_SUCCESSFUL) {
648 if (status!=RTEMS_SUCCESSFUL) {
647 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
649 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
648 }
650 }
649 }
651 }
650 if (status == RTEMS_SUCCESSFUL) // PRC0
652 if (status == RTEMS_SUCCESSFUL) // PRC0
651 {
653 {
652 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
654 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
653 if (status!=RTEMS_SUCCESSFUL) {
655 if (status!=RTEMS_SUCCESSFUL) {
654 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
656 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
655 }
657 }
656 }
658 }
657 if (status == RTEMS_SUCCESSFUL) // AVF1
659 if (status == RTEMS_SUCCESSFUL) // AVF1
658 {
660 {
659 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
661 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
660 if (status!=RTEMS_SUCCESSFUL) {
662 if (status!=RTEMS_SUCCESSFUL) {
661 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
663 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
662 }
664 }
663 }
665 }
664 if (status == RTEMS_SUCCESSFUL) // PRC1
666 if (status == RTEMS_SUCCESSFUL) // PRC1
665 {
667 {
666 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
668 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
667 if (status!=RTEMS_SUCCESSFUL) {
669 if (status!=RTEMS_SUCCESSFUL) {
668 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
670 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
669 }
671 }
670 }
672 }
671 if (status == RTEMS_SUCCESSFUL) // AVF2
673 if (status == RTEMS_SUCCESSFUL) // AVF2
672 {
674 {
673 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
675 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
674 if (status!=RTEMS_SUCCESSFUL) {
676 if (status!=RTEMS_SUCCESSFUL) {
675 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
677 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
676 }
678 }
677 }
679 }
678 if (status == RTEMS_SUCCESSFUL) // PRC2
680 if (status == RTEMS_SUCCESSFUL) // PRC2
679 {
681 {
680 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
682 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
681 if (status!=RTEMS_SUCCESSFUL) {
683 if (status!=RTEMS_SUCCESSFUL) {
682 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
684 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
683 }
685 }
684 }
686 }
685
687
686 //****************
688 //****************
687 // WAVEFORM PICKER
689 // WAVEFORM PICKER
688 if (status == RTEMS_SUCCESSFUL) // WFRM
690 if (status == RTEMS_SUCCESSFUL) // WFRM
689 {
691 {
690 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
692 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
691 if (status!=RTEMS_SUCCESSFUL) {
693 if (status!=RTEMS_SUCCESSFUL) {
692 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
694 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
693 }
695 }
694 }
696 }
695 if (status == RTEMS_SUCCESSFUL) // CWF3
697 if (status == RTEMS_SUCCESSFUL) // CWF3
696 {
698 {
697 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
699 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
698 if (status!=RTEMS_SUCCESSFUL) {
700 if (status!=RTEMS_SUCCESSFUL) {
699 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
701 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
700 }
702 }
701 }
703 }
702 if (status == RTEMS_SUCCESSFUL) // CWF2
704 if (status == RTEMS_SUCCESSFUL) // CWF2
703 {
705 {
704 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
706 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
705 if (status!=RTEMS_SUCCESSFUL) {
707 if (status!=RTEMS_SUCCESSFUL) {
706 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
708 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
707 }
709 }
708 }
710 }
709 if (status == RTEMS_SUCCESSFUL) // CWF1
711 if (status == RTEMS_SUCCESSFUL) // CWF1
710 {
712 {
711 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
713 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
712 if (status!=RTEMS_SUCCESSFUL) {
714 if (status!=RTEMS_SUCCESSFUL) {
713 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
715 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
714 }
716 }
715 }
717 }
716 if (status == RTEMS_SUCCESSFUL) // SWBD
718 if (status == RTEMS_SUCCESSFUL) // SWBD
717 {
719 {
718 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
720 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
719 if (status!=RTEMS_SUCCESSFUL) {
721 if (status!=RTEMS_SUCCESSFUL) {
720 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
722 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
721 }
723 }
722 }
724 }
723
725
724 //*****
726 //*****
725 // MISC
727 // MISC
726 if (status == RTEMS_SUCCESSFUL) // HOUS
728 if (status == RTEMS_SUCCESSFUL) // HOUS
727 {
729 {
728 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
730 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
729 if (status!=RTEMS_SUCCESSFUL) {
731 if (status!=RTEMS_SUCCESSFUL) {
730 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
732 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
731 }
733 }
732 }
734 }
733 if (status == RTEMS_SUCCESSFUL) // AVGV
735 if (status == RTEMS_SUCCESSFUL) // AVGV
734 {
736 {
735 status = rtems_task_start( Task_id[TASKID_AVGV], avgv_task, 1 );
737 status = rtems_task_start( Task_id[TASKID_AVGV], avgv_task, 1 );
736 if (status!=RTEMS_SUCCESSFUL) {
738 if (status!=RTEMS_SUCCESSFUL) {
737 BOOT_PRINTF("in INIT *** Error starting TASK_AVGV\n")
739 BOOT_PRINTF("in INIT *** Error starting TASK_AVGV\n")
738 }
740 }
739 }
741 }
740 if (status == RTEMS_SUCCESSFUL) // DUMB
742 if (status == RTEMS_SUCCESSFUL) // DUMB
741 {
743 {
742 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
744 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
743 if (status!=RTEMS_SUCCESSFUL) {
745 if (status!=RTEMS_SUCCESSFUL) {
744 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
746 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
745 }
747 }
746 }
748 }
747 if (status == RTEMS_SUCCESSFUL) // LOAD
749 if (status == RTEMS_SUCCESSFUL) // LOAD
748 {
750 {
749 status = rtems_task_start( Task_id[TASKID_LOAD], load_task, 1 );
751 status = rtems_task_start( Task_id[TASKID_LOAD], load_task, 1 );
750 if (status!=RTEMS_SUCCESSFUL) {
752 if (status!=RTEMS_SUCCESSFUL) {
751 BOOT_PRINTF("in INIT *** Error starting TASK_LOAD\n")
753 BOOT_PRINTF("in INIT *** Error starting TASK_LOAD\n")
752 }
754 }
753 }
755 }
754
756
755 return status;
757 return status;
756 }
758 }
757
759
758 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
760 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
759 {
761 {
760 rtems_status_code status_recv;
762 rtems_status_code status_recv;
761 rtems_status_code status_send;
763 rtems_status_code status_send;
762 rtems_status_code status_q_p0;
764 rtems_status_code status_q_p0;
763 rtems_status_code status_q_p1;
765 rtems_status_code status_q_p1;
764 rtems_status_code status_q_p2;
766 rtems_status_code status_q_p2;
765 rtems_status_code ret;
767 rtems_status_code ret;
766 rtems_id queue_id;
768 rtems_id queue_id;
767
769
768 ret = RTEMS_SUCCESSFUL;
770 ret = RTEMS_SUCCESSFUL;
769 queue_id = RTEMS_ID_NONE;
771 queue_id = RTEMS_ID_NONE;
770
772
771 //****************************************
773 //****************************************
772 // create the queue for handling valid TCs
774 // create the queue for handling valid TCs
773 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
775 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
774 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
776 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
775 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
777 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
776 if ( status_recv != RTEMS_SUCCESSFUL ) {
778 if ( status_recv != RTEMS_SUCCESSFUL ) {
777 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
779 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
778 }
780 }
779
781
780 //************************************************
782 //************************************************
781 // create the queue for handling TM packet sending
783 // create the queue for handling TM packet sending
782 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
784 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
783 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
785 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
784 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
786 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
785 if ( status_send != RTEMS_SUCCESSFUL ) {
787 if ( status_send != RTEMS_SUCCESSFUL ) {
786 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
788 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
787 }
789 }
788
790
789 //*****************************************************************************
791 //*****************************************************************************
790 // create the queue for handling averaged spectral matrices for processing @ f0
792 // create the queue for handling averaged spectral matrices for processing @ f0
791 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
793 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
792 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
794 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
793 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
795 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
794 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
796 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
795 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
797 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
796 }
798 }
797
799
798 //*****************************************************************************
800 //*****************************************************************************
799 // create the queue for handling averaged spectral matrices for processing @ f1
801 // create the queue for handling averaged spectral matrices for processing @ f1
800 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
802 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
801 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
803 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
802 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
804 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
803 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
805 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
804 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
806 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
805 }
807 }
806
808
807 //*****************************************************************************
809 //*****************************************************************************
808 // create the queue for handling averaged spectral matrices for processing @ f2
810 // create the queue for handling averaged spectral matrices for processing @ f2
809 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
811 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
810 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
812 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
811 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
813 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
812 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
814 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
813 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
815 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
814 }
816 }
815
817
816 if ( status_recv != RTEMS_SUCCESSFUL )
818 if ( status_recv != RTEMS_SUCCESSFUL )
817 {
819 {
818 ret = status_recv;
820 ret = status_recv;
819 }
821 }
820 else if( status_send != RTEMS_SUCCESSFUL )
822 else if( status_send != RTEMS_SUCCESSFUL )
821 {
823 {
822 ret = status_send;
824 ret = status_send;
823 }
825 }
824 else if( status_q_p0 != RTEMS_SUCCESSFUL )
826 else if( status_q_p0 != RTEMS_SUCCESSFUL )
825 {
827 {
826 ret = status_q_p0;
828 ret = status_q_p0;
827 }
829 }
828 else if( status_q_p1 != RTEMS_SUCCESSFUL )
830 else if( status_q_p1 != RTEMS_SUCCESSFUL )
829 {
831 {
830 ret = status_q_p1;
832 ret = status_q_p1;
831 }
833 }
832 else
834 else
833 {
835 {
834 ret = status_q_p2;
836 ret = status_q_p2;
835 }
837 }
836
838
837 return ret;
839 return ret;
838 }
840 }
839
841
840 rtems_status_code create_timecode_timer( void )
842 rtems_status_code create_timecode_timer( void )
841 {
843 {
842 rtems_status_code status;
844 rtems_status_code status;
843
845
844 status = rtems_timer_create( timecode_timer_name, &timecode_timer_id );
846 status = rtems_timer_create( timecode_timer_name, &timecode_timer_id );
845
847
846 if ( status != RTEMS_SUCCESSFUL )
848 if ( status != RTEMS_SUCCESSFUL )
847 {
849 {
848 PRINTF1("in create_timer_timecode *** ERR creating SPTC timer, %d\n", status)
850 PRINTF1("in create_timer_timecode *** ERR creating SPTC timer, %d\n", status)
849 }
851 }
850 else
852 else
851 {
853 {
852 PRINTF("in create_timer_timecode *** OK creating SPTC timer\n")
854 PRINTF("in create_timer_timecode *** OK creating SPTC timer\n")
853 }
855 }
854
856
855 return status;
857 return status;
856 }
858 }
857
859
858 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
860 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
859 {
861 {
860 rtems_status_code status;
862 rtems_status_code status;
861 rtems_name queue_name;
863 rtems_name queue_name;
862
864
863 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
865 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
864
866
865 status = rtems_message_queue_ident( queue_name, 0, queue_id );
867 status = rtems_message_queue_ident( queue_name, 0, queue_id );
866
868
867 return status;
869 return status;
868 }
870 }
869
871
870 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
872 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
871 {
873 {
872 rtems_status_code status;
874 rtems_status_code status;
873 rtems_name queue_name;
875 rtems_name queue_name;
874
876
875 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
877 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
876
878
877 status = rtems_message_queue_ident( queue_name, 0, queue_id );
879 status = rtems_message_queue_ident( queue_name, 0, queue_id );
878
880
879 return status;
881 return status;
880 }
882 }
881
883
882 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
884 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
883 {
885 {
884 rtems_status_code status;
886 rtems_status_code status;
885 rtems_name queue_name;
887 rtems_name queue_name;
886
888
887 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
889 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
888
890
889 status = rtems_message_queue_ident( queue_name, 0, queue_id );
891 status = rtems_message_queue_ident( queue_name, 0, queue_id );
890
892
891 return status;
893 return status;
892 }
894 }
893
895
894 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
896 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
895 {
897 {
896 rtems_status_code status;
898 rtems_status_code status;
897 rtems_name queue_name;
899 rtems_name queue_name;
898
900
899 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
901 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
900
902
901 status = rtems_message_queue_ident( queue_name, 0, queue_id );
903 status = rtems_message_queue_ident( queue_name, 0, queue_id );
902
904
903 return status;
905 return status;
904 }
906 }
905
907
906 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
908 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
907 {
909 {
908 rtems_status_code status;
910 rtems_status_code status;
909 rtems_name queue_name;
911 rtems_name queue_name;
910
912
911 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
913 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
912
914
913 status = rtems_message_queue_ident( queue_name, 0, queue_id );
915 status = rtems_message_queue_ident( queue_name, 0, queue_id );
914
916
915 return status;
917 return status;
916 }
918 }
917
919
918 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max )
920 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max )
919 {
921 {
920 u_int32_t count;
922 u_int32_t count;
921 rtems_status_code status;
923 rtems_status_code status;
922
924
923 count = 0;
925 count = 0;
924
926
925 status = rtems_message_queue_get_number_pending( queue_id, &count );
927 status = rtems_message_queue_get_number_pending( queue_id, &count );
926
928
927 count = count + 1;
929 count = count + 1;
928
930
929 if (status != RTEMS_SUCCESSFUL)
931 if (status != RTEMS_SUCCESSFUL)
930 {
932 {
931 PRINTF1("in update_queue_max_count *** ERR = %d\n", status)
933 PRINTF1("in update_queue_max_count *** ERR = %d\n", status)
932 }
934 }
933 else
935 else
934 {
936 {
935 if (count > *fifo_size_max)
937 if (count > *fifo_size_max)
936 {
938 {
937 *fifo_size_max = count;
939 *fifo_size_max = count;
938 }
940 }
939 }
941 }
940 }
942 }
941
943
942 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
944 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
943 {
945 {
944 unsigned char i;
946 unsigned char i;
945
947
946 //***************
948 //***************
947 // BUFFER ADDRESS
949 // BUFFER ADDRESS
948 for(i=0; i<nbNodes; i++)
950 for(i=0; i<nbNodes; i++)
949 {
951 {
950 ring[i].coarseTime = INT32_ALL_F;
952 ring[i].coarseTime = INT32_ALL_F;
951 ring[i].fineTime = INT32_ALL_F;
953 ring[i].fineTime = INT32_ALL_F;
952 ring[i].sid = INIT_CHAR;
954 ring[i].sid = INIT_CHAR;
953 ring[i].status = INIT_CHAR;
955 ring[i].status = INIT_CHAR;
954 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
956 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
955 }
957 }
956
958
957 //*****
959 //*****
958 // NEXT
960 // NEXT
959 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
961 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
960 for(i=0; i<nbNodes-1; i++)
962 for(i=0; i<nbNodes-1; i++)
961 {
963 {
962 ring[i].next = (ring_node*) &ring[ i + 1 ];
964 ring[i].next = (ring_node*) &ring[ i + 1 ];
963 }
965 }
964
966
965 //*********
967 //*********
966 // PREVIOUS
968 // PREVIOUS
967 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
969 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
968 for(i=1; i<nbNodes; i++)
970 for(i=1; i<nbNodes; i++)
969 {
971 {
970 ring[i].previous = (ring_node*) &ring[ i - 1 ];
972 ring[i].previous = (ring_node*) &ring[ i - 1 ];
971 }
973 }
972 }
974 }
@@ -1,830 +1,840
1 /** Functions related to data processing.
1 /** Functions related to data processing.
2 *
2 *
3 * @file
3 * @file
4 * @author P. LEROY
4 * @author P. LEROY
5 *
5 *
6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 *
7 *
8 */
8 */
9
9
10 #include "fsw_processing.h"
10 #include "fsw_processing.h"
11 #include "fsw_processing_globals.c"
11 #include "fsw_processing_globals.c"
12 #include "fsw_init.h"
12 #include "fsw_init.h"
13
13
14 unsigned int nb_sm_f0 = 0;
14 unsigned int nb_sm_f0 = 0;
15 unsigned int nb_sm_f0_aux_f1= 0;
15 unsigned int nb_sm_f0_aux_f1= 0;
16 unsigned int nb_sm_f1 = 0;
16 unsigned int nb_sm_f1 = 0;
17 unsigned int nb_sm_f0_aux_f2= 0;
17 unsigned int nb_sm_f0_aux_f2= 0;
18
18
19 typedef enum restartState_t
19 typedef enum restartState_t
20 {
20 {
21 WAIT_FOR_F2,
21 WAIT_FOR_F2,
22 WAIT_FOR_F1,
22 WAIT_FOR_F1,
23 WAIT_FOR_F0
23 WAIT_FOR_F0
24 } restartState;
24 } restartState;
25
25
26 //************************
26 //************************
27 // spectral matrices rings
27 // spectral matrices rings
28 ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ] = {0};
28 ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ] = {0};
29 ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ] = {0};
29 ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ] = {0};
30 ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ] = {0};
30 ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ] = {0};
31 ring_node *current_ring_node_sm_f0 = NULL;
31 ring_node *current_ring_node_sm_f0 = NULL;
32 ring_node *current_ring_node_sm_f1 = NULL;
32 ring_node *current_ring_node_sm_f1 = NULL;
33 ring_node *current_ring_node_sm_f2 = NULL;
33 ring_node *current_ring_node_sm_f2 = NULL;
34 ring_node *ring_node_for_averaging_sm_f0= NULL;
34 ring_node *ring_node_for_averaging_sm_f0= NULL;
35 ring_node *ring_node_for_averaging_sm_f1= NULL;
35 ring_node *ring_node_for_averaging_sm_f1= NULL;
36 ring_node *ring_node_for_averaging_sm_f2= NULL;
36 ring_node *ring_node_for_averaging_sm_f2= NULL;
37
37
38 //
38 //
39 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
39 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
40 {
40 {
41 ring_node *node;
41 ring_node *node;
42
42
43 node = NULL;
43 node = NULL;
44 switch ( frequencyChannel ) {
44 switch ( frequencyChannel ) {
45 case CHANNELF0:
45 case CHANNELF0:
46 node = ring_node_for_averaging_sm_f0;
46 node = ring_node_for_averaging_sm_f0;
47 break;
47 break;
48 case CHANNELF1:
48 case CHANNELF1:
49 node = ring_node_for_averaging_sm_f1;
49 node = ring_node_for_averaging_sm_f1;
50 break;
50 break;
51 case CHANNELF2:
51 case CHANNELF2:
52 node = ring_node_for_averaging_sm_f2;
52 node = ring_node_for_averaging_sm_f2;
53 break;
53 break;
54 default:
54 default:
55 break;
55 break;
56 }
56 }
57
57
58 return node;
58 return node;
59 }
59 }
60
60
61 //***********************************************************
61 //***********************************************************
62 // Interrupt Service Routine for spectral matrices processing
62 // Interrupt Service Routine for spectral matrices processing
63
63
64 void spectral_matrices_isr_f0( int statusReg )
64 void spectral_matrices_isr_f0( int statusReg )
65 {
65 {
66 unsigned char status;
66 unsigned char status;
67 rtems_status_code status_code;
67 rtems_status_code status_code;
68 ring_node *full_ring_node;
68 ring_node *full_ring_node;
69
69
70 status = (unsigned char) (statusReg & BITS_STATUS_F0); // [0011] get the status_ready_matrix_f0_x bits
70 status = (unsigned char) (statusReg & BITS_STATUS_F0); // [0011] get the status_ready_matrix_f0_x bits
71
71
72 switch(status)
72 switch(status)
73 {
73 {
74 case 0:
74 case 0:
75 break;
75 break;
76 case BIT_READY_0_1:
76 case BIT_READY_0_1:
77 // UNEXPECTED VALUE
77 // UNEXPECTED VALUE
78 spectral_matrix_regs->status = BIT_READY_0_1; // [0011]
78 spectral_matrix_regs->status = BIT_READY_0_1; // [0011]
79 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
79 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
80 break;
80 break;
81 case BIT_READY_0:
81 case BIT_READY_0:
82 full_ring_node = current_ring_node_sm_f0->previous;
82 full_ring_node = current_ring_node_sm_f0->previous;
83 full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
83 full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
84 full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time;
84 full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time;
85 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
85 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
86 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
86 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
87 // if there are enough ring nodes ready, wake up an AVFx task
87 // if there are enough ring nodes ready, wake up an AVFx task
88 nb_sm_f0 = nb_sm_f0 + 1;
88 nb_sm_f0 = nb_sm_f0 + 1;
89 if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
89 if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
90 {
90 {
91 ring_node_for_averaging_sm_f0 = full_ring_node;
91 ring_node_for_averaging_sm_f0 = full_ring_node;
92 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
92 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
93 {
93 {
94 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
94 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
95 }
95 }
96 nb_sm_f0 = 0;
96 nb_sm_f0 = 0;
97 }
97 }
98 spectral_matrix_regs->status = BIT_READY_0; // [0000 0001]
98 spectral_matrix_regs->status = BIT_READY_0; // [0000 0001]
99 break;
99 break;
100 case BIT_READY_1:
100 case BIT_READY_1:
101 full_ring_node = current_ring_node_sm_f0->previous;
101 full_ring_node = current_ring_node_sm_f0->previous;
102 full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
102 full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
103 full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time;
103 full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time;
104 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
104 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
105 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
105 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
106 // if there are enough ring nodes ready, wake up an AVFx task
106 // if there are enough ring nodes ready, wake up an AVFx task
107 nb_sm_f0 = nb_sm_f0 + 1;
107 nb_sm_f0 = nb_sm_f0 + 1;
108 if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
108 if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
109 {
109 {
110 ring_node_for_averaging_sm_f0 = full_ring_node;
110 ring_node_for_averaging_sm_f0 = full_ring_node;
111 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
111 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
112 {
112 {
113 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
113 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
114 }
114 }
115 nb_sm_f0 = 0;
115 nb_sm_f0 = 0;
116 }
116 }
117 spectral_matrix_regs->status = BIT_READY_1; // [0000 0010]
117 spectral_matrix_regs->status = BIT_READY_1; // [0000 0010]
118 break;
118 break;
119 default:
119 default:
120 break;
120 break;
121 }
121 }
122 }
122 }
123
123
124 void spectral_matrices_isr_f1( int statusReg )
124 void spectral_matrices_isr_f1( int statusReg )
125 {
125 {
126 rtems_status_code status_code;
126 rtems_status_code status_code;
127 unsigned char status;
127 unsigned char status;
128 ring_node *full_ring_node;
128 ring_node *full_ring_node;
129
129
130 status = (unsigned char) ((statusReg & BITS_STATUS_F1) >> SHIFT_2_BITS); // [1100] get the status_ready_matrix_f1_x bits
130 status = (unsigned char) ((statusReg & BITS_STATUS_F1) >> SHIFT_2_BITS); // [1100] get the status_ready_matrix_f1_x bits
131
131
132 switch(status)
132 switch(status)
133 {
133 {
134 case 0:
134 case 0:
135 break;
135 break;
136 case BIT_READY_0_1:
136 case BIT_READY_0_1:
137 // UNEXPECTED VALUE
137 // UNEXPECTED VALUE
138 spectral_matrix_regs->status = BITS_STATUS_F1; // [1100]
138 spectral_matrix_regs->status = BITS_STATUS_F1; // [1100]
139 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
139 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
140 break;
140 break;
141 case BIT_READY_0:
141 case BIT_READY_0:
142 full_ring_node = current_ring_node_sm_f1->previous;
142 full_ring_node = current_ring_node_sm_f1->previous;
143 full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
143 full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
144 full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
144 full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
145 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
145 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
146 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
146 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
147 // if there are enough ring nodes ready, wake up an AVFx task
147 // if there are enough ring nodes ready, wake up an AVFx task
148 nb_sm_f1 = nb_sm_f1 + 1;
148 nb_sm_f1 = nb_sm_f1 + 1;
149 if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
149 if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
150 {
150 {
151 ring_node_for_averaging_sm_f1 = full_ring_node;
151 ring_node_for_averaging_sm_f1 = full_ring_node;
152 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
152 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
153 {
153 {
154 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
154 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
155 }
155 }
156 nb_sm_f1 = 0;
156 nb_sm_f1 = 0;
157 }
157 }
158 spectral_matrix_regs->status = BIT_STATUS_F1_0; // [0000 0100]
158 spectral_matrix_regs->status = BIT_STATUS_F1_0; // [0000 0100]
159 break;
159 break;
160 case BIT_READY_1:
160 case BIT_READY_1:
161 full_ring_node = current_ring_node_sm_f1->previous;
161 full_ring_node = current_ring_node_sm_f1->previous;
162 full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
162 full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
163 full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
163 full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
164 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
164 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
165 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
165 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
166 // if there are enough ring nodes ready, wake up an AVFx task
166 // if there are enough ring nodes ready, wake up an AVFx task
167 nb_sm_f1 = nb_sm_f1 + 1;
167 nb_sm_f1 = nb_sm_f1 + 1;
168 if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
168 if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
169 {
169 {
170 ring_node_for_averaging_sm_f1 = full_ring_node;
170 ring_node_for_averaging_sm_f1 = full_ring_node;
171 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
171 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
172 {
172 {
173 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
173 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
174 }
174 }
175 nb_sm_f1 = 0;
175 nb_sm_f1 = 0;
176 }
176 }
177 spectral_matrix_regs->status = BIT_STATUS_F1_1; // [1000 0000]
177 spectral_matrix_regs->status = BIT_STATUS_F1_1; // [1000 0000]
178 break;
178 break;
179 default:
179 default:
180 break;
180 break;
181 }
181 }
182 }
182 }
183
183
184 void spectral_matrices_isr_f2( int statusReg )
184 void spectral_matrices_isr_f2( int statusReg )
185 {
185 {
186 unsigned char status;
186 unsigned char status;
187 rtems_status_code status_code;
187 rtems_status_code status_code;
188
188
189 status = (unsigned char) ((statusReg & BITS_STATUS_F2) >> SHIFT_4_BITS); // [0011 0000] get the status_ready_matrix_f2_x bits
189 status = (unsigned char) ((statusReg & BITS_STATUS_F2) >> SHIFT_4_BITS); // [0011 0000] get the status_ready_matrix_f2_x bits
190
190
191 switch(status)
191 switch(status)
192 {
192 {
193 case 0:
193 case 0:
194 break;
194 break;
195 case BIT_READY_0_1:
195 case BIT_READY_0_1:
196 // UNEXPECTED VALUE
196 // UNEXPECTED VALUE
197 spectral_matrix_regs->status = BITS_STATUS_F2; // [0011 0000]
197 spectral_matrix_regs->status = BITS_STATUS_F2; // [0011 0000]
198 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
198 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
199 break;
199 break;
200 case BIT_READY_0:
200 case BIT_READY_0:
201 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
201 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
202 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
202 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
203 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
203 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
204 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
204 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
205 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
205 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
206 spectral_matrix_regs->status = BIT_STATUS_F2_0; // [0001 0000]
206 spectral_matrix_regs->status = BIT_STATUS_F2_0; // [0001 0000]
207 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
207 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
208 {
208 {
209 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
209 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
210 }
210 }
211 break;
211 break;
212 case BIT_READY_1:
212 case BIT_READY_1:
213 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
213 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
214 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
214 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
215 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
215 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
216 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
216 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
217 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
217 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
218 spectral_matrix_regs->status = BIT_STATUS_F2_1; // [0010 0000]
218 spectral_matrix_regs->status = BIT_STATUS_F2_1; // [0010 0000]
219 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
219 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
220 {
220 {
221 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
221 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
222 }
222 }
223 break;
223 break;
224 default:
224 default:
225 break;
225 break;
226 }
226 }
227 }
227 }
228
228
229 void spectral_matrix_isr_error_handler( int statusReg )
229 void spectral_matrix_isr_error_handler( int statusReg )
230 {
230 {
231 // STATUS REGISTER
231 // STATUS REGISTER
232 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
232 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
233 // 10 9 8
233 // 10 9 8
234 // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
234 // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
235 // 7 6 5 4 3 2 1 0
235 // 7 6 5 4 3 2 1 0
236 // [bad_component_err] not defined in the last version of the VHDL code
236 // [bad_component_err] not defined in the last version of the VHDL code
237
237
238 rtems_status_code status_code;
238 rtems_status_code status_code;
239
239
240 //***************************************************
240 //***************************************************
241 // the ASM status register is copied in the HK packet
241 // the ASM status register is copied in the HK packet
242 housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) ((statusReg & BITS_HK_AA_SM) >> SHIFT_7_BITS); // [0111 1000 0000]
242 housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) ((statusReg & BITS_HK_AA_SM) >> SHIFT_7_BITS); // [0111 1000 0000]
243
243
244 if (statusReg & BITS_SM_ERR) // [0111 1100 0000]
244 if (statusReg & BITS_SM_ERR) // [0111 1100 0000]
245 {
245 {
246 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
246 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
247 }
247 }
248
248
249 spectral_matrix_regs->status = spectral_matrix_regs->status & BITS_SM_ERR;
249 spectral_matrix_regs->status = spectral_matrix_regs->status & BITS_SM_ERR;
250
250
251 }
251 }
252
252
253 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
253 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
254 {
254 {
255 // STATUS REGISTER
255 // STATUS REGISTER
256 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
256 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
257 // 10 9 8
257 // 10 9 8
258 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
258 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
259 // 7 6 5 4 3 2 1 0
259 // 7 6 5 4 3 2 1 0
260
260
261 int statusReg;
261 int statusReg;
262
262
263 static restartState state = WAIT_FOR_F2;
263 static restartState state = WAIT_FOR_F2;
264
264
265 statusReg = spectral_matrix_regs->status;
265 statusReg = spectral_matrix_regs->status;
266
266
267 if (thisIsAnASMRestart == 0)
267 if (thisIsAnASMRestart == 0)
268 { // this is not a restart sequence, process incoming matrices normally
268 { // this is not a restart sequence, process incoming matrices normally
269 spectral_matrices_isr_f0( statusReg );
269 spectral_matrices_isr_f0( statusReg );
270
270
271 spectral_matrices_isr_f1( statusReg );
271 spectral_matrices_isr_f1( statusReg );
272
272
273 spectral_matrices_isr_f2( statusReg );
273 spectral_matrices_isr_f2( statusReg );
274 }
274 }
275 else
275 else
276 { // a restart sequence has to be launched
276 { // a restart sequence has to be launched
277 switch (state) {
277 switch (state) {
278 case WAIT_FOR_F2:
278 case WAIT_FOR_F2:
279 if ((statusReg & BITS_STATUS_F2) != INIT_CHAR) // [0011 0000] check the status_ready_matrix_f2_x bits
279 if ((statusReg & BITS_STATUS_F2) != INIT_CHAR) // [0011 0000] check the status_ready_matrix_f2_x bits
280 {
280 {
281 state = WAIT_FOR_F1;
281 state = WAIT_FOR_F1;
282 }
282 }
283 break;
283 break;
284 case WAIT_FOR_F1:
284 case WAIT_FOR_F1:
285 if ((statusReg & BITS_STATUS_F1) != INIT_CHAR) // [0000 1100] check the status_ready_matrix_f1_x bits
285 if ((statusReg & BITS_STATUS_F1) != INIT_CHAR) // [0000 1100] check the status_ready_matrix_f1_x bits
286 {
286 {
287 state = WAIT_FOR_F0;
287 state = WAIT_FOR_F0;
288 }
288 }
289 break;
289 break;
290 case WAIT_FOR_F0:
290 case WAIT_FOR_F0:
291 if ((statusReg & BITS_STATUS_F0) != INIT_CHAR) // [0000 0011] check the status_ready_matrix_f0_x bits
291 if ((statusReg & BITS_STATUS_F0) != INIT_CHAR) // [0000 0011] check the status_ready_matrix_f0_x bits
292 {
292 {
293 state = WAIT_FOR_F2;
293 state = WAIT_FOR_F2;
294 thisIsAnASMRestart = 0;
294 thisIsAnASMRestart = 0;
295 }
295 }
296 break;
296 break;
297 default:
297 default:
298 break;
298 break;
299 }
299 }
300 reset_sm_status();
300 reset_sm_status();
301 }
301 }
302
302
303 spectral_matrix_isr_error_handler( statusReg );
303 spectral_matrix_isr_error_handler( statusReg );
304
304
305 }
305 }
306
306
307 //******************
307 //******************
308 // Spectral Matrices
308 // Spectral Matrices
309
309
310 void reset_nb_sm( void )
310 void reset_nb_sm( void )
311 {
311 {
312 nb_sm_f0 = 0;
312 nb_sm_f0 = 0;
313 nb_sm_f0_aux_f1 = 0;
313 nb_sm_f0_aux_f1 = 0;
314 nb_sm_f0_aux_f2 = 0;
314 nb_sm_f0_aux_f2 = 0;
315
315
316 nb_sm_f1 = 0;
316 nb_sm_f1 = 0;
317 }
317 }
318
318
319 void SM_init_rings( void )
319 void SM_init_rings( void )
320 {
320 {
321 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
321 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
322 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
322 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
323 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
323 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
324
324
325 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
325 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
326 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
326 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
327 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
327 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
328 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
328 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
329 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
329 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
330 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
330 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
331 }
331 }
332
332
333 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
333 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
334 {
334 {
335 unsigned char i;
335 unsigned char i;
336
336
337 ring[ nbNodes - 1 ].next
337 ring[ nbNodes - 1 ].next
338 = (ring_node_asm*) &ring[ 0 ];
338 = (ring_node_asm*) &ring[ 0 ];
339
339
340 for(i=0; i<nbNodes-1; i++)
340 for(i=0; i<nbNodes-1; i++)
341 {
341 {
342 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
342 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
343 }
343 }
344 }
344 }
345
345
346 void SM_reset_current_ring_nodes( void )
346 void SM_reset_current_ring_nodes( void )
347 {
347 {
348 current_ring_node_sm_f0 = sm_ring_f0[0].next;
348 current_ring_node_sm_f0 = sm_ring_f0[0].next;
349 current_ring_node_sm_f1 = sm_ring_f1[0].next;
349 current_ring_node_sm_f1 = sm_ring_f1[0].next;
350 current_ring_node_sm_f2 = sm_ring_f2[0].next;
350 current_ring_node_sm_f2 = sm_ring_f2[0].next;
351
351
352 ring_node_for_averaging_sm_f0 = NULL;
352 ring_node_for_averaging_sm_f0 = NULL;
353 ring_node_for_averaging_sm_f1 = NULL;
353 ring_node_for_averaging_sm_f1 = NULL;
354 ring_node_for_averaging_sm_f2 = NULL;
354 ring_node_for_averaging_sm_f2 = NULL;
355 }
355 }
356
356
357 //*****************
357 //*****************
358 // Basic Parameters
358 // Basic Parameters
359
359
360 void BP_init_header( bp_packet *packet,
360 void BP_init_header( bp_packet *packet,
361 unsigned int apid, unsigned char sid,
361 unsigned int apid, unsigned char sid,
362 unsigned int packetLength, unsigned char blkNr )
362 unsigned int packetLength, unsigned char blkNr )
363 {
363 {
364 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
364 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
365 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
365 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
366 packet->reserved = INIT_CHAR;
366 packet->reserved = INIT_CHAR;
367 packet->userApplication = CCSDS_USER_APP;
367 packet->userApplication = CCSDS_USER_APP;
368 packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
368 packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
369 packet->packetID[1] = (unsigned char) (apid);
369 packet->packetID[1] = (unsigned char) (apid);
370 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
370 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
371 packet->packetSequenceControl[1] = INIT_CHAR;
371 packet->packetSequenceControl[1] = INIT_CHAR;
372 packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
372 packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
373 packet->packetLength[1] = (unsigned char) (packetLength);
373 packet->packetLength[1] = (unsigned char) (packetLength);
374 // DATA FIELD HEADER
374 // DATA FIELD HEADER
375 packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
375 packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
376 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
376 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
377 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
377 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
378 packet->destinationID = TM_DESTINATION_ID_GROUND;
378 packet->destinationID = TM_DESTINATION_ID_GROUND;
379 packet->time[BYTE_0] = INIT_CHAR;
379 packet->time[BYTE_0] = INIT_CHAR;
380 packet->time[BYTE_1] = INIT_CHAR;
380 packet->time[BYTE_1] = INIT_CHAR;
381 packet->time[BYTE_2] = INIT_CHAR;
381 packet->time[BYTE_2] = INIT_CHAR;
382 packet->time[BYTE_3] = INIT_CHAR;
382 packet->time[BYTE_3] = INIT_CHAR;
383 packet->time[BYTE_4] = INIT_CHAR;
383 packet->time[BYTE_4] = INIT_CHAR;
384 packet->time[BYTE_5] = INIT_CHAR;
384 packet->time[BYTE_5] = INIT_CHAR;
385 // AUXILIARY DATA HEADER
385 // AUXILIARY DATA HEADER
386 packet->sid = sid;
386 packet->sid = sid;
387 packet->pa_bia_status_info = INIT_CHAR;
387 packet->pa_bia_status_info = INIT_CHAR;
388 packet->sy_lfr_common_parameters_spare = INIT_CHAR;
388 packet->sy_lfr_common_parameters_spare = INIT_CHAR;
389 packet->sy_lfr_common_parameters = INIT_CHAR;
389 packet->sy_lfr_common_parameters = INIT_CHAR;
390 packet->acquisitionTime[BYTE_0] = INIT_CHAR;
390 packet->acquisitionTime[BYTE_0] = INIT_CHAR;
391 packet->acquisitionTime[BYTE_1] = INIT_CHAR;
391 packet->acquisitionTime[BYTE_1] = INIT_CHAR;
392 packet->acquisitionTime[BYTE_2] = INIT_CHAR;
392 packet->acquisitionTime[BYTE_2] = INIT_CHAR;
393 packet->acquisitionTime[BYTE_3] = INIT_CHAR;
393 packet->acquisitionTime[BYTE_3] = INIT_CHAR;
394 packet->acquisitionTime[BYTE_4] = INIT_CHAR;
394 packet->acquisitionTime[BYTE_4] = INIT_CHAR;
395 packet->acquisitionTime[BYTE_5] = INIT_CHAR;
395 packet->acquisitionTime[BYTE_5] = INIT_CHAR;
396 packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB
396 packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB
397 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
397 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
398 }
398 }
399
399
400 void BP_init_header_with_spare( bp_packet_with_spare *packet,
400 void BP_init_header_with_spare( bp_packet_with_spare *packet,
401 unsigned int apid, unsigned char sid,
401 unsigned int apid, unsigned char sid,
402 unsigned int packetLength , unsigned char blkNr)
402 unsigned int packetLength , unsigned char blkNr)
403 {
403 {
404 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
404 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
405 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
405 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
406 packet->reserved = INIT_CHAR;
406 packet->reserved = INIT_CHAR;
407 packet->userApplication = CCSDS_USER_APP;
407 packet->userApplication = CCSDS_USER_APP;
408 packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
408 packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
409 packet->packetID[1] = (unsigned char) (apid);
409 packet->packetID[1] = (unsigned char) (apid);
410 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
410 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
411 packet->packetSequenceControl[1] = INIT_CHAR;
411 packet->packetSequenceControl[1] = INIT_CHAR;
412 packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
412 packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
413 packet->packetLength[1] = (unsigned char) (packetLength);
413 packet->packetLength[1] = (unsigned char) (packetLength);
414 // DATA FIELD HEADER
414 // DATA FIELD HEADER
415 packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
415 packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
416 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
416 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
417 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
417 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
418 packet->destinationID = TM_DESTINATION_ID_GROUND;
418 packet->destinationID = TM_DESTINATION_ID_GROUND;
419 // AUXILIARY DATA HEADER
419 // AUXILIARY DATA HEADER
420 packet->sid = sid;
420 packet->sid = sid;
421 packet->pa_bia_status_info = INIT_CHAR;
421 packet->pa_bia_status_info = INIT_CHAR;
422 packet->sy_lfr_common_parameters_spare = INIT_CHAR;
422 packet->sy_lfr_common_parameters_spare = INIT_CHAR;
423 packet->sy_lfr_common_parameters = INIT_CHAR;
423 packet->sy_lfr_common_parameters = INIT_CHAR;
424 packet->time[BYTE_0] = INIT_CHAR;
424 packet->time[BYTE_0] = INIT_CHAR;
425 packet->time[BYTE_1] = INIT_CHAR;
425 packet->time[BYTE_1] = INIT_CHAR;
426 packet->time[BYTE_2] = INIT_CHAR;
426 packet->time[BYTE_2] = INIT_CHAR;
427 packet->time[BYTE_3] = INIT_CHAR;
427 packet->time[BYTE_3] = INIT_CHAR;
428 packet->time[BYTE_4] = INIT_CHAR;
428 packet->time[BYTE_4] = INIT_CHAR;
429 packet->time[BYTE_5] = INIT_CHAR;
429 packet->time[BYTE_5] = INIT_CHAR;
430 packet->source_data_spare = INIT_CHAR;
430 packet->source_data_spare = INIT_CHAR;
431 packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB
431 packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB
432 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
432 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
433 }
433 }
434
434
435 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
435 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
436 {
436 {
437 rtems_status_code status;
437 rtems_status_code status;
438
438
439 // SEND PACKET
439 // SEND PACKET
440 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
440 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
441 if (status != RTEMS_SUCCESSFUL)
441 if (status != RTEMS_SUCCESSFUL)
442 {
442 {
443 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
443 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
444 }
444 }
445 }
445 }
446
446
447 void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
447 void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
448 {
448 {
449 /** This function is used to send the BP paquets when needed.
449 /** This function is used to send the BP paquets when needed.
450 *
450 *
451 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
451 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
452 *
452 *
453 * @return void
453 * @return void
454 *
454 *
455 * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition.
455 * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition.
456 * BURST paquets are sent everytime.
456 * BURST paquets are sent everytime.
457 *
457 *
458 */
458 */
459
459
460 rtems_status_code status;
460 rtems_status_code status;
461
461
462 // SEND PACKET
462 // SEND PACKET
463 // before lastValidTransitionDate, the data are drops even if they are ready
463 // before lastValidTransitionDate, the data are drops even if they are ready
464 // this guarantees that no SBM packets will be received before the requested enter mode time
464 // this guarantees that no SBM packets will be received before the requested enter mode time
465 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
465 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
466 {
466 {
467 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
467 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
468 if (status != RTEMS_SUCCESSFUL)
468 if (status != RTEMS_SUCCESSFUL)
469 {
469 {
470 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
470 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
471 }
471 }
472 }
472 }
473 }
473 }
474
474
475 //******************
475 //******************
476 // general functions
476 // general functions
477
477
478 void reset_sm_status( void )
478 void reset_sm_status( void )
479 {
479 {
480 // error
480 // error
481 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
481 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
482 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
482 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
483 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
483 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
484 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
484 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
485
485
486 spectral_matrix_regs->status = BITS_STATUS_REG; // [0111 1111 1111]
486 spectral_matrix_regs->status = BITS_STATUS_REG; // [0111 1111 1111]
487 }
487 }
488
488
489 void reset_spectral_matrix_regs( void )
489 void reset_spectral_matrix_regs( void )
490 {
490 {
491 /** This function resets the spectral matrices module registers.
491 /** This function resets the spectral matrices module registers.
492 *
492 *
493 * The registers affected by this function are located at the following offset addresses:
493 * The registers affected by this function are located at the following offset addresses:
494 *
494 *
495 * - 0x00 config
495 * - 0x00 config
496 * - 0x04 status
496 * - 0x04 status
497 * - 0x08 matrixF0_Address0
497 * - 0x08 matrixF0_Address0
498 * - 0x10 matrixFO_Address1
498 * - 0x10 matrixFO_Address1
499 * - 0x14 matrixF1_Address
499 * - 0x14 matrixF1_Address
500 * - 0x18 matrixF2_Address
500 * - 0x18 matrixF2_Address
501 *
501 *
502 */
502 */
503
503
504 set_sm_irq_onError( 0 );
504 set_sm_irq_onError( 0 );
505
505
506 set_sm_irq_onNewMatrix( 0 );
506 set_sm_irq_onNewMatrix( 0 );
507
507
508 reset_sm_status();
508 reset_sm_status();
509
509
510 // F1
510 // F1
511 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
511 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
512 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
512 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
513 // F2
513 // F2
514 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
514 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
515 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
515 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
516 // F3
516 // F3
517 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
517 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
518 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
518 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
519
519
520 spectral_matrix_regs->matrix_length = DEFAULT_MATRIX_LENGTH; // 25 * 128 / 16 = 200 = 0xc8
520 spectral_matrix_regs->matrix_length = DEFAULT_MATRIX_LENGTH; // 25 * 128 / 16 = 200 = 0xc8
521 }
521 }
522
522
523 void set_time( unsigned char *time, unsigned char * timeInBuffer )
523 void set_time( unsigned char *time, unsigned char * timeInBuffer )
524 {
524 {
525 time[BYTE_0] = timeInBuffer[BYTE_0];
525 time[BYTE_0] = timeInBuffer[BYTE_0];
526 time[BYTE_1] = timeInBuffer[BYTE_1];
526 time[BYTE_1] = timeInBuffer[BYTE_1];
527 time[BYTE_2] = timeInBuffer[BYTE_2];
527 time[BYTE_2] = timeInBuffer[BYTE_2];
528 time[BYTE_3] = timeInBuffer[BYTE_3];
528 time[BYTE_3] = timeInBuffer[BYTE_3];
529 time[BYTE_4] = timeInBuffer[BYTE_6];
529 time[BYTE_4] = timeInBuffer[BYTE_6];
530 time[BYTE_5] = timeInBuffer[BYTE_7];
530 time[BYTE_5] = timeInBuffer[BYTE_7];
531 }
531 }
532
532
533 unsigned long long int get_acquisition_time( unsigned char *timePtr )
533 unsigned long long int get_acquisition_time( unsigned char *timePtr )
534 {
534 {
535 unsigned long long int acquisitionTimeAslong;
535 unsigned long long int acquisitionTimeAslong;
536 acquisitionTimeAslong = INIT_CHAR;
536 acquisitionTimeAslong = INIT_CHAR;
537 acquisitionTimeAslong =
537 acquisitionTimeAslong =
538 ( (unsigned long long int) (timePtr[BYTE_0] & SYNC_BIT_MASK) << SHIFT_5_BYTES ) // [0111 1111] mask the synchronization bit
538 ( (unsigned long long int) (timePtr[BYTE_0] & SYNC_BIT_MASK) << SHIFT_5_BYTES ) // [0111 1111] mask the synchronization bit
539 + ( (unsigned long long int) timePtr[BYTE_1] << SHIFT_4_BYTES )
539 + ( (unsigned long long int) timePtr[BYTE_1] << SHIFT_4_BYTES )
540 + ( (unsigned long long int) timePtr[BYTE_2] << SHIFT_3_BYTES )
540 + ( (unsigned long long int) timePtr[BYTE_2] << SHIFT_3_BYTES )
541 + ( (unsigned long long int) timePtr[BYTE_3] << SHIFT_2_BYTES )
541 + ( (unsigned long long int) timePtr[BYTE_3] << SHIFT_2_BYTES )
542 + ( (unsigned long long int) timePtr[BYTE_6] << SHIFT_1_BYTE )
542 + ( (unsigned long long int) timePtr[BYTE_6] << SHIFT_1_BYTE )
543 + ( (unsigned long long int) timePtr[BYTE_7] );
543 + ( (unsigned long long int) timePtr[BYTE_7] );
544 return acquisitionTimeAslong;
544 return acquisitionTimeAslong;
545 }
545 }
546
546
547 unsigned char getSID( rtems_event_set event )
547 unsigned char getSID( rtems_event_set event )
548 {
548 {
549 unsigned char sid;
549 unsigned char sid;
550
550
551 rtems_event_set eventSetBURST;
551 rtems_event_set eventSetBURST;
552 rtems_event_set eventSetSBM;
552 rtems_event_set eventSetSBM;
553
553
554 sid = 0;
554 sid = 0;
555
555
556 //******
556 //******
557 // BURST
557 // BURST
558 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
558 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
559 | RTEMS_EVENT_BURST_BP1_F1
559 | RTEMS_EVENT_BURST_BP1_F1
560 | RTEMS_EVENT_BURST_BP2_F0
560 | RTEMS_EVENT_BURST_BP2_F0
561 | RTEMS_EVENT_BURST_BP2_F1;
561 | RTEMS_EVENT_BURST_BP2_F1;
562
562
563 //****
563 //****
564 // SBM
564 // SBM
565 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
565 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
566 | RTEMS_EVENT_SBM_BP1_F1
566 | RTEMS_EVENT_SBM_BP1_F1
567 | RTEMS_EVENT_SBM_BP2_F0
567 | RTEMS_EVENT_SBM_BP2_F0
568 | RTEMS_EVENT_SBM_BP2_F1;
568 | RTEMS_EVENT_SBM_BP2_F1;
569
569
570 if (event & eventSetBURST)
570 if (event & eventSetBURST)
571 {
571 {
572 sid = SID_BURST_BP1_F0;
572 sid = SID_BURST_BP1_F0;
573 }
573 }
574 else if (event & eventSetSBM)
574 else if (event & eventSetSBM)
575 {
575 {
576 sid = SID_SBM1_BP1_F0;
576 sid = SID_SBM1_BP1_F0;
577 }
577 }
578 else
578 else
579 {
579 {
580 sid = 0;
580 sid = 0;
581 }
581 }
582
582
583 return sid;
583 return sid;
584 }
584 }
585
585
586 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
586 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
587 {
587 {
588 unsigned int i;
588 unsigned int i;
589 float re;
589 float re;
590 float im;
590 float im;
591
591
592 for (i=0; i<NB_BINS_PER_SM; i++){
592 for (i=0; i<NB_BINS_PER_SM; i++){
593 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) ];
593 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) ];
594 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) + 1];
594 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) + 1];
595 outputASM[ ( asmComponent *NB_BINS_PER_SM) + i] = re;
595 outputASM[ ( asmComponent *NB_BINS_PER_SM) + i] = re;
596 outputASM[ ((asmComponent+1)*NB_BINS_PER_SM) + i] = im;
596 outputASM[ ((asmComponent+1)*NB_BINS_PER_SM) + i] = im;
597 }
597 }
598 }
598 }
599
599
600 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
600 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
601 {
601 {
602 unsigned int i;
602 unsigned int i;
603 float re;
603 float re;
604
604
605 for (i=0; i<NB_BINS_PER_SM; i++){
605 for (i=0; i<NB_BINS_PER_SM; i++){
606 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
606 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
607 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
607 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
608 }
608 }
609 }
609 }
610
610
611 void ASM_patch( float *inputASM, float *outputASM )
611 void ASM_patch( float *inputASM, float *outputASM )
612 {
612 {
613 extractReImVectors( inputASM, outputASM, ASM_COMP_B1B2); // b1b2
613 extractReImVectors( inputASM, outputASM, ASM_COMP_B1B2); // b1b2
614 extractReImVectors( inputASM, outputASM, ASM_COMP_B1B3 ); // b1b3
614 extractReImVectors( inputASM, outputASM, ASM_COMP_B1B3 ); // b1b3
615 extractReImVectors( inputASM, outputASM, ASM_COMP_B1E1 ); // b1e1
615 extractReImVectors( inputASM, outputASM, ASM_COMP_B1E1 ); // b1e1
616 extractReImVectors( inputASM, outputASM, ASM_COMP_B1E2 ); // b1e2
616 extractReImVectors( inputASM, outputASM, ASM_COMP_B1E2 ); // b1e2
617 extractReImVectors( inputASM, outputASM, ASM_COMP_B2B3 ); // b2b3
617 extractReImVectors( inputASM, outputASM, ASM_COMP_B2B3 ); // b2b3
618 extractReImVectors( inputASM, outputASM, ASM_COMP_B2E1 ); // b2e1
618 extractReImVectors( inputASM, outputASM, ASM_COMP_B2E1 ); // b2e1
619 extractReImVectors( inputASM, outputASM, ASM_COMP_B2E2 ); // b2e2
619 extractReImVectors( inputASM, outputASM, ASM_COMP_B2E2 ); // b2e2
620 extractReImVectors( inputASM, outputASM, ASM_COMP_B3E1 ); // b3e1
620 extractReImVectors( inputASM, outputASM, ASM_COMP_B3E1 ); // b3e1
621 extractReImVectors( inputASM, outputASM, ASM_COMP_B3E2 ); // b3e2
621 extractReImVectors( inputASM, outputASM, ASM_COMP_B3E2 ); // b3e2
622 extractReImVectors( inputASM, outputASM, ASM_COMP_E1E2 ); // e1e2
622 extractReImVectors( inputASM, outputASM, ASM_COMP_E1E2 ); // e1e2
623
623
624 copyReVectors(inputASM, outputASM, ASM_COMP_B1B1 ); // b1b1
624 copyReVectors(inputASM, outputASM, ASM_COMP_B1B1 ); // b1b1
625 copyReVectors(inputASM, outputASM, ASM_COMP_B2B2 ); // b2b2
625 copyReVectors(inputASM, outputASM, ASM_COMP_B2B2 ); // b2b2
626 copyReVectors(inputASM, outputASM, ASM_COMP_B3B3); // b3b3
626 copyReVectors(inputASM, outputASM, ASM_COMP_B3B3); // b3b3
627 copyReVectors(inputASM, outputASM, ASM_COMP_E1E1); // e1e1
627 copyReVectors(inputASM, outputASM, ASM_COMP_E1E1); // e1e1
628 copyReVectors(inputASM, outputASM, ASM_COMP_E2E2); // e2e2
628 copyReVectors(inputASM, outputASM, ASM_COMP_E2E2); // e2e2
629 }
629 }
630
630
631 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
631 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
632 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage,
632 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage,
633 unsigned char ASMIndexStart,
633 unsigned char ASMIndexStart,
634 unsigned char channel )
634 unsigned char channel )
635 {
635 {
636 //*************
636 //*************
637 // input format
637 // input format
638 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
638 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
639 //**************
639 //**************
640 // output format
640 // output format
641 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
641 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
642 //************
642 //************
643 // compression
643 // compression
644 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
644 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
645 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
645 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
646
646
647 int frequencyBin;
647 int frequencyBin;
648 int asmComponent;
648 int asmComponent;
649 int offsetASM;
649 int offsetASM;
650 int offsetCompressed;
650 int offsetCompressed;
651 int offsetFBin;
651 int offsetFBin;
652 int fBinMask;
652 int fBinMask;
653 int k;
653 int k;
654
654
655 // BUILD DATA
655 // BUILD DATA
656 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
656 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
657 {
657 {
658 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
658 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
659 {
659 {
660 offsetCompressed = // NO TIME OFFSET
660 offsetCompressed = // NO TIME OFFSET
661 (frequencyBin * NB_VALUES_PER_SM)
661 (frequencyBin * NB_VALUES_PER_SM)
662 + asmComponent;
662 + asmComponent;
663 offsetASM = // NO TIME OFFSET
663 offsetASM = // NO TIME OFFSET
664 (asmComponent * NB_BINS_PER_SM)
664 (asmComponent * NB_BINS_PER_SM)
665 + ASMIndexStart
665 + ASMIndexStart
666 + (frequencyBin * nbBinsToAverage);
666 + (frequencyBin * nbBinsToAverage);
667 offsetFBin = ASMIndexStart
667 offsetFBin = ASMIndexStart
668 + (frequencyBin * nbBinsToAverage);
668 + (frequencyBin * nbBinsToAverage);
669 compressed_spec_mat[ offsetCompressed ] = 0;
669 compressed_spec_mat[ offsetCompressed ] = 0;
670 for ( k = 0; k < nbBinsToAverage; k++ )
670 for ( k = 0; k < nbBinsToAverage; k++ )
671 {
671 {
672 fBinMask = getFBinMask( offsetFBin + k, channel );
672 fBinMask = getFBinMask( offsetFBin + k, channel );
673 compressed_spec_mat[offsetCompressed ] = compressed_spec_mat[ offsetCompressed ]
673 compressed_spec_mat[offsetCompressed ] = compressed_spec_mat[ offsetCompressed ]
674 + (averaged_spec_mat[ offsetASM + k ] * fBinMask);
674 + (averaged_spec_mat[ offsetASM + k ] * fBinMask);
675 }
675 }
676 if (divider != 0)
676 if (divider != 0)
677 {
677 {
678 compressed_spec_mat[ offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
678 compressed_spec_mat[ offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
679 }
679 }
680 else
680 else
681 {
681 {
682 compressed_spec_mat[ offsetCompressed ] = INIT_FLOAT;
682 compressed_spec_mat[ offsetCompressed ] = INIT_FLOAT;
683 }
683 }
684 }
684 }
685 }
685 }
686
686
687 }
687 }
688
688
689 int getFBinMask( int index, unsigned char channel )
689 int getFBinMask( int index, unsigned char channel )
690 {
690 {
691 unsigned int indexInChar;
691 unsigned int indexInChar;
692 unsigned int indexInTheChar;
692 unsigned int indexInTheChar;
693 int fbin;
693 int fbin;
694 unsigned char *sy_lfr_fbins_fx_word1;
694 unsigned char *sy_lfr_fbins_fx_word1;
695
695
696 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
696 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
697
697
698 switch(channel)
698 switch(channel)
699 {
699 {
700 case CHANNELF0:
700 case CHANNELF0:
701 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0;
701 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0;
702 break;
702 break;
703 case CHANNELF1:
703 case CHANNELF1:
704 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1;
704 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1;
705 break;
705 break;
706 case CHANNELF2:
706 case CHANNELF2:
707 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2;
707 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2;
708 break;
708 break;
709 default:
709 default:
710 PRINTF("ERR *** in getFBinMask, wrong frequency channel")
710 PRINTF("ERR *** in getFBinMask, wrong frequency channel")
711 }
711 }
712
712
713 indexInChar = index >> SHIFT_3_BITS;
713 indexInChar = index >> SHIFT_3_BITS;
714 indexInTheChar = index - (indexInChar * BITS_PER_BYTE);
714 indexInTheChar = index - (indexInChar * BITS_PER_BYTE);
715
715
716 fbin = (int) ((sy_lfr_fbins_fx_word1[ BYTES_PER_MASK - 1 - indexInChar] >> indexInTheChar) & 1);
716 fbin = (int) ((sy_lfr_fbins_fx_word1[ BYTES_PER_MASK - 1 - indexInChar] >> indexInTheChar) & 1);
717
717
718 return fbin;
718 return fbin;
719 }
719 }
720
720
721 unsigned char isPolluted( u_int64_t t0, u_int64_t t1, u_int64_t tbad0, u_int64_t tbad1 )
721 unsigned char isPolluted( u_int64_t t0, u_int64_t t1, u_int64_t tbad0, u_int64_t tbad1 )
722 {
722 {
723 unsigned char polluted;
723 unsigned char polluted;
724
724
725 polluted = MATRIX_IS_NOT_POLLUTED;
725 polluted = MATRIX_IS_NOT_POLLUTED;
726
726
727 if ( ((tbad0 < t0) && (t0 < tbad1)) // t0 is inside the polluted range
727 if ( ((tbad0 < t0) && (t0 < tbad1)) // t0 is inside the polluted range
728 || ((tbad0 < t1) && (t1 < tbad1)) // t1 is inside the polluted range
728 || ((tbad0 < t1) && (t1 < tbad1)) // t1 is inside the polluted range
729 || ((t0 < tbad0) && (tbad1 < t1)) // the polluted range is inside the signal range
729 || ((t0 < tbad0) && (tbad1 < t1)) // the polluted range is inside the signal range
730 || ((tbad0 < t0) && (t1 < tbad1))) // the signal range is inside the polluted range
730 || ((tbad0 < t0) && (t1 < tbad1))) // the signal range is inside the polluted range
731 {
731 {
732 polluted = MATRIX_IS_POLLUTED;
732 polluted = MATRIX_IS_POLLUTED;
733 }
733 }
734
734
735 return polluted;
735 return polluted;
736 }
736 }
737
737
738 unsigned char acquisitionTimeIsValid( unsigned int coarseTime, unsigned int fineTime, unsigned char channel)
738 unsigned char acquisitionTimeIsValid( unsigned int coarseTime, unsigned int fineTime, unsigned char channel)
739 {
739 {
740 u_int64_t t0;
740 u_int64_t t0;
741 u_int64_t t1;
741 u_int64_t t1;
742 u_int64_t tc;
742 u_int64_t tc;
743 u_int64_t tbad0;
743 u_int64_t tbad0;
744 u_int64_t tbad1;
744 u_int64_t tbad1;
745
745
746 u_int64_t modulusInFineTime;
746 u_int64_t modulusInFineTime;
747 u_int64_t offsetInFineTime;
747 u_int64_t offsetInFineTime;
748 u_int64_t shiftInFineTime;
748 u_int64_t shiftInFineTime;
749 u_int64_t tbadInFineTime;
749 u_int64_t tbadInFineTime;
750
750
751 u_int64_t timecodeReference;
751 u_int64_t timecodeReference;
752
752
753 unsigned char pasFilteringIsEnabled;
753 unsigned char pasFilteringIsEnabled;
754 unsigned char ret;
754 unsigned char ret;
755
755
756 pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 1); // [0000 0001]
757 ret = MATRIX_IS_NOT_POLLUTED;
758
759 // compute the acquitionTime range
760 modulusInFineTime = ((u_int64_t) filterPar.sy_lfr_pas_filter_modulus) * CONST_65536;
761 offsetInFineTime = ((u_int64_t) filterPar.sy_lfr_pas_filter_offset) * CONST_65536;
762 shiftInFineTime = ((u_int64_t) filterPar.sy_lfr_pas_filter_shift) * CONST_65536;
763 tbadInFineTime = ((u_int64_t) filterPar.sy_lfr_pas_filter_tbad) * CONST_65536;
764
765 // compute acquisition time from caoarseTime and fineTime
756 // compute acquisition time from caoarseTime and fineTime
766 t0 = ( ((u_int64_t)coarseTime) << SHIFT_2_BYTES ) + (u_int64_t) fineTime;
757 t0 = ( ((u_int64_t)coarseTime) << SHIFT_2_BYTES ) + (u_int64_t) fineTime;
758 t1 = t0;
759 tc = t0;
760 tbad0 = t0;
761 tbad1 = t0;
762
767 switch(channel)
763 switch(channel)
768 {
764 {
769 case CHANNELF0:
765 case CHANNELF0:
770 t1 = t0 + ACQUISITION_DURATION_F0;
766 t1 = t0 + ACQUISITION_DURATION_F0;
771 tc = t0 + HALF_ACQUISITION_DURATION_F0;
767 tc = t0 + HALF_ACQUISITION_DURATION_F0;
772 break;
768 break;
773 case CHANNELF1:
769 case CHANNELF1:
774 t1 = t0 + ACQUISITION_DURATION_F1;
770 t1 = t0 + ACQUISITION_DURATION_F1;
775 tc = t0 + HALF_ACQUISITION_DURATION_F1;
771 tc = t0 + HALF_ACQUISITION_DURATION_F1;
776 break;
772 break;
777 case CHANNELF2:
773 case CHANNELF2:
778 t1 = t0 + ACQUISITION_DURATION_F2;
774 t1 = t0 + ACQUISITION_DURATION_F2;
779 tc = t0 + HALF_ACQUISITION_DURATION_F2;
775 tc = t0 + HALF_ACQUISITION_DURATION_F2;
780 break;
776 break;
777 default:
778 break;
781 }
779 }
782
780
783 // INTERSECTION TEST #1
781 // compute the acquitionTime range
784 timecodeReference = (tc - (tc % modulusInFineTime)) - modulusInFineTime ;
782 modulusInFineTime = filterPar.modulus_in_finetime;
785 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
783 offsetInFineTime = filterPar.offset_in_finetime;
786 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
784 shiftInFineTime = filterPar.shift_in_finetime;
787 ret = isPolluted( t0, t1, tbad0, tbad1 );
785 tbadInFineTime = filterPar.tbad_in_finetime;
786 timecodeReference = INIT_INT;
788
787
789 // INTERSECTION TEST #2
788 pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 1); // [0000 0001]
790 timecodeReference = (tc - (tc % modulusInFineTime)) ;
789 ret = MATRIX_IS_NOT_POLLUTED;
791 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
792 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
793 if (ret == MATRIX_IS_NOT_POLLUTED)
794 {
795 ret = isPolluted( t0, t1, tbad0, tbad1 );
796 }
797
790
798 // INTERSECTION TEST #3
791 if ( (tbadInFineTime == 0) || (pasFilteringIsEnabled == 0) )
799 timecodeReference = (tc - (tc % modulusInFineTime)) + modulusInFineTime ;
800 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
801 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
802 if (ret == MATRIX_IS_NOT_POLLUTED)
803 {
804 ret = isPolluted( t0, t1, tbad0, tbad1 );
805 }
806
807 if (pasFilteringIsEnabled == 0)
808 {
792 {
809 ret = MATRIX_IS_NOT_POLLUTED;
793 ret = MATRIX_IS_NOT_POLLUTED;
810 }
794 }
795 else
796 {
797 // INTERSECTION TEST #1
798 timecodeReference = (tc - (tc % modulusInFineTime)) - modulusInFineTime ;
799 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
800 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
801 ret = isPolluted( t0, t1, tbad0, tbad1 );
802
803 // INTERSECTION TEST #2
804 if (ret == MATRIX_IS_NOT_POLLUTED)
805 {
806 timecodeReference = (tc - (tc % modulusInFineTime)) ;
807 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
808 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
809 ret = isPolluted( t0, t1, tbad0, tbad1 );
810 }
811
812 // INTERSECTION TEST #3
813 if (ret == MATRIX_IS_NOT_POLLUTED)
814 {
815 timecodeReference = (tc - (tc % modulusInFineTime)) + modulusInFineTime ;
816 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
817 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
818 ret = isPolluted( t0, t1, tbad0, tbad1 );
819 }
820 }
811
821
812 return ret;
822 return ret;
813 }
823 }
814
824
815 void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
825 void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
816 {
826 {
817 unsigned char bin;
827 unsigned char bin;
818 unsigned char kcoeff;
828 unsigned char kcoeff;
819
829
820 for (bin=0; bin<nb_bins_norm; bin++)
830 for (bin=0; bin<nb_bins_norm; bin++)
821 {
831 {
822 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
832 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
823 {
833 {
824 output_kcoeff[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff ) * SBM_COEFF_PER_NORM_COEFF ]
834 output_kcoeff[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff ) * SBM_COEFF_PER_NORM_COEFF ]
825 = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
835 = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
826 output_kcoeff[ ( ( (bin * NB_K_COEFF_PER_BIN ) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ) + 1 ]
836 output_kcoeff[ ( ( (bin * NB_K_COEFF_PER_BIN ) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ) + 1 ]
827 = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
837 = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
828 }
838 }
829 }
839 }
830 }
840 }
@@ -1,2061 +1,2068
1 /** Functions to load and dump parameters in the LFR registers.
1 /** Functions to load and dump parameters in the LFR registers.
2 *
2 *
3 * @file
3 * @file
4 * @author P. LEROY
4 * @author P. LEROY
5 *
5 *
6 * A group of functions to handle TC related to parameter loading and dumping.\n
6 * A group of functions to handle TC related to parameter loading and dumping.\n
7 * TC_LFR_LOAD_COMMON_PAR\n
7 * TC_LFR_LOAD_COMMON_PAR\n
8 * TC_LFR_LOAD_NORMAL_PAR\n
8 * TC_LFR_LOAD_NORMAL_PAR\n
9 * TC_LFR_LOAD_BURST_PAR\n
9 * TC_LFR_LOAD_BURST_PAR\n
10 * TC_LFR_LOAD_SBM1_PAR\n
10 * TC_LFR_LOAD_SBM1_PAR\n
11 * TC_LFR_LOAD_SBM2_PAR\n
11 * TC_LFR_LOAD_SBM2_PAR\n
12 *
12 *
13 */
13 */
14
14
15 #include "tc_load_dump_parameters.h"
15 #include "tc_load_dump_parameters.h"
16
16
17 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_1 = {0};
17 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_1 = {0};
18 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2 = {0};
18 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2 = {0};
19 ring_node kcoefficient_node_1 = {0};
19 ring_node kcoefficient_node_1 = {0};
20 ring_node kcoefficient_node_2 = {0};
20 ring_node kcoefficient_node_2 = {0};
21
21
22 int action_load_common_par(ccsdsTelecommandPacket_t *TC)
22 int action_load_common_par(ccsdsTelecommandPacket_t *TC)
23 {
23 {
24 /** This function updates the LFR registers with the incoming common parameters.
24 /** This function updates the LFR registers with the incoming common parameters.
25 *
25 *
26 * @param TC points to the TeleCommand packet that is being processed
26 * @param TC points to the TeleCommand packet that is being processed
27 *
27 *
28 *
28 *
29 */
29 */
30
30
31 parameter_dump_packet.sy_lfr_common_parameters_spare = TC->dataAndCRC[0];
31 parameter_dump_packet.sy_lfr_common_parameters_spare = TC->dataAndCRC[0];
32 parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
32 parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
33 set_wfp_data_shaping( );
33 set_wfp_data_shaping( );
34 return LFR_SUCCESSFUL;
34 return LFR_SUCCESSFUL;
35 }
35 }
36
36
37 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
37 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
38 {
38 {
39 /** This function updates the LFR registers with the incoming normal parameters.
39 /** This function updates the LFR registers with the incoming normal parameters.
40 *
40 *
41 * @param TC points to the TeleCommand packet that is being processed
41 * @param TC points to the TeleCommand packet that is being processed
42 * @param queue_id is the id of the queue which handles TM related to this execution step
42 * @param queue_id is the id of the queue which handles TM related to this execution step
43 *
43 *
44 */
44 */
45
45
46 int result;
46 int result;
47 int flag;
47 int flag;
48 rtems_status_code status;
48 rtems_status_code status;
49
49
50 flag = LFR_SUCCESSFUL;
50 flag = LFR_SUCCESSFUL;
51
51
52 if ( (lfrCurrentMode == LFR_MODE_NORMAL) ||
52 if ( (lfrCurrentMode == LFR_MODE_NORMAL) ||
53 (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) {
53 (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) {
54 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
54 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
55 flag = LFR_DEFAULT;
55 flag = LFR_DEFAULT;
56 }
56 }
57
57
58 // CHECK THE PARAMETERS SET CONSISTENCY
58 // CHECK THE PARAMETERS SET CONSISTENCY
59 if (flag == LFR_SUCCESSFUL)
59 if (flag == LFR_SUCCESSFUL)
60 {
60 {
61 flag = check_normal_par_consistency( TC, queue_id );
61 flag = check_normal_par_consistency( TC, queue_id );
62 }
62 }
63
63
64 // SET THE PARAMETERS IF THEY ARE CONSISTENT
64 // SET THE PARAMETERS IF THEY ARE CONSISTENT
65 if (flag == LFR_SUCCESSFUL)
65 if (flag == LFR_SUCCESSFUL)
66 {
66 {
67 result = set_sy_lfr_n_swf_l( TC );
67 result = set_sy_lfr_n_swf_l( TC );
68 result = set_sy_lfr_n_swf_p( TC );
68 result = set_sy_lfr_n_swf_p( TC );
69 result = set_sy_lfr_n_bp_p0( TC );
69 result = set_sy_lfr_n_bp_p0( TC );
70 result = set_sy_lfr_n_bp_p1( TC );
70 result = set_sy_lfr_n_bp_p1( TC );
71 result = set_sy_lfr_n_asm_p( TC );
71 result = set_sy_lfr_n_asm_p( TC );
72 result = set_sy_lfr_n_cwf_long_f3( TC );
72 result = set_sy_lfr_n_cwf_long_f3( TC );
73 }
73 }
74
74
75 return flag;
75 return flag;
76 }
76 }
77
77
78 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
78 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
79 {
79 {
80 /** This function updates the LFR registers with the incoming burst parameters.
80 /** This function updates the LFR registers with the incoming burst parameters.
81 *
81 *
82 * @param TC points to the TeleCommand packet that is being processed
82 * @param TC points to the TeleCommand packet that is being processed
83 * @param queue_id is the id of the queue which handles TM related to this execution step
83 * @param queue_id is the id of the queue which handles TM related to this execution step
84 *
84 *
85 */
85 */
86
86
87 int flag;
87 int flag;
88 rtems_status_code status;
88 rtems_status_code status;
89 unsigned char sy_lfr_b_bp_p0;
89 unsigned char sy_lfr_b_bp_p0;
90 unsigned char sy_lfr_b_bp_p1;
90 unsigned char sy_lfr_b_bp_p1;
91 float aux;
91 float aux;
92
92
93 flag = LFR_SUCCESSFUL;
93 flag = LFR_SUCCESSFUL;
94
94
95 if ( lfrCurrentMode == LFR_MODE_BURST ) {
95 if ( lfrCurrentMode == LFR_MODE_BURST ) {
96 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
96 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
97 flag = LFR_DEFAULT;
97 flag = LFR_DEFAULT;
98 }
98 }
99
99
100 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
100 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
101 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
101 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
102
102
103 // sy_lfr_b_bp_p0 shall not be lower than its default value
103 // sy_lfr_b_bp_p0 shall not be lower than its default value
104 if (flag == LFR_SUCCESSFUL)
104 if (flag == LFR_SUCCESSFUL)
105 {
105 {
106 if (sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0 )
106 if (sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0 )
107 {
107 {
108 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0 + DATAFIELD_OFFSET, sy_lfr_b_bp_p0 );
108 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 flag = WRONG_APP_DATA;
109 flag = WRONG_APP_DATA;
110 }
110 }
111 }
111 }
112 // sy_lfr_b_bp_p1 shall not be lower than its default value
112 // sy_lfr_b_bp_p1 shall not be lower than its default value
113 if (flag == LFR_SUCCESSFUL)
113 if (flag == LFR_SUCCESSFUL)
114 {
114 {
115 if (sy_lfr_b_bp_p1 < DEFAULT_SY_LFR_B_BP_P1 )
115 if (sy_lfr_b_bp_p1 < DEFAULT_SY_LFR_B_BP_P1 )
116 {
116 {
117 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P1 + DATAFIELD_OFFSET, sy_lfr_b_bp_p1 );
117 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 flag = WRONG_APP_DATA;
118 flag = WRONG_APP_DATA;
119 }
119 }
120 }
120 }
121 //****************************************************************
121 //****************************************************************
122 // check the consistency between sy_lfr_b_bp_p0 and sy_lfr_b_bp_p1
122 // check the consistency between sy_lfr_b_bp_p0 and sy_lfr_b_bp_p1
123 if (flag == LFR_SUCCESSFUL)
123 if (flag == LFR_SUCCESSFUL)
124 {
124 {
125 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
125 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
126 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
126 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
127 aux = ( (float ) sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0 ) - floor(sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0);
127 aux = ( (float ) sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0 ) - floor(sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0);
128 if (aux > FLOAT_EQUAL_ZERO)
128 if (aux > FLOAT_EQUAL_ZERO)
129 {
129 {
130 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0 + DATAFIELD_OFFSET, sy_lfr_b_bp_p0 );
130 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 flag = LFR_DEFAULT;
131 flag = LFR_DEFAULT;
132 }
132 }
133 }
133 }
134
134
135 // SET THE PARAMETERS
135 // SET THE PARAMETERS
136 if (flag == LFR_SUCCESSFUL)
136 if (flag == LFR_SUCCESSFUL)
137 {
137 {
138 flag = set_sy_lfr_b_bp_p0( TC );
138 flag = set_sy_lfr_b_bp_p0( TC );
139 flag = set_sy_lfr_b_bp_p1( TC );
139 flag = set_sy_lfr_b_bp_p1( TC );
140 }
140 }
141
141
142 return flag;
142 return flag;
143 }
143 }
144
144
145 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
145 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
146 {
146 {
147 /** This function updates the LFR registers with the incoming sbm1 parameters.
147 /** This function updates the LFR registers with the incoming sbm1 parameters.
148 *
148 *
149 * @param TC points to the TeleCommand packet that is being processed
149 * @param TC points to the TeleCommand packet that is being processed
150 * @param queue_id is the id of the queue which handles TM related to this execution step
150 * @param queue_id is the id of the queue which handles TM related to this execution step
151 *
151 *
152 */
152 */
153
153
154 int flag;
154 int flag;
155 rtems_status_code status;
155 rtems_status_code status;
156 unsigned char sy_lfr_s1_bp_p0;
156 unsigned char sy_lfr_s1_bp_p0;
157 unsigned char sy_lfr_s1_bp_p1;
157 unsigned char sy_lfr_s1_bp_p1;
158 float aux;
158 float aux;
159
159
160 flag = LFR_SUCCESSFUL;
160 flag = LFR_SUCCESSFUL;
161
161
162 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
162 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
163 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
163 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
164 flag = LFR_DEFAULT;
164 flag = LFR_DEFAULT;
165 }
165 }
166
166
167 sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
167 sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
168 sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
168 sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
169
169
170 // sy_lfr_s1_bp_p0
170 // sy_lfr_s1_bp_p0
171 if (flag == LFR_SUCCESSFUL)
171 if (flag == LFR_SUCCESSFUL)
172 {
172 {
173 if (sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0 )
173 if (sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0 )
174 {
174 {
175 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p0 );
175 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 flag = WRONG_APP_DATA;
176 flag = WRONG_APP_DATA;
177 }
177 }
178 }
178 }
179 // sy_lfr_s1_bp_p1
179 // sy_lfr_s1_bp_p1
180 if (flag == LFR_SUCCESSFUL)
180 if (flag == LFR_SUCCESSFUL)
181 {
181 {
182 if (sy_lfr_s1_bp_p1 < DEFAULT_SY_LFR_S1_BP_P1 )
182 if (sy_lfr_s1_bp_p1 < DEFAULT_SY_LFR_S1_BP_P1 )
183 {
183 {
184 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P1 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p1 );
184 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 flag = WRONG_APP_DATA;
185 flag = WRONG_APP_DATA;
186 }
186 }
187 }
187 }
188 //******************************************************************
188 //******************************************************************
189 // check the consistency between sy_lfr_s1_bp_p0 and sy_lfr_s1_bp_p1
189 // check the consistency between sy_lfr_s1_bp_p0 and sy_lfr_s1_bp_p1
190 if (flag == LFR_SUCCESSFUL)
190 if (flag == LFR_SUCCESSFUL)
191 {
191 {
192 aux = ( (float ) sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE) )
192 aux = ( (float ) sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE) )
193 - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE));
193 - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE));
194 if (aux > FLOAT_EQUAL_ZERO)
194 if (aux > FLOAT_EQUAL_ZERO)
195 {
195 {
196 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p0 );
196 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 flag = LFR_DEFAULT;
197 flag = LFR_DEFAULT;
198 }
198 }
199 }
199 }
200
200
201 // SET THE PARAMETERS
201 // SET THE PARAMETERS
202 if (flag == LFR_SUCCESSFUL)
202 if (flag == LFR_SUCCESSFUL)
203 {
203 {
204 flag = set_sy_lfr_s1_bp_p0( TC );
204 flag = set_sy_lfr_s1_bp_p0( TC );
205 flag = set_sy_lfr_s1_bp_p1( TC );
205 flag = set_sy_lfr_s1_bp_p1( TC );
206 }
206 }
207
207
208 return flag;
208 return flag;
209 }
209 }
210
210
211 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
211 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
212 {
212 {
213 /** This function updates the LFR registers with the incoming sbm2 parameters.
213 /** This function updates the LFR registers with the incoming sbm2 parameters.
214 *
214 *
215 * @param TC points to the TeleCommand packet that is being processed
215 * @param TC points to the TeleCommand packet that is being processed
216 * @param queue_id is the id of the queue which handles TM related to this execution step
216 * @param queue_id is the id of the queue which handles TM related to this execution step
217 *
217 *
218 */
218 */
219
219
220 int flag;
220 int flag;
221 rtems_status_code status;
221 rtems_status_code status;
222 unsigned char sy_lfr_s2_bp_p0;
222 unsigned char sy_lfr_s2_bp_p0;
223 unsigned char sy_lfr_s2_bp_p1;
223 unsigned char sy_lfr_s2_bp_p1;
224 float aux;
224 float aux;
225
225
226 flag = LFR_SUCCESSFUL;
226 flag = LFR_SUCCESSFUL;
227
227
228 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
228 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
229 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
229 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
230 flag = LFR_DEFAULT;
230 flag = LFR_DEFAULT;
231 }
231 }
232
232
233 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
233 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
234 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
234 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
235
235
236 // sy_lfr_s2_bp_p0
236 // sy_lfr_s2_bp_p0
237 if (flag == LFR_SUCCESSFUL)
237 if (flag == LFR_SUCCESSFUL)
238 {
238 {
239 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
239 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
240 {
240 {
241 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p0 );
241 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 flag = WRONG_APP_DATA;
242 flag = WRONG_APP_DATA;
243 }
243 }
244 }
244 }
245 // sy_lfr_s2_bp_p1
245 // sy_lfr_s2_bp_p1
246 if (flag == LFR_SUCCESSFUL)
246 if (flag == LFR_SUCCESSFUL)
247 {
247 {
248 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
248 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
249 {
249 {
250 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P1 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p1 );
250 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 flag = WRONG_APP_DATA;
251 flag = WRONG_APP_DATA;
252 }
252 }
253 }
253 }
254 //******************************************************************
254 //******************************************************************
255 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
255 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
256 if (flag == LFR_SUCCESSFUL)
256 if (flag == LFR_SUCCESSFUL)
257 {
257 {
258 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
258 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
259 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
259 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
260 aux = ( (float ) sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0 ) - floor(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
260 aux = ( (float ) sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0 ) - floor(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
261 if (aux > FLOAT_EQUAL_ZERO)
261 if (aux > FLOAT_EQUAL_ZERO)
262 {
262 {
263 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p0 );
263 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 flag = LFR_DEFAULT;
264 flag = LFR_DEFAULT;
265 }
265 }
266 }
266 }
267
267
268 // SET THE PARAMETERS
268 // SET THE PARAMETERS
269 if (flag == LFR_SUCCESSFUL)
269 if (flag == LFR_SUCCESSFUL)
270 {
270 {
271 flag = set_sy_lfr_s2_bp_p0( TC );
271 flag = set_sy_lfr_s2_bp_p0( TC );
272 flag = set_sy_lfr_s2_bp_p1( TC );
272 flag = set_sy_lfr_s2_bp_p1( TC );
273 }
273 }
274
274
275 return flag;
275 return flag;
276 }
276 }
277
277
278 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
278 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
279 {
279 {
280 /** This function updates the LFR registers with the incoming sbm2 parameters.
280 /** This function updates the LFR registers with the incoming sbm2 parameters.
281 *
281 *
282 * @param TC points to the TeleCommand packet that is being processed
282 * @param TC points to the TeleCommand packet that is being processed
283 * @param queue_id is the id of the queue which handles TM related to this execution step
283 * @param queue_id is the id of the queue which handles TM related to this execution step
284 *
284 *
285 */
285 */
286
286
287 int flag;
287 int flag;
288
288
289 flag = LFR_DEFAULT;
289 flag = LFR_DEFAULT;
290
290
291 flag = set_sy_lfr_kcoeff( TC, queue_id );
291 flag = set_sy_lfr_kcoeff( TC, queue_id );
292
292
293 return flag;
293 return flag;
294 }
294 }
295
295
296 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
296 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
297 {
297 {
298 /** This function updates the LFR registers with the incoming sbm2 parameters.
298 /** This function updates the LFR registers with the incoming sbm2 parameters.
299 *
299 *
300 * @param TC points to the TeleCommand packet that is being processed
300 * @param TC points to the TeleCommand packet that is being processed
301 * @param queue_id is the id of the queue which handles TM related to this execution step
301 * @param queue_id is the id of the queue which handles TM related to this execution step
302 *
302 *
303 */
303 */
304
304
305 int flag;
305 int flag;
306
306
307 flag = LFR_DEFAULT;
307 flag = LFR_DEFAULT;
308
308
309 flag = set_sy_lfr_fbins( TC );
309 flag = set_sy_lfr_fbins( TC );
310
310
311 // once the fbins masks have been stored, they have to be merged with the masks which handle the reaction wheels frequencies filtering
311 // once the fbins masks have been stored, they have to be merged with the masks which handle the reaction wheels frequencies filtering
312 merge_fbins_masks();
312 merge_fbins_masks();
313
313
314 return flag;
314 return flag;
315 }
315 }
316
316
317 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
317 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
318 {
318 {
319 /** This function updates the LFR registers with the incoming sbm2 parameters.
319 /** This function updates the LFR registers with the incoming sbm2 parameters.
320 *
320 *
321 * @param TC points to the TeleCommand packet that is being processed
321 * @param TC points to the TeleCommand packet that is being processed
322 * @param queue_id is the id of the queue which handles TM related to this execution step
322 * @param queue_id is the id of the queue which handles TM related to this execution step
323 *
323 *
324 */
324 */
325
325
326 int flag;
326 int flag;
327 unsigned char k;
327 unsigned char k;
328
328
329 flag = LFR_DEFAULT;
329 flag = LFR_DEFAULT;
330 k = INIT_CHAR;
330 k = INIT_CHAR;
331
331
332 flag = check_sy_lfr_filter_parameters( TC, queue_id );
332 flag = check_sy_lfr_filter_parameters( TC, queue_id );
333
333
334 if (flag == LFR_SUCCESSFUL)
334 if (flag == LFR_SUCCESSFUL)
335 {
335 {
336 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ];
336 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ];
337 parameter_dump_packet.sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
337 parameter_dump_packet.sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
338 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_0 ];
338 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_0 ];
339 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_1 ];
339 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_1 ];
340 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_2 ];
340 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_2 ];
341 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_3 ];
341 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_3 ];
342 parameter_dump_packet.sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
342 parameter_dump_packet.sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
343 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_0 ];
343 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_0 ];
344 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_1 ];
344 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_1 ];
345 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_2 ];
345 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_2 ];
346 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_3 ];
346 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_3 ];
347 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_0 ];
347 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_0 ];
348 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_1 ];
348 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_1 ];
349 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_2 ];
349 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_2 ];
350 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_3 ];
350 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_3 ];
351
351
352 //****************************
352 //****************************
353 // store PAS filter parameters
353 // store PAS filter parameters
354
354 // sy_lfr_pas_filter_enabled
355 // sy_lfr_pas_filter_enabled
355 filterPar.spare_sy_lfr_pas_filter_enabled = parameter_dump_packet.spare_sy_lfr_pas_filter_enabled;
356 filterPar.spare_sy_lfr_pas_filter_enabled = parameter_dump_packet.spare_sy_lfr_pas_filter_enabled;
356 set_sy_lfr_pas_filter_enabled( parameter_dump_packet.spare_sy_lfr_pas_filter_enabled & BIT_PAS_FILTER_ENABLED );
357 set_sy_lfr_pas_filter_enabled( parameter_dump_packet.spare_sy_lfr_pas_filter_enabled & BIT_PAS_FILTER_ENABLED );
358
357 // sy_lfr_pas_filter_modulus
359 // sy_lfr_pas_filter_modulus
358 filterPar.sy_lfr_pas_filter_modulus = parameter_dump_packet.sy_lfr_pas_filter_modulus;
360 filterPar.modulus_in_finetime = ((uint64_t) parameter_dump_packet.sy_lfr_pas_filter_modulus) * CONST_65536;
361
359 // sy_lfr_pas_filter_tbad
362 // sy_lfr_pas_filter_tbad
360 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_tbad,
363 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_tbad,
361 parameter_dump_packet.sy_lfr_pas_filter_tbad );
364 parameter_dump_packet.sy_lfr_pas_filter_tbad );
365 filterPar.tbad_in_finetime = (uint64_t) (filterPar.sy_lfr_pas_filter_tbad * CONST_65536);
366
362 // sy_lfr_pas_filter_offset
367 // sy_lfr_pas_filter_offset
363 filterPar.sy_lfr_pas_filter_offset = parameter_dump_packet.sy_lfr_pas_filter_offset;
368 filterPar.offset_in_finetime = ((uint64_t) parameter_dump_packet.sy_lfr_pas_filter_offset) * CONST_65536;
369
364 // sy_lfr_pas_filter_shift
370 // sy_lfr_pas_filter_shift
365 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_shift,
371 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_shift,
366 parameter_dump_packet.sy_lfr_pas_filter_shift );
372 parameter_dump_packet.sy_lfr_pas_filter_shift );
373 filterPar.shift_in_finetime = (uint64_t) (filterPar.sy_lfr_pas_filter_shift * CONST_65536);
367
374
368 //****************************************************
375 //****************************************************
369 // store the parameter sy_lfr_sc_rw_delta_f as a float
376 // store the parameter sy_lfr_sc_rw_delta_f as a float
370 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f,
377 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f,
371 parameter_dump_packet.sy_lfr_sc_rw_delta_f );
378 parameter_dump_packet.sy_lfr_sc_rw_delta_f );
372
379
373 // copy rw.._k.. from the incoming TC to the local parameter_dump_packet
380 // copy rw.._k.. from the incoming TC to the local parameter_dump_packet
374 for (k = 0; k < NB_RW_K_COEFFS * NB_BYTES_PER_RW_K_COEFF; k++)
381 for (k = 0; k < NB_RW_K_COEFFS * NB_BYTES_PER_RW_K_COEFF; k++)
375 {
382 {
376 parameter_dump_packet.sy_lfr_rw1_k1[k] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_RW1_K1 + k ];
383 parameter_dump_packet.sy_lfr_rw1_k1[k] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_RW1_K1 + k ];
377 }
384 }
378
385
379 //***********************************************
386 //***********************************************
380 // store the parameter sy_lfr_rw.._k.. as a float
387 // store the parameter sy_lfr_rw.._k.. as a float
381 // rw1_k
388 // rw1_k
382 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k1, parameter_dump_packet.sy_lfr_rw1_k1 );
389 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k1, parameter_dump_packet.sy_lfr_rw1_k1 );
383 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k2, parameter_dump_packet.sy_lfr_rw1_k2 );
390 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k2, parameter_dump_packet.sy_lfr_rw1_k2 );
384 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k3, parameter_dump_packet.sy_lfr_rw1_k3 );
391 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k3, parameter_dump_packet.sy_lfr_rw1_k3 );
385 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k4, parameter_dump_packet.sy_lfr_rw1_k4 );
392 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k4, parameter_dump_packet.sy_lfr_rw1_k4 );
386 // rw2_k
393 // rw2_k
387 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k1, parameter_dump_packet.sy_lfr_rw2_k1 );
394 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k1, parameter_dump_packet.sy_lfr_rw2_k1 );
388 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k2, parameter_dump_packet.sy_lfr_rw2_k2 );
395 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k2, parameter_dump_packet.sy_lfr_rw2_k2 );
389 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k3, parameter_dump_packet.sy_lfr_rw2_k3 );
396 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k3, parameter_dump_packet.sy_lfr_rw2_k3 );
390 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k4, parameter_dump_packet.sy_lfr_rw2_k4 );
397 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k4, parameter_dump_packet.sy_lfr_rw2_k4 );
391 // rw3_k
398 // rw3_k
392 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k1, parameter_dump_packet.sy_lfr_rw3_k1 );
399 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k1, parameter_dump_packet.sy_lfr_rw3_k1 );
393 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k2, parameter_dump_packet.sy_lfr_rw3_k2 );
400 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k2, parameter_dump_packet.sy_lfr_rw3_k2 );
394 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k3, parameter_dump_packet.sy_lfr_rw3_k3 );
401 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k3, parameter_dump_packet.sy_lfr_rw3_k3 );
395 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k4, parameter_dump_packet.sy_lfr_rw3_k4 );
402 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k4, parameter_dump_packet.sy_lfr_rw3_k4 );
396 // rw4_k
403 // rw4_k
397 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k1, parameter_dump_packet.sy_lfr_rw4_k1 );
404 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k1, parameter_dump_packet.sy_lfr_rw4_k1 );
398 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k2, parameter_dump_packet.sy_lfr_rw4_k2 );
405 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k2, parameter_dump_packet.sy_lfr_rw4_k2 );
399 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k3, parameter_dump_packet.sy_lfr_rw4_k3 );
406 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k3, parameter_dump_packet.sy_lfr_rw4_k3 );
400 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k4, parameter_dump_packet.sy_lfr_rw4_k4 );
407 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k4, parameter_dump_packet.sy_lfr_rw4_k4 );
401
408
402 }
409 }
403
410
404 return flag;
411 return flag;
405 }
412 }
406
413
407 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
414 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
408 {
415 {
409 /** This function updates the LFR registers with the incoming sbm2 parameters.
416 /** This function updates the LFR registers with the incoming sbm2 parameters.
410 *
417 *
411 * @param TC points to the TeleCommand packet that is being processed
418 * @param TC points to the TeleCommand packet that is being processed
412 * @param queue_id is the id of the queue which handles TM related to this execution step
419 * @param queue_id is the id of the queue which handles TM related to this execution step
413 *
420 *
414 */
421 */
415
422
416 unsigned int address;
423 unsigned int address;
417 rtems_status_code status;
424 rtems_status_code status;
418 unsigned int freq;
425 unsigned int freq;
419 unsigned int bin;
426 unsigned int bin;
420 unsigned int coeff;
427 unsigned int coeff;
421 unsigned char *kCoeffPtr;
428 unsigned char *kCoeffPtr;
422 unsigned char *kCoeffDumpPtr;
429 unsigned char *kCoeffDumpPtr;
423
430
424 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
431 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
425 // F0 => 11 bins
432 // F0 => 11 bins
426 // F1 => 13 bins
433 // F1 => 13 bins
427 // F2 => 12 bins
434 // F2 => 12 bins
428 // 36 bins to dump in two packets (30 bins max per packet)
435 // 36 bins to dump in two packets (30 bins max per packet)
429
436
430 //*********
437 //*********
431 // PACKET 1
438 // PACKET 1
432 // 11 F0 bins, 13 F1 bins and 6 F2 bins
439 // 11 F0 bins, 13 F1 bins and 6 F2 bins
433 kcoefficients_dump_1.destinationID = TC->sourceID;
440 kcoefficients_dump_1.destinationID = TC->sourceID;
434 increment_seq_counter_destination_id_dump( kcoefficients_dump_1.packetSequenceControl, TC->sourceID );
441 increment_seq_counter_destination_id_dump( kcoefficients_dump_1.packetSequenceControl, TC->sourceID );
435 for( freq = 0;
442 for( freq = 0;
436 freq < NB_BINS_COMPRESSED_SM_F0;
443 freq < NB_BINS_COMPRESSED_SM_F0;
437 freq++ )
444 freq++ )
438 {
445 {
439 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1] = freq;
446 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1] = freq;
440 bin = freq;
447 bin = freq;
441 // printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
448 // printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
442 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
449 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
443 {
450 {
444 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
451 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
445 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
452 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
446 ]; // 2 for the kcoeff_frequency
453 ]; // 2 for the kcoeff_frequency
447 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
454 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
448 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
455 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
449 }
456 }
450 }
457 }
451 for( freq = NB_BINS_COMPRESSED_SM_F0;
458 for( freq = NB_BINS_COMPRESSED_SM_F0;
452 freq < ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
459 freq < ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
453 freq++ )
460 freq++ )
454 {
461 {
455 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = freq;
462 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = freq;
456 bin = freq - NB_BINS_COMPRESSED_SM_F0;
463 bin = freq - NB_BINS_COMPRESSED_SM_F0;
457 // printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
464 // printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
458 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
465 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
459 {
466 {
460 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
467 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
461 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
468 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
462 ]; // 2 for the kcoeff_frequency
469 ]; // 2 for the kcoeff_frequency
463 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
470 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
464 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
471 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
465 }
472 }
466 }
473 }
467 for( freq = ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
474 for( freq = ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
468 freq < KCOEFF_BLK_NR_PKT1 ;
475 freq < KCOEFF_BLK_NR_PKT1 ;
469 freq++ )
476 freq++ )
470 {
477 {
471 kcoefficients_dump_1.kcoeff_blks[ (freq * KCOEFF_BLK_SIZE) + 1 ] = freq;
478 kcoefficients_dump_1.kcoeff_blks[ (freq * KCOEFF_BLK_SIZE) + 1 ] = freq;
472 bin = freq - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
479 bin = freq - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
473 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
480 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
474 for ( coeff = 0; coeff <NB_K_COEFF_PER_BIN; coeff++ )
481 for ( coeff = 0; coeff <NB_K_COEFF_PER_BIN; coeff++ )
475 {
482 {
476 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
483 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
477 (freq * KCOEFF_BLK_SIZE) + (coeff * NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
484 (freq * KCOEFF_BLK_SIZE) + (coeff * NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
478 ]; // 2 for the kcoeff_frequency
485 ]; // 2 for the kcoeff_frequency
479 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
486 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
480 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
487 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
481 }
488 }
482 }
489 }
483 kcoefficients_dump_1.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
490 kcoefficients_dump_1.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
484 kcoefficients_dump_1.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
491 kcoefficients_dump_1.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
485 kcoefficients_dump_1.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
492 kcoefficients_dump_1.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
486 kcoefficients_dump_1.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
493 kcoefficients_dump_1.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
487 kcoefficients_dump_1.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
494 kcoefficients_dump_1.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
488 kcoefficients_dump_1.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
495 kcoefficients_dump_1.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
489 // SEND DATA
496 // SEND DATA
490 kcoefficient_node_1.status = 1;
497 kcoefficient_node_1.status = 1;
491 address = (unsigned int) &kcoefficient_node_1;
498 address = (unsigned int) &kcoefficient_node_1;
492 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
499 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
493 if (status != RTEMS_SUCCESSFUL) {
500 if (status != RTEMS_SUCCESSFUL) {
494 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
501 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
495 }
502 }
496
503
497 //********
504 //********
498 // PACKET 2
505 // PACKET 2
499 // 6 F2 bins
506 // 6 F2 bins
500 kcoefficients_dump_2.destinationID = TC->sourceID;
507 kcoefficients_dump_2.destinationID = TC->sourceID;
501 increment_seq_counter_destination_id_dump( kcoefficients_dump_2.packetSequenceControl, TC->sourceID );
508 increment_seq_counter_destination_id_dump( kcoefficients_dump_2.packetSequenceControl, TC->sourceID );
502 for( freq = 0;
509 for( freq = 0;
503 freq < KCOEFF_BLK_NR_PKT2;
510 freq < KCOEFF_BLK_NR_PKT2;
504 freq++ )
511 freq++ )
505 {
512 {
506 kcoefficients_dump_2.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = KCOEFF_BLK_NR_PKT1 + freq;
513 kcoefficients_dump_2.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = KCOEFF_BLK_NR_PKT1 + freq;
507 bin = freq + KCOEFF_BLK_NR_PKT2;
514 bin = freq + KCOEFF_BLK_NR_PKT2;
508 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
515 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
509 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
516 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
510 {
517 {
511 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[
518 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[
512 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ ]; // 2 for the kcoeff_frequency
519 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ ]; // 2 for the kcoeff_frequency
513 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
520 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
514 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
521 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
515 }
522 }
516 }
523 }
517 kcoefficients_dump_2.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
524 kcoefficients_dump_2.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
518 kcoefficients_dump_2.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
525 kcoefficients_dump_2.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
519 kcoefficients_dump_2.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
526 kcoefficients_dump_2.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
520 kcoefficients_dump_2.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
527 kcoefficients_dump_2.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
521 kcoefficients_dump_2.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
528 kcoefficients_dump_2.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
522 kcoefficients_dump_2.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
529 kcoefficients_dump_2.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
523 // SEND DATA
530 // SEND DATA
524 kcoefficient_node_2.status = 1;
531 kcoefficient_node_2.status = 1;
525 address = (unsigned int) &kcoefficient_node_2;
532 address = (unsigned int) &kcoefficient_node_2;
526 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
533 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
527 if (status != RTEMS_SUCCESSFUL) {
534 if (status != RTEMS_SUCCESSFUL) {
528 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
535 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
529 }
536 }
530
537
531 return status;
538 return status;
532 }
539 }
533
540
534 int action_dump_par( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
541 int action_dump_par( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
535 {
542 {
536 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
543 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
537 *
544 *
538 * @param queue_id is the id of the queue which handles TM related to this execution step.
545 * @param queue_id is the id of the queue which handles TM related to this execution step.
539 *
546 *
540 * @return RTEMS directive status codes:
547 * @return RTEMS directive status codes:
541 * - RTEMS_SUCCESSFUL - message sent successfully
548 * - RTEMS_SUCCESSFUL - message sent successfully
542 * - RTEMS_INVALID_ID - invalid queue id
549 * - RTEMS_INVALID_ID - invalid queue id
543 * - RTEMS_INVALID_SIZE - invalid message size
550 * - RTEMS_INVALID_SIZE - invalid message size
544 * - RTEMS_INVALID_ADDRESS - buffer is NULL
551 * - RTEMS_INVALID_ADDRESS - buffer is NULL
545 * - RTEMS_UNSATISFIED - out of message buffers
552 * - RTEMS_UNSATISFIED - out of message buffers
546 * - RTEMS_TOO_MANY - queue s limit has been reached
553 * - RTEMS_TOO_MANY - queue s limit has been reached
547 *
554 *
548 */
555 */
549
556
550 int status;
557 int status;
551
558
552 increment_seq_counter_destination_id_dump( parameter_dump_packet.packetSequenceControl, TC->sourceID );
559 increment_seq_counter_destination_id_dump( parameter_dump_packet.packetSequenceControl, TC->sourceID );
553 parameter_dump_packet.destinationID = TC->sourceID;
560 parameter_dump_packet.destinationID = TC->sourceID;
554
561
555 // UPDATE TIME
562 // UPDATE TIME
556 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
563 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
557 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
564 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
558 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
565 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
559 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
566 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
560 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
567 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
561 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
568 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
562 // SEND DATA
569 // SEND DATA
563 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
570 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
564 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
571 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
565 if (status != RTEMS_SUCCESSFUL) {
572 if (status != RTEMS_SUCCESSFUL) {
566 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
573 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
567 }
574 }
568
575
569 return status;
576 return status;
570 }
577 }
571
578
572 //***********************
579 //***********************
573 // NORMAL MODE PARAMETERS
580 // NORMAL MODE PARAMETERS
574
581
575 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
582 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
576 {
583 {
577 unsigned char msb;
584 unsigned char msb;
578 unsigned char lsb;
585 unsigned char lsb;
579 int flag;
586 int flag;
580 float aux;
587 float aux;
581 rtems_status_code status;
588 rtems_status_code status;
582
589
583 unsigned int sy_lfr_n_swf_l;
590 unsigned int sy_lfr_n_swf_l;
584 unsigned int sy_lfr_n_swf_p;
591 unsigned int sy_lfr_n_swf_p;
585 unsigned int sy_lfr_n_asm_p;
592 unsigned int sy_lfr_n_asm_p;
586 unsigned char sy_lfr_n_bp_p0;
593 unsigned char sy_lfr_n_bp_p0;
587 unsigned char sy_lfr_n_bp_p1;
594 unsigned char sy_lfr_n_bp_p1;
588 unsigned char sy_lfr_n_cwf_long_f3;
595 unsigned char sy_lfr_n_cwf_long_f3;
589
596
590 flag = LFR_SUCCESSFUL;
597 flag = LFR_SUCCESSFUL;
591
598
592 //***************
599 //***************
593 // get parameters
600 // get parameters
594 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
601 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
595 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
602 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
596 sy_lfr_n_swf_l = (msb * CONST_256) + lsb;
603 sy_lfr_n_swf_l = (msb * CONST_256) + lsb;
597
604
598 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
605 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
599 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
606 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
600 sy_lfr_n_swf_p = (msb * CONST_256) + lsb;
607 sy_lfr_n_swf_p = (msb * CONST_256) + lsb;
601
608
602 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
609 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
603 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
610 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
604 sy_lfr_n_asm_p = (msb * CONST_256) + lsb;
611 sy_lfr_n_asm_p = (msb * CONST_256) + lsb;
605
612
606 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
613 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
607
614
608 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
615 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
609
616
610 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
617 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
611
618
612 //******************
619 //******************
613 // check consistency
620 // check consistency
614 // sy_lfr_n_swf_l
621 // sy_lfr_n_swf_l
615 if (sy_lfr_n_swf_l != DFLT_SY_LFR_N_SWF_L)
622 if (sy_lfr_n_swf_l != DFLT_SY_LFR_N_SWF_L)
616 {
623 {
617 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L + DATAFIELD_OFFSET, sy_lfr_n_swf_l );
624 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L + DATAFIELD_OFFSET, sy_lfr_n_swf_l );
618 flag = WRONG_APP_DATA;
625 flag = WRONG_APP_DATA;
619 }
626 }
620 // sy_lfr_n_swf_p
627 // sy_lfr_n_swf_p
621 if (flag == LFR_SUCCESSFUL)
628 if (flag == LFR_SUCCESSFUL)
622 {
629 {
623 if ( sy_lfr_n_swf_p < MIN_SY_LFR_N_SWF_P )
630 if ( sy_lfr_n_swf_p < MIN_SY_LFR_N_SWF_P )
624 {
631 {
625 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P + DATAFIELD_OFFSET, sy_lfr_n_swf_p );
632 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P + DATAFIELD_OFFSET, sy_lfr_n_swf_p );
626 flag = WRONG_APP_DATA;
633 flag = WRONG_APP_DATA;
627 }
634 }
628 }
635 }
629 // sy_lfr_n_bp_p0
636 // sy_lfr_n_bp_p0
630 if (flag == LFR_SUCCESSFUL)
637 if (flag == LFR_SUCCESSFUL)
631 {
638 {
632 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
639 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
633 {
640 {
634 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0 + DATAFIELD_OFFSET, sy_lfr_n_bp_p0 );
641 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0 + DATAFIELD_OFFSET, sy_lfr_n_bp_p0 );
635 flag = WRONG_APP_DATA;
642 flag = WRONG_APP_DATA;
636 }
643 }
637 }
644 }
638 // sy_lfr_n_asm_p
645 // sy_lfr_n_asm_p
639 if (flag == LFR_SUCCESSFUL)
646 if (flag == LFR_SUCCESSFUL)
640 {
647 {
641 if (sy_lfr_n_asm_p == 0)
648 if (sy_lfr_n_asm_p == 0)
642 {
649 {
643 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
650 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
644 flag = WRONG_APP_DATA;
651 flag = WRONG_APP_DATA;
645 }
652 }
646 }
653 }
647 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
654 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
648 if (flag == LFR_SUCCESSFUL)
655 if (flag == LFR_SUCCESSFUL)
649 {
656 {
650 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
657 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
651 if (aux > FLOAT_EQUAL_ZERO)
658 if (aux > FLOAT_EQUAL_ZERO)
652 {
659 {
653 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
660 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
654 flag = WRONG_APP_DATA;
661 flag = WRONG_APP_DATA;
655 }
662 }
656 }
663 }
657 // sy_lfr_n_bp_p1
664 // sy_lfr_n_bp_p1
658 if (flag == LFR_SUCCESSFUL)
665 if (flag == LFR_SUCCESSFUL)
659 {
666 {
660 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
667 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
661 {
668 {
662 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
669 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
663 flag = WRONG_APP_DATA;
670 flag = WRONG_APP_DATA;
664 }
671 }
665 }
672 }
666 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
673 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
667 if (flag == LFR_SUCCESSFUL)
674 if (flag == LFR_SUCCESSFUL)
668 {
675 {
669 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
676 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
670 if (aux > FLOAT_EQUAL_ZERO)
677 if (aux > FLOAT_EQUAL_ZERO)
671 {
678 {
672 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
679 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
673 flag = LFR_DEFAULT;
680 flag = LFR_DEFAULT;
674 }
681 }
675 }
682 }
676 // sy_lfr_n_cwf_long_f3
683 // sy_lfr_n_cwf_long_f3
677
684
678 return flag;
685 return flag;
679 }
686 }
680
687
681 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
688 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
682 {
689 {
683 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
690 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
684 *
691 *
685 * @param TC points to the TeleCommand packet that is being processed
692 * @param TC points to the TeleCommand packet that is being processed
686 * @param queue_id is the id of the queue which handles TM related to this execution step
693 * @param queue_id is the id of the queue which handles TM related to this execution step
687 *
694 *
688 */
695 */
689
696
690 int result;
697 int result;
691
698
692 result = LFR_SUCCESSFUL;
699 result = LFR_SUCCESSFUL;
693
700
694 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
701 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
695 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
702 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
696
703
697 return result;
704 return result;
698 }
705 }
699
706
700 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
707 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
701 {
708 {
702 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
709 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
703 *
710 *
704 * @param TC points to the TeleCommand packet that is being processed
711 * @param TC points to the TeleCommand packet that is being processed
705 * @param queue_id is the id of the queue which handles TM related to this execution step
712 * @param queue_id is the id of the queue which handles TM related to this execution step
706 *
713 *
707 */
714 */
708
715
709 int result;
716 int result;
710
717
711 result = LFR_SUCCESSFUL;
718 result = LFR_SUCCESSFUL;
712
719
713 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
720 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
714 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
721 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
715
722
716 return result;
723 return result;
717 }
724 }
718
725
719 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
726 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
720 {
727 {
721 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
728 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
722 *
729 *
723 * @param TC points to the TeleCommand packet that is being processed
730 * @param TC points to the TeleCommand packet that is being processed
724 * @param queue_id is the id of the queue which handles TM related to this execution step
731 * @param queue_id is the id of the queue which handles TM related to this execution step
725 *
732 *
726 */
733 */
727
734
728 int result;
735 int result;
729
736
730 result = LFR_SUCCESSFUL;
737 result = LFR_SUCCESSFUL;
731
738
732 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
739 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
733 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
740 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
734
741
735 return result;
742 return result;
736 }
743 }
737
744
738 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
745 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
739 {
746 {
740 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
747 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
741 *
748 *
742 * @param TC points to the TeleCommand packet that is being processed
749 * @param TC points to the TeleCommand packet that is being processed
743 * @param queue_id is the id of the queue which handles TM related to this execution step
750 * @param queue_id is the id of the queue which handles TM related to this execution step
744 *
751 *
745 */
752 */
746
753
747 int status;
754 int status;
748
755
749 status = LFR_SUCCESSFUL;
756 status = LFR_SUCCESSFUL;
750
757
751 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
758 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
752
759
753 return status;
760 return status;
754 }
761 }
755
762
756 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
763 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
757 {
764 {
758 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
765 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
759 *
766 *
760 * @param TC points to the TeleCommand packet that is being processed
767 * @param TC points to the TeleCommand packet that is being processed
761 * @param queue_id is the id of the queue which handles TM related to this execution step
768 * @param queue_id is the id of the queue which handles TM related to this execution step
762 *
769 *
763 */
770 */
764
771
765 int status;
772 int status;
766
773
767 status = LFR_SUCCESSFUL;
774 status = LFR_SUCCESSFUL;
768
775
769 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
776 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
770
777
771 return status;
778 return status;
772 }
779 }
773
780
774 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
781 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
775 {
782 {
776 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
783 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
777 *
784 *
778 * @param TC points to the TeleCommand packet that is being processed
785 * @param TC points to the TeleCommand packet that is being processed
779 * @param queue_id is the id of the queue which handles TM related to this execution step
786 * @param queue_id is the id of the queue which handles TM related to this execution step
780 *
787 *
781 */
788 */
782
789
783 int status;
790 int status;
784
791
785 status = LFR_SUCCESSFUL;
792 status = LFR_SUCCESSFUL;
786
793
787 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
794 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
788
795
789 return status;
796 return status;
790 }
797 }
791
798
792 //**********************
799 //**********************
793 // BURST MODE PARAMETERS
800 // BURST MODE PARAMETERS
794
801
795 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
802 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
796 {
803 {
797 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
804 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
798 *
805 *
799 * @param TC points to the TeleCommand packet that is being processed
806 * @param TC points to the TeleCommand packet that is being processed
800 * @param queue_id is the id of the queue which handles TM related to this execution step
807 * @param queue_id is the id of the queue which handles TM related to this execution step
801 *
808 *
802 */
809 */
803
810
804 int status;
811 int status;
805
812
806 status = LFR_SUCCESSFUL;
813 status = LFR_SUCCESSFUL;
807
814
808 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
815 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
809
816
810 return status;
817 return status;
811 }
818 }
812
819
813 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
820 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
814 {
821 {
815 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
822 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
816 *
823 *
817 * @param TC points to the TeleCommand packet that is being processed
824 * @param TC points to the TeleCommand packet that is being processed
818 * @param queue_id is the id of the queue which handles TM related to this execution step
825 * @param queue_id is the id of the queue which handles TM related to this execution step
819 *
826 *
820 */
827 */
821
828
822 int status;
829 int status;
823
830
824 status = LFR_SUCCESSFUL;
831 status = LFR_SUCCESSFUL;
825
832
826 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
833 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
827
834
828 return status;
835 return status;
829 }
836 }
830
837
831 //*********************
838 //*********************
832 // SBM1 MODE PARAMETERS
839 // SBM1 MODE PARAMETERS
833
840
834 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
841 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
835 {
842 {
836 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
843 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
837 *
844 *
838 * @param TC points to the TeleCommand packet that is being processed
845 * @param TC points to the TeleCommand packet that is being processed
839 * @param queue_id is the id of the queue which handles TM related to this execution step
846 * @param queue_id is the id of the queue which handles TM related to this execution step
840 *
847 *
841 */
848 */
842
849
843 int status;
850 int status;
844
851
845 status = LFR_SUCCESSFUL;
852 status = LFR_SUCCESSFUL;
846
853
847 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
854 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
848
855
849 return status;
856 return status;
850 }
857 }
851
858
852 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
859 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
853 {
860 {
854 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
861 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
855 *
862 *
856 * @param TC points to the TeleCommand packet that is being processed
863 * @param TC points to the TeleCommand packet that is being processed
857 * @param queue_id is the id of the queue which handles TM related to this execution step
864 * @param queue_id is the id of the queue which handles TM related to this execution step
858 *
865 *
859 */
866 */
860
867
861 int status;
868 int status;
862
869
863 status = LFR_SUCCESSFUL;
870 status = LFR_SUCCESSFUL;
864
871
865 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
872 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
866
873
867 return status;
874 return status;
868 }
875 }
869
876
870 //*********************
877 //*********************
871 // SBM2 MODE PARAMETERS
878 // SBM2 MODE PARAMETERS
872
879
873 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC )
880 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC )
874 {
881 {
875 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
882 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
876 *
883 *
877 * @param TC points to the TeleCommand packet that is being processed
884 * @param TC points to the TeleCommand packet that is being processed
878 * @param queue_id is the id of the queue which handles TM related to this execution step
885 * @param queue_id is the id of the queue which handles TM related to this execution step
879 *
886 *
880 */
887 */
881
888
882 int status;
889 int status;
883
890
884 status = LFR_SUCCESSFUL;
891 status = LFR_SUCCESSFUL;
885
892
886 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
893 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
887
894
888 return status;
895 return status;
889 }
896 }
890
897
891 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
898 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
892 {
899 {
893 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
900 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
894 *
901 *
895 * @param TC points to the TeleCommand packet that is being processed
902 * @param TC points to the TeleCommand packet that is being processed
896 * @param queue_id is the id of the queue which handles TM related to this execution step
903 * @param queue_id is the id of the queue which handles TM related to this execution step
897 *
904 *
898 */
905 */
899
906
900 int status;
907 int status;
901
908
902 status = LFR_SUCCESSFUL;
909 status = LFR_SUCCESSFUL;
903
910
904 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
911 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
905
912
906 return status;
913 return status;
907 }
914 }
908
915
909 //*******************
916 //*******************
910 // TC_LFR_UPDATE_INFO
917 // TC_LFR_UPDATE_INFO
911
918
912 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
919 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
913 {
920 {
914 unsigned int status;
921 unsigned int status;
915
922
916 status = LFR_DEFAULT;
923 status = LFR_DEFAULT;
917
924
918 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
925 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
919 || (mode == LFR_MODE_BURST)
926 || (mode == LFR_MODE_BURST)
920 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
927 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
921 {
928 {
922 status = LFR_SUCCESSFUL;
929 status = LFR_SUCCESSFUL;
923 }
930 }
924 else
931 else
925 {
932 {
926 status = LFR_DEFAULT;
933 status = LFR_DEFAULT;
927 }
934 }
928
935
929 return status;
936 return status;
930 }
937 }
931
938
932 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
939 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
933 {
940 {
934 unsigned int status;
941 unsigned int status;
935
942
936 status = LFR_DEFAULT;
943 status = LFR_DEFAULT;
937
944
938 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
945 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
939 || (mode == TDS_MODE_BURST)
946 || (mode == TDS_MODE_BURST)
940 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
947 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
941 || (mode == TDS_MODE_LFM))
948 || (mode == TDS_MODE_LFM))
942 {
949 {
943 status = LFR_SUCCESSFUL;
950 status = LFR_SUCCESSFUL;
944 }
951 }
945 else
952 else
946 {
953 {
947 status = LFR_DEFAULT;
954 status = LFR_DEFAULT;
948 }
955 }
949
956
950 return status;
957 return status;
951 }
958 }
952
959
953 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
960 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
954 {
961 {
955 unsigned int status;
962 unsigned int status;
956
963
957 status = LFR_DEFAULT;
964 status = LFR_DEFAULT;
958
965
959 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
966 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
960 || (mode == THR_MODE_BURST))
967 || (mode == THR_MODE_BURST))
961 {
968 {
962 status = LFR_SUCCESSFUL;
969 status = LFR_SUCCESSFUL;
963 }
970 }
964 else
971 else
965 {
972 {
966 status = LFR_DEFAULT;
973 status = LFR_DEFAULT;
967 }
974 }
968
975
969 return status;
976 return status;
970 }
977 }
971
978
972 void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value )
979 void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value )
973 {
980 {
974 unsigned char flag;
981 unsigned char flag;
975 unsigned char flagPosInByte;
982 unsigned char flagPosInByte;
976 unsigned char newFlag;
983 unsigned char newFlag;
977 unsigned char flagMask;
984 unsigned char flagMask;
978
985
979 // if the frequency value is not a number, the flag is set to 0 and the frequency RWx_Fy is not filtered
986 // if the frequency value is not a number, the flag is set to 0 and the frequency RWx_Fy is not filtered
980 if (isnan(value))
987 if (isnan(value))
981 {
988 {
982 flag = FLAG_NAN;
989 flag = FLAG_NAN;
983 }
990 }
984 else
991 else
985 {
992 {
986 flag = FLAG_IAN;
993 flag = FLAG_IAN;
987 }
994 }
988
995
989 switch(wheel)
996 switch(wheel)
990 {
997 {
991 case WHEEL_1:
998 case WHEEL_1:
992 flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
999 flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
993 flagMask = ~(1 << flagPosInByte);
1000 flagMask = ~(1 << flagPosInByte);
994 newFlag = flag << flagPosInByte;
1001 newFlag = flag << flagPosInByte;
995 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
1002 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
996 break;
1003 break;
997 case WHEEL_2:
1004 case WHEEL_2:
998 flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
1005 flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
999 flagMask = ~(1 << flagPosInByte);
1006 flagMask = ~(1 << flagPosInByte);
1000 newFlag = flag << flagPosInByte;
1007 newFlag = flag << flagPosInByte;
1001 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
1008 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
1002 break;
1009 break;
1003 case WHEEL_3:
1010 case WHEEL_3:
1004 flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
1011 flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
1005 flagMask = ~(1 << flagPosInByte);
1012 flagMask = ~(1 << flagPosInByte);
1006 newFlag = flag << flagPosInByte;
1013 newFlag = flag << flagPosInByte;
1007 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1014 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1008 break;
1015 break;
1009 case WHEEL_4:
1016 case WHEEL_4:
1010 flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
1017 flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
1011 flagMask = ~(1 << flagPosInByte);
1018 flagMask = ~(1 << flagPosInByte);
1012 newFlag = flag << flagPosInByte;
1019 newFlag = flag << flagPosInByte;
1013 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1020 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1014 break;
1021 break;
1015 default:
1022 default:
1016 break;
1023 break;
1017 }
1024 }
1018 }
1025 }
1019
1026
1020 void set_hk_lfr_sc_rw_f_flags( void )
1027 void set_hk_lfr_sc_rw_f_flags( void )
1021 {
1028 {
1022 // RW1
1029 // RW1
1023 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_1, rw_f.cp_rpw_sc_rw1_f1 );
1030 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_1, rw_f.cp_rpw_sc_rw1_f1 );
1024 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_2, rw_f.cp_rpw_sc_rw1_f2 );
1031 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_2, rw_f.cp_rpw_sc_rw1_f2 );
1025 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_3, rw_f.cp_rpw_sc_rw1_f3 );
1032 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_3, rw_f.cp_rpw_sc_rw1_f3 );
1026 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_4, rw_f.cp_rpw_sc_rw1_f4 );
1033 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_4, rw_f.cp_rpw_sc_rw1_f4 );
1027
1034
1028 // RW2
1035 // RW2
1029 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_1, rw_f.cp_rpw_sc_rw2_f1 );
1036 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_1, rw_f.cp_rpw_sc_rw2_f1 );
1030 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_2, rw_f.cp_rpw_sc_rw2_f2 );
1037 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_2, rw_f.cp_rpw_sc_rw2_f2 );
1031 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_3, rw_f.cp_rpw_sc_rw2_f3 );
1038 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_3, rw_f.cp_rpw_sc_rw2_f3 );
1032 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_4, rw_f.cp_rpw_sc_rw2_f4 );
1039 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_4, rw_f.cp_rpw_sc_rw2_f4 );
1033
1040
1034 // RW3
1041 // RW3
1035 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_1, rw_f.cp_rpw_sc_rw3_f1 );
1042 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_1, rw_f.cp_rpw_sc_rw3_f1 );
1036 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_2, rw_f.cp_rpw_sc_rw3_f2 );
1043 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_2, rw_f.cp_rpw_sc_rw3_f2 );
1037 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_3, rw_f.cp_rpw_sc_rw3_f3 );
1044 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_3, rw_f.cp_rpw_sc_rw3_f3 );
1038 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_4, rw_f.cp_rpw_sc_rw3_f4 );
1045 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_4, rw_f.cp_rpw_sc_rw3_f4 );
1039
1046
1040 // RW4
1047 // RW4
1041 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_1, rw_f.cp_rpw_sc_rw4_f1 );
1048 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_1, rw_f.cp_rpw_sc_rw4_f1 );
1042 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_2, rw_f.cp_rpw_sc_rw4_f2 );
1049 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_2, rw_f.cp_rpw_sc_rw4_f2 );
1043 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_3, rw_f.cp_rpw_sc_rw4_f3 );
1050 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_3, rw_f.cp_rpw_sc_rw4_f3 );
1044 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_4, rw_f.cp_rpw_sc_rw4_f4 );
1051 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_4, rw_f.cp_rpw_sc_rw4_f4 );
1045 }
1052 }
1046
1053
1047 int check_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value )
1054 int check_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value )
1048 {
1055 {
1049 float rw_k;
1056 float rw_k;
1050 int ret;
1057 int ret;
1051
1058
1052 ret = LFR_SUCCESSFUL;
1059 ret = LFR_SUCCESSFUL;
1053 rw_k = INIT_FLOAT;
1060 rw_k = INIT_FLOAT;
1054
1061
1055 copyFloatByChar( (unsigned char*) &rw_k, (unsigned char*) &TC->packetID[ offset ] );
1062 copyFloatByChar( (unsigned char*) &rw_k, (unsigned char*) &TC->packetID[ offset ] );
1056
1063
1057 *pos = offset;
1064 *pos = offset;
1058 *value = rw_k;
1065 *value = rw_k;
1059
1066
1060 if (rw_k < MIN_SY_LFR_RW_F)
1067 if (rw_k < MIN_SY_LFR_RW_F)
1061 {
1068 {
1062 ret = WRONG_APP_DATA;
1069 ret = WRONG_APP_DATA;
1063 }
1070 }
1064
1071
1065 return ret;
1072 return ret;
1066 }
1073 }
1067
1074
1068 int check_all_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int *pos, float*value )
1075 int check_all_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int *pos, float*value )
1069 {
1076 {
1070 int ret;
1077 int ret;
1071
1078
1072 ret = LFR_SUCCESSFUL;
1079 ret = LFR_SUCCESSFUL;
1073
1080
1074 //****
1081 //****
1075 //****
1082 //****
1076 // RW1
1083 // RW1
1077 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1, pos, value ); // F1
1084 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1, pos, value ); // F1
1078 if (ret == LFR_SUCCESSFUL) // F2
1085 if (ret == LFR_SUCCESSFUL) // F2
1079 {
1086 {
1080 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2, pos, value );
1087 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2, pos, value );
1081 }
1088 }
1082 if (ret == LFR_SUCCESSFUL) // F3
1089 if (ret == LFR_SUCCESSFUL) // F3
1083 {
1090 {
1084 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3, pos, value );
1091 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3, pos, value );
1085 }
1092 }
1086 if (ret == LFR_SUCCESSFUL) // F4
1093 if (ret == LFR_SUCCESSFUL) // F4
1087 {
1094 {
1088 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4, pos, value );
1095 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4, pos, value );
1089 }
1096 }
1090
1097
1091 //****
1098 //****
1092 //****
1099 //****
1093 // RW2
1100 // RW2
1094 if (ret == LFR_SUCCESSFUL) // F1
1101 if (ret == LFR_SUCCESSFUL) // F1
1095 {
1102 {
1096 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1, pos, value );
1103 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1, pos, value );
1097 }
1104 }
1098 if (ret == LFR_SUCCESSFUL) // F2
1105 if (ret == LFR_SUCCESSFUL) // F2
1099 {
1106 {
1100 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2, pos, value );
1107 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2, pos, value );
1101 }
1108 }
1102 if (ret == LFR_SUCCESSFUL) // F3
1109 if (ret == LFR_SUCCESSFUL) // F3
1103 {
1110 {
1104 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3, pos, value );
1111 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3, pos, value );
1105 }
1112 }
1106 if (ret == LFR_SUCCESSFUL) // F4
1113 if (ret == LFR_SUCCESSFUL) // F4
1107 {
1114 {
1108 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4, pos, value );
1115 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4, pos, value );
1109 }
1116 }
1110
1117
1111 //****
1118 //****
1112 //****
1119 //****
1113 // RW3
1120 // RW3
1114 if (ret == LFR_SUCCESSFUL) // F1
1121 if (ret == LFR_SUCCESSFUL) // F1
1115 {
1122 {
1116 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1, pos, value );
1123 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1, pos, value );
1117 }
1124 }
1118 if (ret == LFR_SUCCESSFUL) // F2
1125 if (ret == LFR_SUCCESSFUL) // F2
1119 {
1126 {
1120 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2, pos, value );
1127 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2, pos, value );
1121 }
1128 }
1122 if (ret == LFR_SUCCESSFUL) // F3
1129 if (ret == LFR_SUCCESSFUL) // F3
1123 {
1130 {
1124 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3, pos, value );
1131 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3, pos, value );
1125 }
1132 }
1126 if (ret == LFR_SUCCESSFUL) // F4
1133 if (ret == LFR_SUCCESSFUL) // F4
1127 {
1134 {
1128 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4, pos, value );
1135 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4, pos, value );
1129 }
1136 }
1130
1137
1131 //****
1138 //****
1132 //****
1139 //****
1133 // RW4
1140 // RW4
1134 if (ret == LFR_SUCCESSFUL) // F1
1141 if (ret == LFR_SUCCESSFUL) // F1
1135 {
1142 {
1136 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1, pos, value );
1143 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1, pos, value );
1137 }
1144 }
1138 if (ret == LFR_SUCCESSFUL) // F2
1145 if (ret == LFR_SUCCESSFUL) // F2
1139 {
1146 {
1140 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2, pos, value );
1147 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2, pos, value );
1141 }
1148 }
1142 if (ret == LFR_SUCCESSFUL) // F3
1149 if (ret == LFR_SUCCESSFUL) // F3
1143 {
1150 {
1144 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3, pos, value );
1151 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3, pos, value );
1145 }
1152 }
1146 if (ret == LFR_SUCCESSFUL) // F4
1153 if (ret == LFR_SUCCESSFUL) // F4
1147 {
1154 {
1148 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4, pos, value );
1155 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4, pos, value );
1149 }
1156 }
1150
1157
1151 return ret;
1158 return ret;
1152 }
1159 }
1153
1160
1154 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC )
1161 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC )
1155 {
1162 {
1156 /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally.
1163 /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally.
1157 *
1164 *
1158 * @param TC points to the TeleCommand packet that is being processed
1165 * @param TC points to the TeleCommand packet that is being processed
1159 *
1166 *
1160 */
1167 */
1161
1168
1162 unsigned char * bytePosPtr; // pointer to the beginning of the incoming TC packet
1169 unsigned char * bytePosPtr; // pointer to the beginning of the incoming TC packet
1163
1170
1164 bytePosPtr = (unsigned char *) &TC->packetID;
1171 bytePosPtr = (unsigned char *) &TC->packetID;
1165
1172
1166 // rw1_f
1173 // rw1_f
1167 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] );
1174 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] );
1168 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] );
1175 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] );
1169 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3 ] );
1176 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3 ] );
1170 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4 ] );
1177 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4 ] );
1171
1178
1172 // rw2_f
1179 // rw2_f
1173 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] );
1180 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] );
1174 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] );
1181 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] );
1175 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3 ] );
1182 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3 ] );
1176 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4 ] );
1183 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4 ] );
1177
1184
1178 // rw3_f
1185 // rw3_f
1179 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] );
1186 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] );
1180 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] );
1187 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] );
1181 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3 ] );
1188 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3 ] );
1182 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4 ] );
1189 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4 ] );
1183
1190
1184 // rw4_f
1191 // rw4_f
1185 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] );
1192 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] );
1186 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] );
1193 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] );
1187 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3 ] );
1194 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3 ] );
1188 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4 ] );
1195 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4 ] );
1189
1196
1190 // test each reaction wheel frequency value. NaN means that the frequency is not filtered
1197 // test each reaction wheel frequency value. NaN means that the frequency is not filtered
1191
1198
1192 }
1199 }
1193
1200
1194 void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, float sy_lfr_rw_k )
1201 void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, float sy_lfr_rw_k )
1195 {
1202 {
1196 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
1203 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
1197 *
1204 *
1198 * @param fbins_mask
1205 * @param fbins_mask
1199 * @param rw_f is the reaction wheel frequency to filter
1206 * @param rw_f is the reaction wheel frequency to filter
1200 * @param delta_f is the frequency step between the frequency bins, it depends on the frequency channel
1207 * @param delta_f is the frequency step between the frequency bins, it depends on the frequency channel
1201 * @param flag [true] filtering enabled [false] filtering disabled
1208 * @param flag [true] filtering enabled [false] filtering disabled
1202 *
1209 *
1203 * @return void
1210 * @return void
1204 *
1211 *
1205 */
1212 */
1206
1213
1207 float f_RW_min;
1214 float f_RW_min;
1208 float f_RW_MAX;
1215 float f_RW_MAX;
1209 float fi_min;
1216 float fi_min;
1210 float fi_MAX;
1217 float fi_MAX;
1211 float fi;
1218 float fi;
1212 float deltaBelow;
1219 float deltaBelow;
1213 float deltaAbove;
1220 float deltaAbove;
1214 float freqToFilterOut;
1221 float freqToFilterOut;
1215 int binBelow;
1222 int binBelow;
1216 int binAbove;
1223 int binAbove;
1217 int closestBin;
1224 int closestBin;
1218 unsigned int whichByte;
1225 unsigned int whichByte;
1219 int selectedByte;
1226 int selectedByte;
1220 int bin;
1227 int bin;
1221 int binToRemove[NB_BINS_TO_REMOVE];
1228 int binToRemove[NB_BINS_TO_REMOVE];
1222 int k;
1229 int k;
1223 bool filteringSet;
1230 bool filteringSet;
1224
1231
1225 closestBin = 0;
1232 closestBin = 0;
1226 whichByte = 0;
1233 whichByte = 0;
1227 bin = 0;
1234 bin = 0;
1228 filteringSet = false;
1235 filteringSet = false;
1229
1236
1230 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1237 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1231 {
1238 {
1232 binToRemove[k] = -1;
1239 binToRemove[k] = -1;
1233 }
1240 }
1234
1241
1235 if (!isnan(rw_f))
1242 if (!isnan(rw_f))
1236 {
1243 {
1237 // compute the frequency range to filter [ rw_f - delta_f; rw_f + delta_f ]
1244 // compute the frequency range to filter [ rw_f - delta_f; rw_f + delta_f ]
1238 f_RW_min = rw_f - ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
1245 f_RW_min = rw_f - ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
1239 f_RW_MAX = rw_f + ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
1246 f_RW_MAX = rw_f + ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
1240
1247
1241 freqToFilterOut = f_RW_min;
1248 freqToFilterOut = f_RW_min;
1242 while ( filteringSet == false )
1249 while ( filteringSet == false )
1243 {
1250 {
1244 // compute the index of the frequency bin immediately below rw_f
1251 // compute the index of the frequency bin immediately below rw_f
1245 binBelow = (int) ( floor( ((double) freqToFilterOut) / ((double) deltaFreq)) );
1252 binBelow = (int) ( floor( ((double) freqToFilterOut) / ((double) deltaFreq)) );
1246 deltaBelow = freqToFilterOut - binBelow * deltaFreq;
1253 deltaBelow = freqToFilterOut - binBelow * deltaFreq;
1247
1254
1248 // compute the index of the frequency bin immediately above rw_f
1255 // compute the index of the frequency bin immediately above rw_f
1249 binAbove = (int) ( ceil( ((double) freqToFilterOut) / ((double) deltaFreq)) );
1256 binAbove = (int) ( ceil( ((double) freqToFilterOut) / ((double) deltaFreq)) );
1250 deltaAbove = binAbove * deltaFreq - freqToFilterOut;
1257 deltaAbove = binAbove * deltaFreq - freqToFilterOut;
1251
1258
1252 // search the closest bin
1259 // search the closest bin
1253 if (deltaAbove > deltaBelow)
1260 if (deltaAbove > deltaBelow)
1254 {
1261 {
1255 closestBin = binBelow;
1262 closestBin = binBelow;
1256 }
1263 }
1257 else
1264 else
1258 {
1265 {
1259 closestBin = binAbove;
1266 closestBin = binAbove;
1260 }
1267 }
1261
1268
1262 // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285]
1269 // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285]
1263 fi = closestBin * deltaFreq;
1270 fi = closestBin * deltaFreq;
1264 fi_min = fi - (deltaFreq * FI_INTERVAL_COEFF);
1271 fi_min = fi - (deltaFreq * FI_INTERVAL_COEFF);
1265 fi_MAX = fi + (deltaFreq * FI_INTERVAL_COEFF);
1272 fi_MAX = fi + (deltaFreq * FI_INTERVAL_COEFF);
1266
1273
1267 //**************************************************************************************
1274 //**************************************************************************************
1268 // be careful here, one shall take into account that the bin 0 IS DROPPED in the spectra
1275 // be careful here, one shall take into account that the bin 0 IS DROPPED in the spectra
1269 // thus, the index 0 in a mask corresponds to the bin 1 of the spectrum
1276 // thus, the index 0 in a mask corresponds to the bin 1 of the spectrum
1270 //**************************************************************************************
1277 //**************************************************************************************
1271
1278
1272 // 1. IF freqToFilterOut is included in [ fi_min; fi_MAX ]
1279 // 1. IF freqToFilterOut is included in [ fi_min; fi_MAX ]
1273 // => remove f_(i), f_(i-1) and f_(i+1)
1280 // => remove f_(i), f_(i-1) and f_(i+1)
1274 if ( ( freqToFilterOut > fi_min ) && ( freqToFilterOut < fi_MAX ) )
1281 if ( ( freqToFilterOut > fi_min ) && ( freqToFilterOut < fi_MAX ) )
1275 {
1282 {
1276 binToRemove[0] = (closestBin - 1) - 1;
1283 binToRemove[0] = (closestBin - 1) - 1;
1277 binToRemove[1] = (closestBin) - 1;
1284 binToRemove[1] = (closestBin) - 1;
1278 binToRemove[2] = (closestBin + 1) - 1;
1285 binToRemove[2] = (closestBin + 1) - 1;
1279 }
1286 }
1280 // 2. ELSE
1287 // 2. ELSE
1281 // => remove the two f_(i) which are around f_RW
1288 // => remove the two f_(i) which are around f_RW
1282 else
1289 else
1283 {
1290 {
1284 binToRemove[0] = (binBelow) - 1;
1291 binToRemove[0] = (binBelow) - 1;
1285 binToRemove[1] = (binAbove) - 1;
1292 binToRemove[1] = (binAbove) - 1;
1286 binToRemove[2] = (-1);
1293 binToRemove[2] = (-1);
1287 }
1294 }
1288
1295
1289 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1296 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1290 {
1297 {
1291 bin = binToRemove[k];
1298 bin = binToRemove[k];
1292 if ( (bin >= BIN_MIN) && (bin <= BIN_MAX) )
1299 if ( (bin >= BIN_MIN) && (bin <= BIN_MAX) )
1293 {
1300 {
1294 whichByte = (bin >> SHIFT_3_BITS); // division by 8
1301 whichByte = (bin >> SHIFT_3_BITS); // division by 8
1295 selectedByte = ( 1 << (bin - (whichByte * BITS_PER_BYTE)) );
1302 selectedByte = ( 1 << (bin - (whichByte * BITS_PER_BYTE)) );
1296 fbins_mask[BYTES_PER_MASK - 1 - whichByte] =
1303 fbins_mask[BYTES_PER_MASK - 1 - whichByte] =
1297 fbins_mask[BYTES_PER_MASK - 1 - whichByte] & ((unsigned char) (~selectedByte)); // bytes are ordered MSB first in the packets
1304 fbins_mask[BYTES_PER_MASK - 1 - whichByte] & ((unsigned char) (~selectedByte)); // bytes are ordered MSB first in the packets
1298
1305
1299 }
1306 }
1300 }
1307 }
1301
1308
1302 // update freqToFilterOut
1309 // update freqToFilterOut
1303 if ( freqToFilterOut == f_RW_MAX )
1310 if ( freqToFilterOut == f_RW_MAX )
1304 {
1311 {
1305 filteringSet = true; // end of the loop
1312 filteringSet = true; // end of the loop
1306 }
1313 }
1307 else
1314 else
1308 {
1315 {
1309 freqToFilterOut = freqToFilterOut + deltaFreq;
1316 freqToFilterOut = freqToFilterOut + deltaFreq;
1310 }
1317 }
1311
1318
1312 if ( freqToFilterOut > f_RW_MAX)
1319 if ( freqToFilterOut > f_RW_MAX)
1313 {
1320 {
1314 freqToFilterOut = f_RW_MAX;
1321 freqToFilterOut = f_RW_MAX;
1315 }
1322 }
1316 }
1323 }
1317 }
1324 }
1318 }
1325 }
1319
1326
1320 void build_sy_lfr_rw_mask( unsigned int channel )
1327 void build_sy_lfr_rw_mask( unsigned int channel )
1321 {
1328 {
1322 unsigned char local_rw_fbins_mask[BYTES_PER_MASK];
1329 unsigned char local_rw_fbins_mask[BYTES_PER_MASK];
1323 unsigned char *maskPtr;
1330 unsigned char *maskPtr;
1324 double deltaF;
1331 double deltaF;
1325 unsigned k;
1332 unsigned k;
1326
1333
1327 maskPtr = NULL;
1334 maskPtr = NULL;
1328 deltaF = DELTAF_F2;
1335 deltaF = DELTAF_F2;
1329
1336
1330 switch (channel)
1337 switch (channel)
1331 {
1338 {
1332 case CHANNELF0:
1339 case CHANNELF0:
1333 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1340 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1334 deltaF = DELTAF_F0;
1341 deltaF = DELTAF_F0;
1335 break;
1342 break;
1336 case CHANNELF1:
1343 case CHANNELF1:
1337 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1344 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1338 deltaF = DELTAF_F1;
1345 deltaF = DELTAF_F1;
1339 break;
1346 break;
1340 case CHANNELF2:
1347 case CHANNELF2:
1341 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1348 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1342 deltaF = DELTAF_F2;
1349 deltaF = DELTAF_F2;
1343 break;
1350 break;
1344 default:
1351 default:
1345 break;
1352 break;
1346 }
1353 }
1347
1354
1348 for (k = 0; k < BYTES_PER_MASK; k++)
1355 for (k = 0; k < BYTES_PER_MASK; k++)
1349 {
1356 {
1350 local_rw_fbins_mask[k] = INT8_ALL_F;
1357 local_rw_fbins_mask[k] = INT8_ALL_F;
1351 }
1358 }
1352
1359
1353 // RW1
1360 // RW1
1354 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, filterPar.sy_lfr_rw1_k1 );
1361 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, filterPar.sy_lfr_rw1_k1 );
1355 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, filterPar.sy_lfr_rw1_k2 );
1362 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, filterPar.sy_lfr_rw1_k2 );
1356 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f3, deltaF, filterPar.sy_lfr_rw1_k3 );
1363 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f3, deltaF, filterPar.sy_lfr_rw1_k3 );
1357 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f4, deltaF, filterPar.sy_lfr_rw1_k4 );
1364 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f4, deltaF, filterPar.sy_lfr_rw1_k4 );
1358
1365
1359 // RW2
1366 // RW2
1360 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, filterPar.sy_lfr_rw2_k1 );
1367 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, filterPar.sy_lfr_rw2_k1 );
1361 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, filterPar.sy_lfr_rw2_k2 );
1368 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, filterPar.sy_lfr_rw2_k2 );
1362 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f3, deltaF, filterPar.sy_lfr_rw2_k3 );
1369 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f3, deltaF, filterPar.sy_lfr_rw2_k3 );
1363 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f4, deltaF, filterPar.sy_lfr_rw2_k4 );
1370 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f4, deltaF, filterPar.sy_lfr_rw2_k4 );
1364
1371
1365 // RW3
1372 // RW3
1366 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, filterPar.sy_lfr_rw3_k1 );
1373 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, filterPar.sy_lfr_rw3_k1 );
1367 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, filterPar.sy_lfr_rw3_k2 );
1374 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, filterPar.sy_lfr_rw3_k2 );
1368 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f3, deltaF, filterPar.sy_lfr_rw3_k3 );
1375 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f3, deltaF, filterPar.sy_lfr_rw3_k3 );
1369 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f4, deltaF, filterPar.sy_lfr_rw3_k4 );
1376 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f4, deltaF, filterPar.sy_lfr_rw3_k4 );
1370
1377
1371 // RW4
1378 // RW4
1372 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, filterPar.sy_lfr_rw4_k1 );
1379 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, filterPar.sy_lfr_rw4_k1 );
1373 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, filterPar.sy_lfr_rw4_k2 );
1380 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, filterPar.sy_lfr_rw4_k2 );
1374 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f3, deltaF, filterPar.sy_lfr_rw4_k3 );
1381 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f3, deltaF, filterPar.sy_lfr_rw4_k3 );
1375 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f4, deltaF, filterPar.sy_lfr_rw4_k4 );
1382 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f4, deltaF, filterPar.sy_lfr_rw4_k4 );
1376
1383
1377 // update the value of the fbins related to reaction wheels frequency filtering
1384 // update the value of the fbins related to reaction wheels frequency filtering
1378 if (maskPtr != NULL)
1385 if (maskPtr != NULL)
1379 {
1386 {
1380 for (k = 0; k < BYTES_PER_MASK; k++)
1387 for (k = 0; k < BYTES_PER_MASK; k++)
1381 {
1388 {
1382 maskPtr[k] = local_rw_fbins_mask[k];
1389 maskPtr[k] = local_rw_fbins_mask[k];
1383 }
1390 }
1384 }
1391 }
1385 }
1392 }
1386
1393
1387 void build_sy_lfr_rw_masks( void )
1394 void build_sy_lfr_rw_masks( void )
1388 {
1395 {
1389 build_sy_lfr_rw_mask( CHANNELF0 );
1396 build_sy_lfr_rw_mask( CHANNELF0 );
1390 build_sy_lfr_rw_mask( CHANNELF1 );
1397 build_sy_lfr_rw_mask( CHANNELF1 );
1391 build_sy_lfr_rw_mask( CHANNELF2 );
1398 build_sy_lfr_rw_mask( CHANNELF2 );
1392 }
1399 }
1393
1400
1394 void merge_fbins_masks( void )
1401 void merge_fbins_masks( void )
1395 {
1402 {
1396 unsigned char k;
1403 unsigned char k;
1397
1404
1398 unsigned char *fbins_f0;
1405 unsigned char *fbins_f0;
1399 unsigned char *fbins_f1;
1406 unsigned char *fbins_f1;
1400 unsigned char *fbins_f2;
1407 unsigned char *fbins_f2;
1401 unsigned char *rw_mask_f0;
1408 unsigned char *rw_mask_f0;
1402 unsigned char *rw_mask_f1;
1409 unsigned char *rw_mask_f1;
1403 unsigned char *rw_mask_f2;
1410 unsigned char *rw_mask_f2;
1404
1411
1405 fbins_f0 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1412 fbins_f0 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1406 fbins_f1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
1413 fbins_f1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
1407 fbins_f2 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
1414 fbins_f2 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
1408 rw_mask_f0 = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1415 rw_mask_f0 = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1409 rw_mask_f1 = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1416 rw_mask_f1 = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1410 rw_mask_f2 = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1417 rw_mask_f2 = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1411
1418
1412 for( k=0; k < BYTES_PER_MASK; k++ )
1419 for( k=0; k < BYTES_PER_MASK; k++ )
1413 {
1420 {
1414 fbins_masks.merged_fbins_mask_f0[k] = fbins_f0[k] & rw_mask_f0[k];
1421 fbins_masks.merged_fbins_mask_f0[k] = fbins_f0[k] & rw_mask_f0[k];
1415 fbins_masks.merged_fbins_mask_f1[k] = fbins_f1[k] & rw_mask_f1[k];
1422 fbins_masks.merged_fbins_mask_f1[k] = fbins_f1[k] & rw_mask_f1[k];
1416 fbins_masks.merged_fbins_mask_f2[k] = fbins_f2[k] & rw_mask_f2[k];
1423 fbins_masks.merged_fbins_mask_f2[k] = fbins_f2[k] & rw_mask_f2[k];
1417 }
1424 }
1418 }
1425 }
1419
1426
1420 //***********
1427 //***********
1421 // FBINS MASK
1428 // FBINS MASK
1422
1429
1423 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
1430 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
1424 {
1431 {
1425 int status;
1432 int status;
1426 unsigned int k;
1433 unsigned int k;
1427 unsigned char *fbins_mask_dump;
1434 unsigned char *fbins_mask_dump;
1428 unsigned char *fbins_mask_TC;
1435 unsigned char *fbins_mask_TC;
1429
1436
1430 status = LFR_SUCCESSFUL;
1437 status = LFR_SUCCESSFUL;
1431
1438
1432 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1439 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1433 fbins_mask_TC = TC->dataAndCRC;
1440 fbins_mask_TC = TC->dataAndCRC;
1434
1441
1435 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1442 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1436 {
1443 {
1437 fbins_mask_dump[k] = fbins_mask_TC[k];
1444 fbins_mask_dump[k] = fbins_mask_TC[k];
1438 }
1445 }
1439
1446
1440 return status;
1447 return status;
1441 }
1448 }
1442
1449
1443 //***************************
1450 //***************************
1444 // TC_LFR_LOAD_PAS_FILTER_PAR
1451 // TC_LFR_LOAD_PAS_FILTER_PAR
1445
1452
1446 int check_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value )
1453 int check_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value )
1447 {
1454 {
1448 float rw_k;
1455 float rw_k;
1449 int ret;
1456 int ret;
1450
1457
1451 ret = LFR_SUCCESSFUL;
1458 ret = LFR_SUCCESSFUL;
1452 rw_k = INIT_FLOAT;
1459 rw_k = INIT_FLOAT;
1453
1460
1454 copyFloatByChar( (unsigned char*) &rw_k, (unsigned char*) &TC->dataAndCRC[ offset ] );
1461 copyFloatByChar( (unsigned char*) &rw_k, (unsigned char*) &TC->dataAndCRC[ offset ] );
1455
1462
1456 *pos = offset;
1463 *pos = offset;
1457 *value = rw_k;
1464 *value = rw_k;
1458
1465
1459 if (rw_k < MIN_SY_LFR_RW_F)
1466 if (rw_k < MIN_SY_LFR_RW_F)
1460 {
1467 {
1461 ret = WRONG_APP_DATA;
1468 ret = WRONG_APP_DATA;
1462 }
1469 }
1463
1470
1464 return ret;
1471 return ret;
1465 }
1472 }
1466
1473
1467 int check_all_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int *pos, float *value )
1474 int check_all_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int *pos, float *value )
1468 {
1475 {
1469 int ret;
1476 int ret;
1470
1477
1471 ret = LFR_SUCCESSFUL;
1478 ret = LFR_SUCCESSFUL;
1472
1479
1473 //****
1480 //****
1474 //****
1481 //****
1475 // RW1
1482 // RW1
1476 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K1, pos, value ); // K1
1483 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K1, pos, value ); // K1
1477 if (ret == LFR_SUCCESSFUL) // K2
1484 if (ret == LFR_SUCCESSFUL) // K2
1478 {
1485 {
1479 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K2, pos, value );
1486 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K2, pos, value );
1480 }
1487 }
1481 if (ret == LFR_SUCCESSFUL) // K3
1488 if (ret == LFR_SUCCESSFUL) // K3
1482 {
1489 {
1483 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K3, pos, value );
1490 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K3, pos, value );
1484 }
1491 }
1485 if (ret == LFR_SUCCESSFUL) // K4
1492 if (ret == LFR_SUCCESSFUL) // K4
1486 {
1493 {
1487 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K4, pos, value );
1494 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K4, pos, value );
1488 }
1495 }
1489
1496
1490 //****
1497 //****
1491 //****
1498 //****
1492 // RW2
1499 // RW2
1493 if (ret == LFR_SUCCESSFUL) // K1
1500 if (ret == LFR_SUCCESSFUL) // K1
1494 {
1501 {
1495 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K1, pos, value );
1502 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K1, pos, value );
1496 }
1503 }
1497 if (ret == LFR_SUCCESSFUL) // K2
1504 if (ret == LFR_SUCCESSFUL) // K2
1498 {
1505 {
1499 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K2, pos, value );
1506 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K2, pos, value );
1500 }
1507 }
1501 if (ret == LFR_SUCCESSFUL) // K3
1508 if (ret == LFR_SUCCESSFUL) // K3
1502 {
1509 {
1503 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K3, pos, value );
1510 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K3, pos, value );
1504 }
1511 }
1505 if (ret == LFR_SUCCESSFUL) // K4
1512 if (ret == LFR_SUCCESSFUL) // K4
1506 {
1513 {
1507 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K4, pos, value );
1514 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K4, pos, value );
1508 }
1515 }
1509
1516
1510 //****
1517 //****
1511 //****
1518 //****
1512 // RW3
1519 // RW3
1513 if (ret == LFR_SUCCESSFUL) // K1
1520 if (ret == LFR_SUCCESSFUL) // K1
1514 {
1521 {
1515 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K1, pos, value );
1522 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K1, pos, value );
1516 }
1523 }
1517 if (ret == LFR_SUCCESSFUL) // K2
1524 if (ret == LFR_SUCCESSFUL) // K2
1518 {
1525 {
1519 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K2, pos, value );
1526 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K2, pos, value );
1520 }
1527 }
1521 if (ret == LFR_SUCCESSFUL) // K3
1528 if (ret == LFR_SUCCESSFUL) // K3
1522 {
1529 {
1523 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K3, pos, value );
1530 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K3, pos, value );
1524 }
1531 }
1525 if (ret == LFR_SUCCESSFUL) // K4
1532 if (ret == LFR_SUCCESSFUL) // K4
1526 {
1533 {
1527 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K4, pos, value );
1534 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K4, pos, value );
1528 }
1535 }
1529
1536
1530 //****
1537 //****
1531 //****
1538 //****
1532 // RW4
1539 // RW4
1533 if (ret == LFR_SUCCESSFUL) // K1
1540 if (ret == LFR_SUCCESSFUL) // K1
1534 {
1541 {
1535 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K1, pos, value );
1542 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K1, pos, value );
1536 }
1543 }
1537 if (ret == LFR_SUCCESSFUL) // K2
1544 if (ret == LFR_SUCCESSFUL) // K2
1538 {
1545 {
1539 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K2, pos, value );
1546 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K2, pos, value );
1540 }
1547 }
1541 if (ret == LFR_SUCCESSFUL) // K3
1548 if (ret == LFR_SUCCESSFUL) // K3
1542 {
1549 {
1543 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K3, pos, value );
1550 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K3, pos, value );
1544 }
1551 }
1545 if (ret == LFR_SUCCESSFUL) // K4
1552 if (ret == LFR_SUCCESSFUL) // K4
1546 {
1553 {
1547 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K4, pos, value );
1554 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K4, pos, value );
1548 }
1555 }
1549
1556
1550 return ret;
1557 return ret;
1551 }
1558 }
1552
1559
1553 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
1560 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
1554 {
1561 {
1555 int flag;
1562 int flag;
1556 rtems_status_code status;
1563 rtems_status_code status;
1557
1564
1558 unsigned char sy_lfr_pas_filter_enabled;
1565 unsigned char sy_lfr_pas_filter_enabled;
1559 unsigned char sy_lfr_pas_filter_modulus;
1566 unsigned char sy_lfr_pas_filter_modulus;
1560 float sy_lfr_pas_filter_tbad;
1567 float sy_lfr_pas_filter_tbad;
1561 unsigned char sy_lfr_pas_filter_offset;
1568 unsigned char sy_lfr_pas_filter_offset;
1562 float sy_lfr_pas_filter_shift;
1569 float sy_lfr_pas_filter_shift;
1563 float sy_lfr_sc_rw_delta_f;
1570 float sy_lfr_sc_rw_delta_f;
1564 char *parPtr;
1571 char *parPtr;
1565 int datafield_pos;
1572 int datafield_pos;
1566 float rw_k;
1573 float rw_k;
1567
1574
1568 flag = LFR_SUCCESSFUL;
1575 flag = LFR_SUCCESSFUL;
1569 sy_lfr_pas_filter_tbad = INIT_FLOAT;
1576 sy_lfr_pas_filter_tbad = INIT_FLOAT;
1570 sy_lfr_pas_filter_shift = INIT_FLOAT;
1577 sy_lfr_pas_filter_shift = INIT_FLOAT;
1571 sy_lfr_sc_rw_delta_f = INIT_FLOAT;
1578 sy_lfr_sc_rw_delta_f = INIT_FLOAT;
1572 parPtr = NULL;
1579 parPtr = NULL;
1573 datafield_pos = INIT_INT;
1580 datafield_pos = INIT_INT;
1574 rw_k = INIT_FLOAT;
1581 rw_k = INIT_FLOAT;
1575
1582
1576 //***************
1583 //***************
1577 // get parameters
1584 // get parameters
1578 sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & BIT_PAS_FILTER_ENABLED; // [0000 0001]
1585 sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & BIT_PAS_FILTER_ENABLED; // [0000 0001]
1579 sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
1586 sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
1580 copyFloatByChar(
1587 copyFloatByChar(
1581 (unsigned char*) &sy_lfr_pas_filter_tbad,
1588 (unsigned char*) &sy_lfr_pas_filter_tbad,
1582 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD ]
1589 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD ]
1583 );
1590 );
1584 sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
1591 sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
1585 copyFloatByChar(
1592 copyFloatByChar(
1586 (unsigned char*) &sy_lfr_pas_filter_shift,
1593 (unsigned char*) &sy_lfr_pas_filter_shift,
1587 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT ]
1594 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT ]
1588 );
1595 );
1589 copyFloatByChar(
1596 copyFloatByChar(
1590 (unsigned char*) &sy_lfr_sc_rw_delta_f,
1597 (unsigned char*) &sy_lfr_sc_rw_delta_f,
1591 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F ]
1598 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F ]
1592 );
1599 );
1593
1600
1594 //******************
1601 //******************
1595 // CHECK CONSISTENCY
1602 // CHECK CONSISTENCY
1596
1603
1597 //**************************
1604 //**************************
1598 // sy_lfr_pas_filter_enabled
1605 // sy_lfr_pas_filter_enabled
1599 // nothing to check, value is 0 or 1
1606 // nothing to check, value is 0 or 1
1600
1607
1601 //**************************
1608 //**************************
1602 // sy_lfr_pas_filter_modulus
1609 // sy_lfr_pas_filter_modulus
1603 if ( (sy_lfr_pas_filter_modulus < MIN_PAS_FILTER_MODULUS) || (sy_lfr_pas_filter_modulus > MAX_PAS_FILTER_MODULUS) )
1610 if ( (sy_lfr_pas_filter_modulus < MIN_PAS_FILTER_MODULUS) || (sy_lfr_pas_filter_modulus > MAX_PAS_FILTER_MODULUS) )
1604 {
1611 {
1605 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1612 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1606 flag = WRONG_APP_DATA;
1613 flag = WRONG_APP_DATA;
1607 }
1614 }
1608
1615
1609 //***********************
1616 //***********************
1610 // sy_lfr_pas_filter_tbad
1617 // sy_lfr_pas_filter_tbad
1611 if (flag == LFR_SUCCESSFUL)
1618 if (flag == LFR_SUCCESSFUL)
1612 {
1619 {
1613 if ( (sy_lfr_pas_filter_tbad < MIN_PAS_FILTER_TBAD) || (sy_lfr_pas_filter_tbad > MAX_PAS_FILTER_TBAD) )
1620 if ( (sy_lfr_pas_filter_tbad < MIN_PAS_FILTER_TBAD) || (sy_lfr_pas_filter_tbad > MAX_PAS_FILTER_TBAD) )
1614 {
1621 {
1615 parPtr = (char*) &sy_lfr_pas_filter_tbad;
1622 parPtr = (char*) &sy_lfr_pas_filter_tbad;
1616 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1623 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1617 flag = WRONG_APP_DATA;
1624 flag = WRONG_APP_DATA;
1618 }
1625 }
1619 }
1626 }
1620
1627
1621 //*************************
1628 //*************************
1622 // sy_lfr_pas_filter_offset
1629 // sy_lfr_pas_filter_offset
1623 if (flag == LFR_SUCCESSFUL)
1630 if (flag == LFR_SUCCESSFUL)
1624 {
1631 {
1625 if ( (sy_lfr_pas_filter_offset < MIN_PAS_FILTER_OFFSET) || (sy_lfr_pas_filter_offset > MAX_PAS_FILTER_OFFSET) )
1632 if ( (sy_lfr_pas_filter_offset < MIN_PAS_FILTER_OFFSET) || (sy_lfr_pas_filter_offset > MAX_PAS_FILTER_OFFSET) )
1626 {
1633 {
1627 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET + DATAFIELD_OFFSET, sy_lfr_pas_filter_offset );
1634 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET + DATAFIELD_OFFSET, sy_lfr_pas_filter_offset );
1628 flag = WRONG_APP_DATA;
1635 flag = WRONG_APP_DATA;
1629 }
1636 }
1630 }
1637 }
1631
1638
1632 //************************
1639 //************************
1633 // sy_lfr_pas_filter_shift
1640 // sy_lfr_pas_filter_shift
1634 if (flag == LFR_SUCCESSFUL)
1641 if (flag == LFR_SUCCESSFUL)
1635 {
1642 {
1636 if ( (sy_lfr_pas_filter_shift < MIN_PAS_FILTER_SHIFT) || (sy_lfr_pas_filter_shift > MAX_PAS_FILTER_SHIFT) )
1643 if ( (sy_lfr_pas_filter_shift < MIN_PAS_FILTER_SHIFT) || (sy_lfr_pas_filter_shift > MAX_PAS_FILTER_SHIFT) )
1637 {
1644 {
1638 parPtr = (char*) &sy_lfr_pas_filter_shift;
1645 parPtr = (char*) &sy_lfr_pas_filter_shift;
1639 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1646 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1640 flag = WRONG_APP_DATA;
1647 flag = WRONG_APP_DATA;
1641 }
1648 }
1642 }
1649 }
1643
1650
1644 //*************************************
1651 //*************************************
1645 // check global coherency of the values
1652 // check global coherency of the values
1646 if (flag == LFR_SUCCESSFUL)
1653 if (flag == LFR_SUCCESSFUL)
1647 {
1654 {
1648 if ( (sy_lfr_pas_filter_tbad + sy_lfr_pas_filter_offset + sy_lfr_pas_filter_shift) > sy_lfr_pas_filter_modulus )
1655 if ( (sy_lfr_pas_filter_tbad + sy_lfr_pas_filter_offset + sy_lfr_pas_filter_shift) > sy_lfr_pas_filter_modulus )
1649 {
1656 {
1650 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1657 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1651 flag = WRONG_APP_DATA;
1658 flag = WRONG_APP_DATA;
1652 }
1659 }
1653 }
1660 }
1654
1661
1655 //*********************
1662 //*********************
1656 // sy_lfr_sc_rw_delta_f
1663 // sy_lfr_sc_rw_delta_f
1657 if (flag == LFR_SUCCESSFUL)
1664 if (flag == LFR_SUCCESSFUL)
1658 {
1665 {
1659 if ( sy_lfr_sc_rw_delta_f < MIN_SY_LFR_SC_RW_DELTA_F )
1666 if ( sy_lfr_sc_rw_delta_f < MIN_SY_LFR_SC_RW_DELTA_F )
1660 {
1667 {
1661 parPtr = (char*) &sy_lfr_pas_filter_shift;
1668 parPtr = (char*) &sy_lfr_pas_filter_shift;
1662 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + DATAFIELD_OFFSET, sy_lfr_sc_rw_delta_f );
1669 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + DATAFIELD_OFFSET, sy_lfr_sc_rw_delta_f );
1663 flag = WRONG_APP_DATA;
1670 flag = WRONG_APP_DATA;
1664 }
1671 }
1665 }
1672 }
1666
1673
1667 //************
1674 //************
1668 // sy_lfr_rw_k
1675 // sy_lfr_rw_k
1669 if (flag == LFR_SUCCESSFUL)
1676 if (flag == LFR_SUCCESSFUL)
1670 {
1677 {
1671 flag = check_all_sy_lfr_rw_k( TC, &datafield_pos, &rw_k );
1678 flag = check_all_sy_lfr_rw_k( TC, &datafield_pos, &rw_k );
1672 if (flag != LFR_SUCCESSFUL)
1679 if (flag != LFR_SUCCESSFUL)
1673 {
1680 {
1674 parPtr = (char*) &sy_lfr_pas_filter_shift;
1681 parPtr = (char*) &sy_lfr_pas_filter_shift;
1675 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, datafield_pos + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1682 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, datafield_pos + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1676 }
1683 }
1677 }
1684 }
1678
1685
1679 return flag;
1686 return flag;
1680 }
1687 }
1681
1688
1682 //**************
1689 //**************
1683 // KCOEFFICIENTS
1690 // KCOEFFICIENTS
1684 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC,rtems_id queue_id )
1691 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC,rtems_id queue_id )
1685 {
1692 {
1686 unsigned int kcoeff;
1693 unsigned int kcoeff;
1687 unsigned short sy_lfr_kcoeff_frequency;
1694 unsigned short sy_lfr_kcoeff_frequency;
1688 unsigned short bin;
1695 unsigned short bin;
1689 float *kcoeffPtr_norm;
1696 float *kcoeffPtr_norm;
1690 float *kcoeffPtr_sbm;
1697 float *kcoeffPtr_sbm;
1691 int status;
1698 int status;
1692 unsigned char *kcoeffLoadPtr;
1699 unsigned char *kcoeffLoadPtr;
1693 unsigned char *kcoeffNormPtr;
1700 unsigned char *kcoeffNormPtr;
1694 unsigned char *kcoeffSbmPtr_a;
1701 unsigned char *kcoeffSbmPtr_a;
1695 unsigned char *kcoeffSbmPtr_b;
1702 unsigned char *kcoeffSbmPtr_b;
1696
1703
1697 sy_lfr_kcoeff_frequency = 0;
1704 sy_lfr_kcoeff_frequency = 0;
1698 bin = 0;
1705 bin = 0;
1699 kcoeffPtr_norm = NULL;
1706 kcoeffPtr_norm = NULL;
1700 kcoeffPtr_sbm = NULL;
1707 kcoeffPtr_sbm = NULL;
1701 status = LFR_SUCCESSFUL;
1708 status = LFR_SUCCESSFUL;
1702
1709
1703 // copy the value of the frequency byte by byte DO NOT USE A SHORT* POINTER
1710 // copy the value of the frequency byte by byte DO NOT USE A SHORT* POINTER
1704 copyInt16ByChar( (unsigned char*) &sy_lfr_kcoeff_frequency, &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY] );
1711 copyInt16ByChar( (unsigned char*) &sy_lfr_kcoeff_frequency, &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY] );
1705
1712
1706
1713
1707 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
1714 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
1708 {
1715 {
1709 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
1716 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
1710 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + DATAFIELD_OFFSET + 1,
1717 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + DATAFIELD_OFFSET + 1,
1711 TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 1] ); // +1 to get the LSB instead of the MSB
1718 TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 1] ); // +1 to get the LSB instead of the MSB
1712 status = LFR_DEFAULT;
1719 status = LFR_DEFAULT;
1713 }
1720 }
1714 else
1721 else
1715 {
1722 {
1716 if ( ( sy_lfr_kcoeff_frequency >= 0 )
1723 if ( ( sy_lfr_kcoeff_frequency >= 0 )
1717 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
1724 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
1718 {
1725 {
1719 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
1726 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
1720 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
1727 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
1721 bin = sy_lfr_kcoeff_frequency;
1728 bin = sy_lfr_kcoeff_frequency;
1722 }
1729 }
1723 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
1730 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
1724 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
1731 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
1725 {
1732 {
1726 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
1733 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
1727 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
1734 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
1728 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
1735 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
1729 }
1736 }
1730 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
1737 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
1731 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
1738 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
1732 {
1739 {
1733 kcoeffPtr_norm = k_coeff_intercalib_f2;
1740 kcoeffPtr_norm = k_coeff_intercalib_f2;
1734 kcoeffPtr_sbm = NULL;
1741 kcoeffPtr_sbm = NULL;
1735 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
1742 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
1736 }
1743 }
1737 }
1744 }
1738
1745
1739 if (kcoeffPtr_norm != NULL ) // update K coefficient for NORMAL data products
1746 if (kcoeffPtr_norm != NULL ) // update K coefficient for NORMAL data products
1740 {
1747 {
1741 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1748 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1742 {
1749 {
1743 // destination
1750 // destination
1744 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + kcoeff ];
1751 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + kcoeff ];
1745 // source
1752 // source
1746 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1753 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1747 // copy source to destination
1754 // copy source to destination
1748 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
1755 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
1749 }
1756 }
1750 }
1757 }
1751
1758
1752 if (kcoeffPtr_sbm != NULL ) // update K coefficient for SBM data products
1759 if (kcoeffPtr_sbm != NULL ) // update K coefficient for SBM data products
1753 {
1760 {
1754 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1761 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1755 {
1762 {
1756 // destination
1763 // destination
1757 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ];
1764 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ];
1758 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ (((bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_KCOEFF_PER_NORM_KCOEFF) + 1 ];
1765 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ (((bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_KCOEFF_PER_NORM_KCOEFF) + 1 ];
1759 // source
1766 // source
1760 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1767 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1761 // copy source to destination
1768 // copy source to destination
1762 copyFloatByChar( kcoeffSbmPtr_a, kcoeffLoadPtr );
1769 copyFloatByChar( kcoeffSbmPtr_a, kcoeffLoadPtr );
1763 copyFloatByChar( kcoeffSbmPtr_b, kcoeffLoadPtr );
1770 copyFloatByChar( kcoeffSbmPtr_b, kcoeffLoadPtr );
1764 }
1771 }
1765 }
1772 }
1766
1773
1767 // print_k_coeff();
1774 // print_k_coeff();
1768
1775
1769 return status;
1776 return status;
1770 }
1777 }
1771
1778
1772 void copyFloatByChar( unsigned char *destination, unsigned char *source )
1779 void copyFloatByChar( unsigned char *destination, unsigned char *source )
1773 {
1780 {
1774 destination[BYTE_0] = source[BYTE_0];
1781 destination[BYTE_0] = source[BYTE_0];
1775 destination[BYTE_1] = source[BYTE_1];
1782 destination[BYTE_1] = source[BYTE_1];
1776 destination[BYTE_2] = source[BYTE_2];
1783 destination[BYTE_2] = source[BYTE_2];
1777 destination[BYTE_3] = source[BYTE_3];
1784 destination[BYTE_3] = source[BYTE_3];
1778 }
1785 }
1779
1786
1780 void copyInt32ByChar( unsigned char *destination, unsigned char *source )
1787 void copyInt32ByChar( unsigned char *destination, unsigned char *source )
1781 {
1788 {
1782 destination[BYTE_0] = source[BYTE_0];
1789 destination[BYTE_0] = source[BYTE_0];
1783 destination[BYTE_1] = source[BYTE_1];
1790 destination[BYTE_1] = source[BYTE_1];
1784 destination[BYTE_2] = source[BYTE_2];
1791 destination[BYTE_2] = source[BYTE_2];
1785 destination[BYTE_3] = source[BYTE_3];
1792 destination[BYTE_3] = source[BYTE_3];
1786 }
1793 }
1787
1794
1788 void copyInt16ByChar( unsigned char *destination, unsigned char *source )
1795 void copyInt16ByChar( unsigned char *destination, unsigned char *source )
1789 {
1796 {
1790 destination[BYTE_0] = source[BYTE_0];
1797 destination[BYTE_0] = source[BYTE_0];
1791 destination[BYTE_1] = source[BYTE_1];
1798 destination[BYTE_1] = source[BYTE_1];
1792 }
1799 }
1793
1800
1794 void floatToChar( float value, unsigned char* ptr)
1801 void floatToChar( float value, unsigned char* ptr)
1795 {
1802 {
1796 unsigned char* valuePtr;
1803 unsigned char* valuePtr;
1797
1804
1798 valuePtr = (unsigned char*) &value;
1805 valuePtr = (unsigned char*) &value;
1799
1806
1800 ptr[BYTE_0] = valuePtr[BYTE_0];
1807 ptr[BYTE_0] = valuePtr[BYTE_0];
1801 ptr[BYTE_1] = valuePtr[BYTE_1];
1808 ptr[BYTE_1] = valuePtr[BYTE_1];
1802 ptr[BYTE_2] = valuePtr[BYTE_2];
1809 ptr[BYTE_2] = valuePtr[BYTE_2];
1803 ptr[BYTE_3] = valuePtr[BYTE_3];
1810 ptr[BYTE_3] = valuePtr[BYTE_3];
1804 }
1811 }
1805
1812
1806 //**********
1813 //**********
1807 // init dump
1814 // init dump
1808
1815
1809 void init_parameter_dump( void )
1816 void init_parameter_dump( void )
1810 {
1817 {
1811 /** This function initialize the parameter_dump_packet global variable with default values.
1818 /** This function initialize the parameter_dump_packet global variable with default values.
1812 *
1819 *
1813 */
1820 */
1814
1821
1815 unsigned int k;
1822 unsigned int k;
1816
1823
1817 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
1824 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
1818 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
1825 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
1819 parameter_dump_packet.reserved = CCSDS_RESERVED;
1826 parameter_dump_packet.reserved = CCSDS_RESERVED;
1820 parameter_dump_packet.userApplication = CCSDS_USER_APP;
1827 parameter_dump_packet.userApplication = CCSDS_USER_APP;
1821 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1828 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1822 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1829 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1823 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1830 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1824 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1831 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1825 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> SHIFT_1_BYTE);
1832 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> SHIFT_1_BYTE);
1826 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
1833 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
1827 // DATA FIELD HEADER
1834 // DATA FIELD HEADER
1828 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1835 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1829 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
1836 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
1830 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
1837 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
1831 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
1838 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
1832 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
1839 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
1833 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
1840 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
1834 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
1841 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
1835 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
1842 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
1836 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
1843 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
1837 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
1844 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
1838 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
1845 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
1839
1846
1840 //******************
1847 //******************
1841 // COMMON PARAMETERS
1848 // COMMON PARAMETERS
1842 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
1849 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
1843 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
1850 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
1844
1851
1845 //******************
1852 //******************
1846 // NORMAL PARAMETERS
1853 // NORMAL PARAMETERS
1847 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> SHIFT_1_BYTE);
1854 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> SHIFT_1_BYTE);
1848 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1855 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1849 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> SHIFT_1_BYTE);
1856 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> SHIFT_1_BYTE);
1850 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1857 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1851 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> SHIFT_1_BYTE);
1858 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> SHIFT_1_BYTE);
1852 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1859 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1853 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1860 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1854 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1861 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1855 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1862 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1856
1863
1857 //*****************
1864 //*****************
1858 // BURST PARAMETERS
1865 // BURST PARAMETERS
1859 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1866 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1860 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1867 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1861
1868
1862 //****************
1869 //****************
1863 // SBM1 PARAMETERS
1870 // SBM1 PARAMETERS
1864 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
1871 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
1865 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1872 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1866
1873
1867 //****************
1874 //****************
1868 // SBM2 PARAMETERS
1875 // SBM2 PARAMETERS
1869 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1876 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1870 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1877 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1871
1878
1872 //************
1879 //************
1873 // FBINS MASKS
1880 // FBINS MASKS
1874 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1881 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1875 {
1882 {
1876 parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = INT8_ALL_F;
1883 parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = INT8_ALL_F;
1877 }
1884 }
1878
1885
1879 // PAS FILTER PARAMETERS
1886 // PAS FILTER PARAMETERS
1880 parameter_dump_packet.pa_rpw_spare8_2 = INIT_CHAR;
1887 parameter_dump_packet.pa_rpw_spare8_2 = INIT_CHAR;
1881 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = INIT_CHAR;
1888 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = INIT_CHAR;
1882 parameter_dump_packet.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
1889 parameter_dump_packet.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
1883 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_TBAD, parameter_dump_packet.sy_lfr_pas_filter_tbad );
1890 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_TBAD, parameter_dump_packet.sy_lfr_pas_filter_tbad );
1884 parameter_dump_packet.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
1891 parameter_dump_packet.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
1885 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift );
1892 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift );
1886 floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f );
1893 floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f );
1887
1894
1888 // RW1_K
1895 // RW1_K
1889 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw1_k1);
1896 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw1_k1);
1890 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw1_k2);
1897 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw1_k2);
1891 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw1_k3);
1898 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw1_k3);
1892 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw1_k4);
1899 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw1_k4);
1893 // RW2_K
1900 // RW2_K
1894 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw2_k1);
1901 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw2_k1);
1895 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw2_k2);
1902 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw2_k2);
1896 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw2_k3);
1903 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw2_k3);
1897 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw2_k4);
1904 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw2_k4);
1898 // RW3_K
1905 // RW3_K
1899 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw3_k1);
1906 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw3_k1);
1900 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw3_k2);
1907 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw3_k2);
1901 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw3_k3);
1908 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw3_k3);
1902 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw3_k4);
1909 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw3_k4);
1903 // RW4_K
1910 // RW4_K
1904 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw4_k1);
1911 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw4_k1);
1905 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw4_k2);
1912 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw4_k2);
1906 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw4_k3);
1913 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw4_k3);
1907 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw4_k4);
1914 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw4_k4);
1908
1915
1909 // LFR_RW_MASK
1916 // LFR_RW_MASK
1910 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1917 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1911 {
1918 {
1912 parameter_dump_packet.sy_lfr_rw_mask_f0_word1[k] = INT8_ALL_F;
1919 parameter_dump_packet.sy_lfr_rw_mask_f0_word1[k] = INT8_ALL_F;
1913 }
1920 }
1914
1921
1915 // once the reaction wheels masks have been initialized, they have to be merged with the fbins masks
1922 // once the reaction wheels masks have been initialized, they have to be merged with the fbins masks
1916 merge_fbins_masks();
1923 merge_fbins_masks();
1917 }
1924 }
1918
1925
1919 void init_kcoefficients_dump( void )
1926 void init_kcoefficients_dump( void )
1920 {
1927 {
1921 init_kcoefficients_dump_packet( &kcoefficients_dump_1, PKTNR_1, KCOEFF_BLK_NR_PKT1 );
1928 init_kcoefficients_dump_packet( &kcoefficients_dump_1, PKTNR_1, KCOEFF_BLK_NR_PKT1 );
1922 init_kcoefficients_dump_packet( &kcoefficients_dump_2, PKTNR_2, KCOEFF_BLK_NR_PKT2 );
1929 init_kcoefficients_dump_packet( &kcoefficients_dump_2, PKTNR_2, KCOEFF_BLK_NR_PKT2 );
1923
1930
1924 kcoefficient_node_1.previous = NULL;
1931 kcoefficient_node_1.previous = NULL;
1925 kcoefficient_node_1.next = NULL;
1932 kcoefficient_node_1.next = NULL;
1926 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1933 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1927 kcoefficient_node_1.coarseTime = INIT_CHAR;
1934 kcoefficient_node_1.coarseTime = INIT_CHAR;
1928 kcoefficient_node_1.fineTime = INIT_CHAR;
1935 kcoefficient_node_1.fineTime = INIT_CHAR;
1929 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1936 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1930 kcoefficient_node_1.status = INIT_CHAR;
1937 kcoefficient_node_1.status = INIT_CHAR;
1931
1938
1932 kcoefficient_node_2.previous = NULL;
1939 kcoefficient_node_2.previous = NULL;
1933 kcoefficient_node_2.next = NULL;
1940 kcoefficient_node_2.next = NULL;
1934 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1941 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1935 kcoefficient_node_2.coarseTime = INIT_CHAR;
1942 kcoefficient_node_2.coarseTime = INIT_CHAR;
1936 kcoefficient_node_2.fineTime = INIT_CHAR;
1943 kcoefficient_node_2.fineTime = INIT_CHAR;
1937 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1944 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1938 kcoefficient_node_2.status = INIT_CHAR;
1945 kcoefficient_node_2.status = INIT_CHAR;
1939 }
1946 }
1940
1947
1941 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1948 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1942 {
1949 {
1943 unsigned int k;
1950 unsigned int k;
1944 unsigned int packetLength;
1951 unsigned int packetLength;
1945
1952
1946 packetLength =
1953 packetLength =
1947 ((blk_nr * KCOEFF_BLK_SIZE) + BYTE_POS_KCOEFFICIENTS_PARAMETES) - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1954 ((blk_nr * KCOEFF_BLK_SIZE) + BYTE_POS_KCOEFFICIENTS_PARAMETES) - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1948
1955
1949 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1956 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1950 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1957 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1951 kcoefficients_dump->reserved = CCSDS_RESERVED;
1958 kcoefficients_dump->reserved = CCSDS_RESERVED;
1952 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1959 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1953 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1960 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1954 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1961 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1955 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1962 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1956 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1963 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1957 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
1964 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
1958 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1965 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1959 // DATA FIELD HEADER
1966 // DATA FIELD HEADER
1960 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1967 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1961 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1968 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1962 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1969 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1963 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1970 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1964 kcoefficients_dump->time[BYTE_0] = INIT_CHAR;
1971 kcoefficients_dump->time[BYTE_0] = INIT_CHAR;
1965 kcoefficients_dump->time[BYTE_1] = INIT_CHAR;
1972 kcoefficients_dump->time[BYTE_1] = INIT_CHAR;
1966 kcoefficients_dump->time[BYTE_2] = INIT_CHAR;
1973 kcoefficients_dump->time[BYTE_2] = INIT_CHAR;
1967 kcoefficients_dump->time[BYTE_3] = INIT_CHAR;
1974 kcoefficients_dump->time[BYTE_3] = INIT_CHAR;
1968 kcoefficients_dump->time[BYTE_4] = INIT_CHAR;
1975 kcoefficients_dump->time[BYTE_4] = INIT_CHAR;
1969 kcoefficients_dump->time[BYTE_5] = INIT_CHAR;
1976 kcoefficients_dump->time[BYTE_5] = INIT_CHAR;
1970 kcoefficients_dump->sid = SID_K_DUMP;
1977 kcoefficients_dump->sid = SID_K_DUMP;
1971
1978
1972 kcoefficients_dump->pkt_cnt = KCOEFF_PKTCNT;
1979 kcoefficients_dump->pkt_cnt = KCOEFF_PKTCNT;
1973 kcoefficients_dump->pkt_nr = PKTNR_1;
1980 kcoefficients_dump->pkt_nr = PKTNR_1;
1974 kcoefficients_dump->blk_nr = blk_nr;
1981 kcoefficients_dump->blk_nr = blk_nr;
1975
1982
1976 //******************
1983 //******************
1977 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1984 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1978 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1985 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1979 for (k=0; k<(KCOEFF_BLK_NR_PKT1 * KCOEFF_BLK_SIZE); k++)
1986 for (k=0; k<(KCOEFF_BLK_NR_PKT1 * KCOEFF_BLK_SIZE); k++)
1980 {
1987 {
1981 kcoefficients_dump->kcoeff_blks[k] = INIT_CHAR;
1988 kcoefficients_dump->kcoeff_blks[k] = INIT_CHAR;
1982 }
1989 }
1983 }
1990 }
1984
1991
1985 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id )
1992 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id )
1986 {
1993 {
1987 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
1994 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
1988 *
1995 *
1989 * @param packet_sequence_control points to the packet sequence control which will be incremented
1996 * @param packet_sequence_control points to the packet sequence control which will be incremented
1990 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
1997 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
1991 *
1998 *
1992 * If the destination ID is not known, a dedicated counter is incremented.
1999 * If the destination ID is not known, a dedicated counter is incremented.
1993 *
2000 *
1994 */
2001 */
1995
2002
1996 unsigned short sequence_cnt;
2003 unsigned short sequence_cnt;
1997 unsigned short segmentation_grouping_flag;
2004 unsigned short segmentation_grouping_flag;
1998 unsigned short new_packet_sequence_control;
2005 unsigned short new_packet_sequence_control;
1999 unsigned char i;
2006 unsigned char i;
2000
2007
2001 switch (destination_id)
2008 switch (destination_id)
2002 {
2009 {
2003 case SID_TC_GROUND:
2010 case SID_TC_GROUND:
2004 i = GROUND;
2011 i = GROUND;
2005 break;
2012 break;
2006 case SID_TC_MISSION_TIMELINE:
2013 case SID_TC_MISSION_TIMELINE:
2007 i = MISSION_TIMELINE;
2014 i = MISSION_TIMELINE;
2008 break;
2015 break;
2009 case SID_TC_TC_SEQUENCES:
2016 case SID_TC_TC_SEQUENCES:
2010 i = TC_SEQUENCES;
2017 i = TC_SEQUENCES;
2011 break;
2018 break;
2012 case SID_TC_RECOVERY_ACTION_CMD:
2019 case SID_TC_RECOVERY_ACTION_CMD:
2013 i = RECOVERY_ACTION_CMD;
2020 i = RECOVERY_ACTION_CMD;
2014 break;
2021 break;
2015 case SID_TC_BACKUP_MISSION_TIMELINE:
2022 case SID_TC_BACKUP_MISSION_TIMELINE:
2016 i = BACKUP_MISSION_TIMELINE;
2023 i = BACKUP_MISSION_TIMELINE;
2017 break;
2024 break;
2018 case SID_TC_DIRECT_CMD:
2025 case SID_TC_DIRECT_CMD:
2019 i = DIRECT_CMD;
2026 i = DIRECT_CMD;
2020 break;
2027 break;
2021 case SID_TC_SPARE_GRD_SRC1:
2028 case SID_TC_SPARE_GRD_SRC1:
2022 i = SPARE_GRD_SRC1;
2029 i = SPARE_GRD_SRC1;
2023 break;
2030 break;
2024 case SID_TC_SPARE_GRD_SRC2:
2031 case SID_TC_SPARE_GRD_SRC2:
2025 i = SPARE_GRD_SRC2;
2032 i = SPARE_GRD_SRC2;
2026 break;
2033 break;
2027 case SID_TC_OBCP:
2034 case SID_TC_OBCP:
2028 i = OBCP;
2035 i = OBCP;
2029 break;
2036 break;
2030 case SID_TC_SYSTEM_CONTROL:
2037 case SID_TC_SYSTEM_CONTROL:
2031 i = SYSTEM_CONTROL;
2038 i = SYSTEM_CONTROL;
2032 break;
2039 break;
2033 case SID_TC_AOCS:
2040 case SID_TC_AOCS:
2034 i = AOCS;
2041 i = AOCS;
2035 break;
2042 break;
2036 case SID_TC_RPW_INTERNAL:
2043 case SID_TC_RPW_INTERNAL:
2037 i = RPW_INTERNAL;
2044 i = RPW_INTERNAL;
2038 break;
2045 break;
2039 default:
2046 default:
2040 i = GROUND;
2047 i = GROUND;
2041 break;
2048 break;
2042 }
2049 }
2043
2050
2044 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
2051 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
2045 sequence_cnt = sequenceCounters_TM_DUMP[ i ] & SEQ_CNT_MASK;
2052 sequence_cnt = sequenceCounters_TM_DUMP[ i ] & SEQ_CNT_MASK;
2046
2053
2047 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
2054 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
2048
2055
2049 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> SHIFT_1_BYTE);
2056 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> SHIFT_1_BYTE);
2050 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
2057 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
2051
2058
2052 // increment the sequence counter
2059 // increment the sequence counter
2053 if ( sequenceCounters_TM_DUMP[ i ] < SEQ_CNT_MAX )
2060 if ( sequenceCounters_TM_DUMP[ i ] < SEQ_CNT_MAX )
2054 {
2061 {
2055 sequenceCounters_TM_DUMP[ i ] = sequenceCounters_TM_DUMP[ i ] + 1;
2062 sequenceCounters_TM_DUMP[ i ] = sequenceCounters_TM_DUMP[ i ] + 1;
2056 }
2063 }
2057 else
2064 else
2058 {
2065 {
2059 sequenceCounters_TM_DUMP[ i ] = 0;
2066 sequenceCounters_TM_DUMP[ i ] = 0;
2060 }
2067 }
2061 }
2068 }
General Comments 0
You need to be logged in to leave comments. Login now