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

Fork of HG_REPOSITORIES/LPP/INSTRUMENTATION/SOLO_LFR/DEV_PLE

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

Removed all remaining unused macros and fixed bug...
jeandet -
r385:bd1252670981 3.2.0.20 No PWD scrub with... draft
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,2 +1,2
1 1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
2 0adeb6c86feb96a126ce48641604949b87c70481 header/lfr_common_headers
2 042275d1388a0f360073a0d85bf50d128f4b8cfc header/lfr_common_headers
Show file before | Show file after | Hide comments
@@ -1,117 +1,120
1 1 cmake_minimum_required (VERSION 2.6)
2 2 project (fsw)
3 3
4 4 include(sparc-rtems)
5 5 include(cppcheck)
6 6
7 7 include_directories("../header"
8 8 "../header/lfr_common_headers"
9 9 "../header/processing"
10 10 "../LFR_basic-parameters"
11 11 "../src")
12 12
13 13 set(SOURCES wf_handler.c
14 14 tc_handler.c
15 15 fsw_misc.c
16 16 fsw_init.c
17 17 fsw_globals.c
18 18 fsw_spacewire.c
19 19 tc_load_dump_parameters.c
20 20 tm_lfr_tc_exe.c
21 21 tc_acceptance.c
22 22 processing/fsw_processing.c
23 23 processing/avf0_prc0.c
24 24 processing/avf1_prc1.c
25 25 processing/avf2_prc2.c
26 26 lfr_cpu_usage_report.c
27 27 ${LFR_BP_SRC}
28 28 ../header/wf_handler.h
29 29 ../header/tc_handler.h
30 30 ../header/grlib_regs.h
31 31 ../header/fsw_misc.h
32 32 ../header/fsw_init.h
33 33 ../header/fsw_spacewire.h
34 34 ../header/tc_load_dump_parameters.h
35 35 ../header/tm_lfr_tc_exe.h
36 36 ../header/tc_acceptance.h
37 37 ../header/processing/fsw_processing.h
38 38 ../header/processing/avf0_prc0.h
39 39 ../header/processing/avf1_prc1.h
40 40 ../header/processing/avf2_prc2.h
41 41 ../header/fsw_params_wf_handler.h
42 42 ../header/lfr_cpu_usage_report.h
43 43 ../header/lfr_common_headers/ccsds_types.h
44 44 ../header/lfr_common_headers/fsw_params.h
45 45 ../header/lfr_common_headers/fsw_params_nb_bytes.h
46 46 ../header/lfr_common_headers/fsw_params_processing.h
47 47 ../header/lfr_common_headers/tm_byte_positions.h
48 48 ../LFR_basic-parameters/basic_parameters.h
49 49 ../LFR_basic-parameters/basic_parameters_params.h
50 50 ../header/GscMemoryLPP.hpp
51 51 )
52 52
53 53
54 54 option(FSW_verbose "Enable verbose LFR" OFF)
55 55 option(FSW_boot_messages "Enable LFR boot messages" OFF)
56 56 option(FSW_debug_messages "Enable LFR debug messages" OFF)
57 57 option(FSW_cpu_usage_report "Enable LFR cpu usage report" OFF)
58 58 option(FSW_stack_report "Enable LFR stack report" OFF)
59 59 option(FSW_vhdl_dev "?" OFF)
60 60 option(FSW_lpp_dpu_destid "Set to debug at LPP" OFF)
61 61 option(FSW_debug_watchdog "Enable debug watchdog" OFF)
62 62 option(FSW_debug_tch "?" OFF)
63 63
64 64 set(SW_VERSION_N1 "3" CACHE STRING "Choose N1 FSW Version." FORCE)
65 65 set(SW_VERSION_N2 "2" CACHE STRING "Choose N2 FSW Version." FORCE)
66 66 set(SW_VERSION_N3 "0" CACHE STRING "Choose N3 FSW Version." FORCE)
67 set(SW_VERSION_N4 "19" CACHE STRING "Choose N4 FSW Version." FORCE)
67 set(SW_VERSION_N4 "20" CACHE STRING "Choose N4 FSW Version." FORCE)
68 68
69 69 if(FSW_verbose)
70 70 add_definitions(-DPRINT_MESSAGES_ON_CONSOLE)
71 71 endif()
72 72 if(FSW_boot_messages)
73 73 add_definitions(-DBOOT_MESSAGES)
74 74 endif()
75 75 if(FSW_debug_messages)
76 76 add_definitions(-DDEBUG_MESSAGES)
77 77 endif()
78 78 if(FSW_cpu_usage_report)
79 79 add_definitions(-DPRINT_TASK_STATISTICS)
80 80 endif()
81 81 if(FSW_stack_report)
82 82 add_definitions(-DPRINT_STACK_REPORT)
83 83 endif()
84 84 if(FSW_vhdl_dev)
85 85 add_definitions(-DVHDL_DEV)
86 86 endif()
87 87 if(FSW_lpp_dpu_destid)
88 88 add_definitions(-DLPP_DPU_DESTID)
89 89 endif()
90 90 if(FSW_debug_watchdog)
91 91 add_definitions(-DDEBUG_WATCHDOG)
92 92 endif()
93 93 if(FSW_debug_tch)
94 94 add_definitions(-DDEBUG_TCH)
95 95 endif()
96 96
97
98
97 99 add_definitions(-DMSB_FIRST_TCH)
98 100
99 101 add_definitions(-DSWVERSION=-1-0)
100 102 add_definitions(-DSW_VERSION_N1=${SW_VERSION_N1})
101 103 add_definitions(-DSW_VERSION_N2=${SW_VERSION_N2})
102 104 add_definitions(-DSW_VERSION_N3=${SW_VERSION_N3})
103 105 add_definitions(-DSW_VERSION_N4=${SW_VERSION_N4})
104 106
107
105 108 add_executable(fsw ${SOURCES})
106 109
107 110 if(fix-b2bst)
108 111 check_b2bst(fsw ${CMAKE_CURRENT_BINARY_DIR})
109 112 endif()
110 113
111 114 if(NOT FSW_lpp_dpu_destid)
112 115 build_srec(fsw ${CMAKE_CURRENT_BINARY_DIR} "${SW_VERSION_N1}-${SW_VERSION_N2}-${SW_VERSION_N3}-${SW_VERSION_N4}")
113 116 endif()
114 117
115 118
116 119 add_test_cppcheck(fsw STYLE UNUSED_FUNCTIONS POSSIBLE_ERROR MISSING_INCLUDE)
117 120
Show file before | Show file after | Hide comments
@@ -1,96 +1,96
1 1 /** Global variables of the LFR flight software.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * Among global variables, there are:
7 7 * - RTEMS names and id.
8 8 * - APB configuration registers.
9 9 * - waveforms global buffers, used by the waveform picker hardware module to store data.
10 10 * - spectral matrices buffesr, used by the hardware module to store data.
11 11 * - variable related to LFR modes parameters.
12 12 * - the global HK packet buffer.
13 13 * - the global dump parameter buffer.
14 14 *
15 15 */
16 16
17 17 #include <rtems.h>
18 18 #include <grspw.h>
19 19
20 20 #include "ccsds_types.h"
21 21 #include "grlib_regs.h"
22 22 #include "fsw_params.h"
23 23 #include "fsw_params_wf_handler.h"
24 24
25 #define NB_OF_TASKS 20
25
26 26 #define NB_OF_MISC_NAMES 5
27 27
28 28 // RTEMS GLOBAL VARIABLES
29 29 rtems_name misc_name[NB_OF_MISC_NAMES] = {0};
30 rtems_name Task_name[NB_OF_TASKS] = {0}; /* array of task names */
31 rtems_id Task_id[NB_OF_TASKS] = {0}; /* array of task ids */
30 rtems_name Task_name[CONFIGURE_MAXIMUM_TASKS-1] = {0}; /* array of task names */
31 rtems_id Task_id[CONFIGURE_MAXIMUM_TASKS-1] = {0}; /* array of task ids */
32 32 rtems_name timecode_timer_name = 0;
33 33 rtems_id timecode_timer_id = RTEMS_ID_NONE;
34 34 rtems_name name_hk_rate_monotonic = 0; // name of the HK rate monotonic
35 35 rtems_id HK_id = RTEMS_ID_NONE;// id of the HK rate monotonic period
36 36 rtems_name name_avgv_rate_monotonic = 0; // name of the AVGV rate monotonic
37 37 rtems_id AVGV_id = RTEMS_ID_NONE;// id of the AVGV rate monotonic period
38 38 int fdSPW = 0;
39 39 int fdUART = 0;
40 40 unsigned char lfrCurrentMode = 0;
41 41 unsigned char pa_bia_status_info = 0;
42 42 unsigned char thisIsAnASMRestart = 0;
43 43 unsigned char oneTcLfrUpdateTimeReceived = 0;
44 44
45 45 // WAVEFORMS GLOBAL VARIABLES // 2048 * 3 * 4 + 2 * 4 = 24576 + 8 bytes = 24584
46 46 // 97 * 256 = 24832 => delta = 248 bytes = 62 words
47 47 // WAVEFORMS GLOBAL VARIABLES // 2688 * 3 * 4 + 2 * 4 = 32256 + 8 bytes = 32264
48 48 // 127 * 256 = 32512 => delta = 248 bytes = 62 words
49 49 // F0 F1 F2 F3
50 50 volatile int wf_buffer_f0[ NB_RING_NODES_F0 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
51 51 volatile int wf_buffer_f1[ NB_RING_NODES_F1 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
52 52 volatile int wf_buffer_f2[ NB_RING_NODES_F2 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
53 53 volatile int wf_buffer_f3[ NB_RING_NODES_F3 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
54 54
55 55 //***********************************
56 56 // SPECTRAL MATRICES GLOBAL VARIABLES
57 57
58 58 // alignment constraints for the spectral matrices buffers => the first data after the time (8 bytes) shall be aligned on 0x00
59 59 volatile int sm_f0[ NB_RING_NODES_SM_F0 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0};
60 60 volatile int sm_f1[ NB_RING_NODES_SM_F1 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0};
61 61 volatile int sm_f2[ NB_RING_NODES_SM_F2 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0};
62 62
63 63 // APB CONFIGURATION REGISTERS
64 64 time_management_regs_t *time_management_regs = (time_management_regs_t*) REGS_ADDR_TIME_MANAGEMENT;
65 65 gptimer_regs_t *gptimer_regs = (gptimer_regs_t *) REGS_ADDR_GPTIMER;
66 66 waveform_picker_regs_0_1_18_t *waveform_picker_regs = (waveform_picker_regs_0_1_18_t*) REGS_ADDR_WAVEFORM_PICKER;
67 67 spectral_matrix_regs_t *spectral_matrix_regs = (spectral_matrix_regs_t*) REGS_ADDR_SPECTRAL_MATRIX;
68 68
69 69 // MODE PARAMETERS
70 70 Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet = {0};
71 71 struct param_local_str param_local = {0};
72 72 unsigned int lastValidEnterModeTime = {0};
73 73
74 74 // HK PACKETS
75 75 Packet_TM_LFR_HK_t housekeeping_packet = {0};
76 76 // message queues occupancy
77 77 unsigned char hk_lfr_q_sd_fifo_size_max = 0;
78 78 unsigned char hk_lfr_q_rv_fifo_size_max = 0;
79 79 unsigned char hk_lfr_q_p0_fifo_size_max = 0;
80 80 unsigned char hk_lfr_q_p1_fifo_size_max = 0;
81 81 unsigned char hk_lfr_q_p2_fifo_size_max = 0;
82 82 // sequence counters are incremented by APID (PID + CAT) and destination ID
83 83 unsigned short sequenceCounters_SCIENCE_NORMAL_BURST __attribute__((aligned(0x4))) = 0;
84 84 unsigned short sequenceCounters_SCIENCE_SBM1_SBM2 __attribute__((aligned(0x4))) = 0;
85 85 unsigned short sequenceCounters_TC_EXE[SEQ_CNT_NB_DEST_ID] __attribute__((aligned(0x4))) = {0};
86 86 unsigned short sequenceCounters_TM_DUMP[SEQ_CNT_NB_DEST_ID] __attribute__((aligned(0x4))) = {0};
87 87 unsigned short sequenceCounterHK __attribute__((aligned(0x4))) = {0};
88 88 spw_stats grspw_stats __attribute__((aligned(0x4))) = {0};
89 89
90 90 // TC_LFR_UPDATE_INFO
91 91 rw_f_t rw_f;
92 92
93 93 // TC_LFR_LOAD_FILTER_PAR
94 94 filterPar_t filterPar = {0};
95 95
96 96 fbins_masks_t fbins_masks = {0};
Show file before | Show file after | Hide comments
@@ -1,1007 +1,1009
1 1 /** This is the RTEMS initialization module.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * This module contains two very different information:
7 7 * - specific instructions to configure the compilation of the RTEMS executive
8 8 * - functions related to the fligth softwre initialization, especially the INIT RTEMS task
9 9 *
10 10 */
11 11
12 12 //*************************
13 13 // GPL reminder to be added
14 14 //*************************
15 15
16 16 #include <rtems.h>
17 17
18 18
19 19 /* configuration information */
20 20
21 21 #define CONFIGURE_INIT
22 22
23 23 #include <bsp.h> /* for device driver prototypes */
24 24
25 25 /* configuration information */
26 26
27 #define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
28 #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
29
30 #define CONFIGURE_MAXIMUM_TASKS 23 // number of tasks concurrently active including INIT
31 #define CONFIGURE_RTEMS_INIT_TASKS_TABLE
32 #define CONFIGURE_EXTRA_TASK_STACKS (3 * RTEMS_MINIMUM_STACK_SIZE)
33 #define CONFIGURE_LIBIO_MAXIMUM_FILE_DESCRIPTORS 32
34 #define CONFIGURE_INIT_TASK_PRIORITY 1 // instead of 100
35 #define CONFIGURE_INIT_TASK_MODE (RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT)
36 #define CONFIGURE_INIT_TASK_ATTRIBUTES (RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT)
37 #define CONFIGURE_MAXIMUM_DRIVERS 16
38 #define CONFIGURE_MAXIMUM_PERIODS 6 // [hous] [load] [avgv]
39 #define CONFIGURE_MAXIMUM_TIMERS 6 // [spiq] [link] [spacewire_reset_link]
40 #define CONFIGURE_MAXIMUM_MESSAGE_QUEUES 5
41 #ifdef PRINT_STACK_REPORT
42 #define CONFIGURE_STACK_CHECKER_ENABLED
43 #endif
27 #include <fsw_params.h>
44 28
45 29 #include <rtems/confdefs.h>
46 30
47 31 /* If --drvmgr was enabled during the configuration of the RTEMS kernel */
48 32 #ifdef RTEMS_DRVMGR_STARTUP
49 33 #ifdef LEON3
50 34 /* Add Timer and UART Driver */
51 35
52 36 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
53 37 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
54 38 #endif
55 39
56 40 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
57 41 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
58 42 #endif
59 43
60 44 #endif
61 45 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
62
63 46 #include <drvmgr/drvmgr_confdefs.h>
64 47 #endif
65 48
66 49 #include "fsw_init.h"
67 50 #include "fsw_config.c"
68 51 #include "GscMemoryLPP.hpp"
69 52
70 53 void initCache()
71 54 {
72 55 // ASI 2 contains a few control registers that have not been assigned as ancillary state registers.
73 56 // These should only be read and written using 32-bit LDA/STA instructions.
74 57 // All cache registers are accessed through load/store operations to the alternate address space (LDA/STA), using ASI = 2.
75 58 // The table below shows the register addresses:
76 59 // 0x00 Cache control register
77 60 // 0x04 Reserved
78 61 // 0x08 Instruction cache configuration register
79 62 // 0x0C Data cache configuration register
80 63
81 64 // Cache Control Register Leon3 / Leon3FT
82 65 // 31..30 29 28 27..24 23 22 21 20..19 18 17 16
83 66 // RFT PS TB DS FD FI FT ST IB
84 67 // 15 14 13..12 11..10 9..8 7..6 5 4 3..2 1..0
85 68 // IP DP ITE IDE DTE DDE DF IF DCS ICS
86 69
87 70 unsigned int cacheControlRegister;
88 71
89 72 CCR_resetCacheControlRegister();
90 73 ASR16_resetRegisterProtectionControlRegister();
91 74
92 75 cacheControlRegister = CCR_getValue();
93 76 PRINTF1("(0) CCR - Cache Control Register = %x\n", cacheControlRegister);
94 77 PRINTF1("(0) ASR16 = %x\n", *asr16Ptr);
95 78
96 79 CCR_enableInstructionCache(); // ICS bits
97 80 CCR_enableDataCache(); // DCS bits
98 81 CCR_enableInstructionBurstFetch(); // IB bit
99 82
100 83 faultTolerantScheme();
101 84
102 85 cacheControlRegister = CCR_getValue();
103 86 PRINTF1("(1) CCR - Cache Control Register = %x\n", cacheControlRegister);
104 87 PRINTF1("(1) ASR16 Register protection control register = %x\n", *asr16Ptr);
105 88
106 89 PRINTF("\n");
107 90 }
108 91
109 92 rtems_task Init( rtems_task_argument ignored )
110 93 {
111 94 /** This is the RTEMS INIT taks, it is the first task launched by the system.
112 95 *
113 96 * @param unused is the starting argument of the RTEMS task
114 97 *
115 98 * The INIT task create and run all other RTEMS tasks.
116 99 *
117 100 */
118 101
119 102 //***********
120 103 // INIT CACHE
121 104
122 105 unsigned char *vhdlVersion;
123 106
124 107 reset_lfr();
125 108
126 109 reset_local_time();
127 110
128 111 rtems_cpu_usage_reset();
129 112
130 113 rtems_status_code status;
131 114 rtems_status_code status_spw;
132 115 rtems_isr_entry old_isr_handler;
133 116
134 117 old_isr_handler = NULL;
135 118
136 119 // UART settings
137 120 enable_apbuart_transmitter();
138 121 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
139 122
140 123 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
141 124
142 125
143 126 PRINTF("\n\n\n\n\n")
144 127
145 128 initCache();
146 129
147 130 PRINTF("*************************\n")
148 131 PRINTF("** LFR Flight Software **\n")
149 132
150 133 PRINTF1("** %d-", SW_VERSION_N1)
151 134 PRINTF1("%d-" , SW_VERSION_N2)
152 135 PRINTF1("%d-" , SW_VERSION_N3)
153 136 PRINTF1("%d **\n", SW_VERSION_N4)
154 137
155 138 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
156 139 PRINTF("** VHDL **\n")
157 140 PRINTF1("** %d-", vhdlVersion[1])
158 141 PRINTF1("%d-" , vhdlVersion[2])
159 142 PRINTF1("%d **\n", vhdlVersion[3])
160 143 PRINTF("*************************\n")
161 144 PRINTF("\n\n")
162 145
163 146 init_parameter_dump();
164 147 init_kcoefficients_dump();
165 148 init_local_mode_parameters();
166 149 init_housekeeping_parameters();
167 150 init_k_coefficients_prc0();
168 151 init_k_coefficients_prc1();
169 152 init_k_coefficients_prc2();
170 153 pa_bia_status_info = INIT_CHAR;
171 154
172 155 // initialize all reaction wheels frequencies to NaN
173 156 rw_f.cp_rpw_sc_rw1_f1 = NAN;
174 157 rw_f.cp_rpw_sc_rw1_f2 = NAN;
175 158 rw_f.cp_rpw_sc_rw1_f3 = NAN;
176 159 rw_f.cp_rpw_sc_rw1_f4 = NAN;
177 160 rw_f.cp_rpw_sc_rw2_f1 = NAN;
178 161 rw_f.cp_rpw_sc_rw2_f2 = NAN;
179 162 rw_f.cp_rpw_sc_rw2_f3 = NAN;
180 163 rw_f.cp_rpw_sc_rw2_f4 = NAN;
181 164 rw_f.cp_rpw_sc_rw3_f1 = NAN;
182 165 rw_f.cp_rpw_sc_rw3_f2 = NAN;
183 166 rw_f.cp_rpw_sc_rw3_f3 = NAN;
184 167 rw_f.cp_rpw_sc_rw3_f4 = NAN;
185 168 rw_f.cp_rpw_sc_rw4_f1 = NAN;
186 169 rw_f.cp_rpw_sc_rw4_f2 = NAN;
187 170 rw_f.cp_rpw_sc_rw4_f3 = NAN;
188 171 rw_f.cp_rpw_sc_rw4_f4 = NAN;
189 172
190 173 // initialize filtering parameters
191 174 filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED;
192 175 filterPar.sy_lfr_sc_rw_delta_f = DEFAULT_SY_LFR_SC_RW_DELTA_F;
193 176 filterPar.sy_lfr_pas_filter_tbad = DEFAULT_SY_LFR_PAS_FILTER_TBAD;
194 177 filterPar.sy_lfr_pas_filter_shift = DEFAULT_SY_LFR_PAS_FILTER_SHIFT;
195 178 filterPar.modulus_in_finetime = DEFAULT_MODULUS;
196 179 filterPar.tbad_in_finetime = DEFAULT_TBAD;
197 180 filterPar.offset_in_finetime = DEFAULT_OFFSET;
198 181 filterPar.shift_in_finetime = DEFAULT_SHIFT;
199 182 update_last_valid_transition_date( DEFAULT_LAST_VALID_TRANSITION_DATE );
200 183
201 184 // waveform picker initialization
202 185 WFP_init_rings();
203 186 LEON_Clear_interrupt( IRQ_SPARC_GPTIMER_WATCHDOG ); // initialize the waveform rings
204 187 WFP_reset_current_ring_nodes();
205 188 reset_waveform_picker_regs();
206 189
207 190 // spectral matrices initialization
208 191 SM_init_rings(); // initialize spectral matrices rings
209 192 SM_reset_current_ring_nodes();
210 193 reset_spectral_matrix_regs();
211 194
212 195 // configure calibration
213 196 configureCalibration( false ); // true means interleaved mode, false is for normal mode
214 197
215 198 updateLFRCurrentMode( LFR_MODE_STANDBY );
216 199
217 200 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
218 201
219 202 create_names(); // create all names
220 203
221 204 status = create_timecode_timer(); // create the timer used by timecode_irq_handler
222 205 if (status != RTEMS_SUCCESSFUL)
223 206 {
224 207 PRINTF1("in INIT *** ERR in create_timer_timecode, code %d", status)
225 208 }
226 209
227 210 status = create_message_queues(); // create message queues
228 211 if (status != RTEMS_SUCCESSFUL)
229 212 {
230 213 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
231 214 }
232 215
233 216 status = create_all_tasks(); // create all tasks
234 217 if (status != RTEMS_SUCCESSFUL)
235 218 {
236 219 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
237 220 }
238 221
239 222 // **************************
240 223 // <SPACEWIRE INITIALIZATION>
241 224 status_spw = spacewire_open_link(); // (1) open the link
242 225 if ( status_spw != RTEMS_SUCCESSFUL )
243 226 {
244 227 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
245 228 }
246 229
247 230 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
248 231 {
249 232 status_spw = spacewire_configure_link( fdSPW );
250 233 if ( status_spw != RTEMS_SUCCESSFUL )
251 234 {
252 235 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
253 236 }
254 237 }
255 238
256 239 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
257 240 {
258 241 status_spw = spacewire_start_link( fdSPW );
259 242 if ( status_spw != RTEMS_SUCCESSFUL )
260 243 {
261 244 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
262 245 }
263 246 }
264 247 // </SPACEWIRE INITIALIZATION>
265 248 // ***************************
266 249
267 250 status = start_all_tasks(); // start all tasks
268 251 if (status != RTEMS_SUCCESSFUL)
269 252 {
270 253 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
271 254 }
272 255
273 256 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
274 257 status = start_recv_send_tasks();
275 258 if ( status != RTEMS_SUCCESSFUL )
276 259 {
277 260 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
278 261 }
279 262
280 263 // suspend science tasks, they will be restarted later depending on the mode
281 264 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
282 265 if (status != RTEMS_SUCCESSFUL)
283 266 {
284 267 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
285 268 }
286 269
287 270 // configure IRQ handling for the waveform picker unit
288 271 status = rtems_interrupt_catch( waveforms_isr,
289 272 IRQ_SPARC_WAVEFORM_PICKER,
290 273 &old_isr_handler) ;
291 274 // configure IRQ handling for the spectral matrices unit
292 275 status = rtems_interrupt_catch( spectral_matrices_isr,
293 276 IRQ_SPARC_SPECTRAL_MATRIX,
294 277 &old_isr_handler) ;
295 278
296 279 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
297 280 if ( status_spw != RTEMS_SUCCESSFUL )
298 281 {
299 282 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
300 283 if ( status != RTEMS_SUCCESSFUL ) {
301 284 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
302 285 }
303 286 }
304 287
305 288 BOOT_PRINTF("delete INIT\n")
306 289
307 290 set_hk_lfr_sc_potential_flag( true );
308 291
309 292 // start the timer to detect a missing spacewire timecode
310 293 // the timeout is larger because the spw IP needs to receive several valid timecodes before generating a tickout
311 294 // if a tickout is generated, the timer is restarted
312 295 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT_INIT, timecode_timer_routine, NULL );
313 296
314 297 grspw_timecode_callback = &timecode_irq_handler;
315 298
316 299 status = rtems_task_delete(RTEMS_SELF);
317 300
318 301 }
319 302
320 303 void init_local_mode_parameters( void )
321 304 {
322 305 /** This function initialize the param_local global variable with default values.
323 306 *
324 307 */
325 308
326 309 unsigned int i;
327 310
328 311 // LOCAL PARAMETERS
329 312
330 313 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
331 314 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
332 315
333 316 // init sequence counters
334 317
335 318 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
336 319 {
337 320 sequenceCounters_TC_EXE[i] = INIT_CHAR;
338 321 sequenceCounters_TM_DUMP[i] = INIT_CHAR;
339 322 }
340 323 sequenceCounters_SCIENCE_NORMAL_BURST = INIT_CHAR;
341 324 sequenceCounters_SCIENCE_SBM1_SBM2 = INIT_CHAR;
342 325 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << TM_PACKET_SEQ_SHIFT;
343 326 }
344 327
345 328 void reset_local_time( void )
346 329 {
347 330 time_management_regs->ctrl = time_management_regs->ctrl | VAL_SOFTWARE_RESET; // [0010] software reset, coarse time = 0x80000000
348 331 }
349 332
350 333 void create_names( void ) // create all names for tasks and queues
351 334 {
352 335 /** This function creates all RTEMS names used in the software for tasks and queues.
353 336 *
354 337 * @return RTEMS directive status codes:
355 338 * - RTEMS_SUCCESSFUL - successful completion
356 339 *
357 340 */
358 341
359 342 // task names
360 343 Task_name[TASKID_AVGV] = rtems_build_name( 'A', 'V', 'G', 'V' );
361 344 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
362 345 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
363 346 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
364 347 Task_name[TASKID_LOAD] = rtems_build_name( 'L', 'O', 'A', 'D' );
365 348 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
366 349 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
367 350 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
368 351 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
369 352 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
370 353 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
371 354 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
372 355 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
373 356 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
374 357 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
375 358 Task_name[TASKID_LINK] = rtems_build_name( 'L', 'I', 'N', 'K' );
376 359 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
377 360 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
378 361 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
379 362 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
380 363 Task_name[TASKID_SCRB] = rtems_build_name( 'S', 'C', 'R', 'B' );
381 364 Task_name[TASKID_CALI] = rtems_build_name( 'C', 'A', 'L', 'I' );
382 365
383 366 // rate monotonic period names
384 367 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
385 368 name_avgv_rate_monotonic = rtems_build_name( 'A', 'V', 'G', 'V' );
386 369
387 370 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
388 371 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
389 372 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
390 373 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
391 374 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
392 375
393 376 timecode_timer_name = rtems_build_name( 'S', 'P', 'T', 'C' );
394 377 }
395 378
396 379 int create_all_tasks( void ) // create all tasks which run in the software
397 380 {
398 381 /** This function creates all RTEMS tasks used in the software.
399 382 *
400 383 * @return RTEMS directive status codes:
401 384 * - RTEMS_SUCCESSFUL - task created successfully
402 385 * - RTEMS_INVALID_ADDRESS - id is NULL
403 386 * - RTEMS_INVALID_NAME - invalid task name
404 387 * - RTEMS_INVALID_PRIORITY - invalid task priority
405 388 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
406 389 * - RTEMS_TOO_MANY - too many tasks created
407 390 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
408 391 * - RTEMS_TOO_MANY - too many global objects
409 392 *
410 393 */
411 394
412 395 rtems_status_code status;
413 396
414 397 //**********
415 398 // SPACEWIRE
416 399 // RECV
417 400 status = rtems_task_create(
418 401 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
419 402 RTEMS_DEFAULT_MODES,
420 403 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
421 404 );
422 405 if (status == RTEMS_SUCCESSFUL) // SEND
423 406 {
424 407 status = rtems_task_create(
425 408 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
426 409 RTEMS_DEFAULT_MODES,
427 410 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
428 411 );
429 412 }
430 413 if (status == RTEMS_SUCCESSFUL) // LINK
431 414 {
432 415 status = rtems_task_create(
433 416 Task_name[TASKID_LINK], TASK_PRIORITY_LINK, RTEMS_MINIMUM_STACK_SIZE,
434 417 RTEMS_DEFAULT_MODES,
435 418 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LINK]
436 419 );
437 420 }
438 421 if (status == RTEMS_SUCCESSFUL) // ACTN
439 422 {
440 423 status = rtems_task_create(
441 424 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
442 425 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
443 426 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
444 427 );
445 428 }
446 429 if (status == RTEMS_SUCCESSFUL) // SPIQ
447 430 {
448 431 status = rtems_task_create(
449 432 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
450 433 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
451 434 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
452 435 );
453 436 }
454 437
455 438 //******************
456 439 // SPECTRAL MATRICES
457 440 if (status == RTEMS_SUCCESSFUL) // AVF0
458 441 {
459 442 status = rtems_task_create(
460 443 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
461 444 RTEMS_DEFAULT_MODES,
462 445 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
463 446 );
464 447 }
465 448 if (status == RTEMS_SUCCESSFUL) // PRC0
466 449 {
467 450 status = rtems_task_create(
468 451 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
469 452 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
470 453 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
471 454 );
472 455 }
473 456 if (status == RTEMS_SUCCESSFUL) // AVF1
474 457 {
475 458 status = rtems_task_create(
476 459 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
477 460 RTEMS_DEFAULT_MODES,
478 461 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
479 462 );
480 463 }
481 464 if (status == RTEMS_SUCCESSFUL) // PRC1
482 465 {
483 466 status = rtems_task_create(
484 467 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
485 468 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
486 469 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
487 470 );
488 471 }
489 472 if (status == RTEMS_SUCCESSFUL) // AVF2
490 473 {
491 474 status = rtems_task_create(
492 475 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
493 476 RTEMS_DEFAULT_MODES,
494 477 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
495 478 );
496 479 }
497 480 if (status == RTEMS_SUCCESSFUL) // PRC2
498 481 {
499 482 status = rtems_task_create(
500 483 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
501 484 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
502 485 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
503 486 );
504 487 }
505 488
506 489 //****************
507 490 // WAVEFORM PICKER
508 491 if (status == RTEMS_SUCCESSFUL) // WFRM
509 492 {
510 493 status = rtems_task_create(
511 494 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
512 495 RTEMS_DEFAULT_MODES,
513 496 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
514 497 );
515 498 }
516 499 if (status == RTEMS_SUCCESSFUL) // CWF3
517 500 {
518 501 status = rtems_task_create(
519 502 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
520 503 RTEMS_DEFAULT_MODES,
521 504 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
522 505 );
523 506 }
524 507 if (status == RTEMS_SUCCESSFUL) // CWF2
525 508 {
526 509 status = rtems_task_create(
527 510 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
528 511 RTEMS_DEFAULT_MODES,
529 512 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
530 513 );
531 514 }
532 515 if (status == RTEMS_SUCCESSFUL) // CWF1
533 516 {
534 517 status = rtems_task_create(
535 518 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
536 519 RTEMS_DEFAULT_MODES,
537 520 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
538 521 );
539 522 }
540 523 if (status == RTEMS_SUCCESSFUL) // SWBD
541 524 {
542 525 status = rtems_task_create(
543 526 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
544 527 RTEMS_DEFAULT_MODES,
545 528 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
546 529 );
547 530 }
548 531
549 532 //*****
550 533 // MISC
551 534 if (status == RTEMS_SUCCESSFUL) // LOAD
552 535 {
553 536 status = rtems_task_create(
554 537 Task_name[TASKID_LOAD], TASK_PRIORITY_LOAD, RTEMS_MINIMUM_STACK_SIZE,
555 538 RTEMS_DEFAULT_MODES,
556 539 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LOAD]
557 540 );
558 541 }
559 542 if (status == RTEMS_SUCCESSFUL) // DUMB
560 543 {
561 544 status = rtems_task_create(
562 545 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
563 546 RTEMS_DEFAULT_MODES,
564 547 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
565 548 );
566 549 }
567 550 if (status == RTEMS_SUCCESSFUL) // SCRUBBING TASK
568 551 {
569 552 status = rtems_task_create(
570 553 Task_name[TASKID_SCRB], TASK_PRIORITY_SCRB, RTEMS_MINIMUM_STACK_SIZE,
571 554 RTEMS_DEFAULT_MODES,
572 555 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SCRB]
573 556 );
574 557 }
575 558 if (status == RTEMS_SUCCESSFUL) // HOUS
576 559 {
577 560 status = rtems_task_create(
578 561 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
579 562 RTEMS_DEFAULT_MODES,
580 563 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
581 564 );
582 565 }
583 566 if (status == RTEMS_SUCCESSFUL) // AVGV
584 567 {
585 568 status = rtems_task_create(
586 569 Task_name[TASKID_AVGV], TASK_PRIORITY_AVGV, RTEMS_MINIMUM_STACK_SIZE,
587 570 RTEMS_DEFAULT_MODES,
588 571 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVGV]
589 572 );
590 573 }
591 574 if (status == RTEMS_SUCCESSFUL) // CALI
592 575 {
593 576 status = rtems_task_create(
594 577 Task_name[TASKID_CALI], TASK_PRIORITY_CALI, RTEMS_MINIMUM_STACK_SIZE,
595 578 RTEMS_DEFAULT_MODES,
596 579 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CALI]
597 580 );
598 581 }
599 582
600 583 return status;
601 584 }
602 585
603 586 int start_recv_send_tasks( void )
604 587 {
605 588 rtems_status_code status;
606 589
607 590 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
608 591 if (status!=RTEMS_SUCCESSFUL) {
609 592 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
610 593 }
611 594
612 595 if (status == RTEMS_SUCCESSFUL) // SEND
613 596 {
614 597 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
615 598 if (status!=RTEMS_SUCCESSFUL) {
616 599 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
617 600 }
618 601 }
619 602
620 603 return status;
621 604 }
622 605
623 606 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
624 607 {
625 608 /** This function starts all RTEMS tasks used in the software.
626 609 *
627 610 * @return RTEMS directive status codes:
628 611 * - RTEMS_SUCCESSFUL - ask started successfully
629 612 * - RTEMS_INVALID_ADDRESS - invalid task entry point
630 613 * - RTEMS_INVALID_ID - invalid task id
631 614 * - RTEMS_INCORRECT_STATE - task not in the dormant state
632 615 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
633 616 *
634 617 */
635 618 // starts all the tasks fot eh flight software
636 619
637 620 rtems_status_code status;
638 621
639 622 //**********
640 623 // SPACEWIRE
641 624 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
642 625 if (status!=RTEMS_SUCCESSFUL) {
643 626 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
644 627 }
645 628
646 629 if (status == RTEMS_SUCCESSFUL) // LINK
647 630 {
648 631 status = rtems_task_start( Task_id[TASKID_LINK], link_task, 1 );
649 632 if (status!=RTEMS_SUCCESSFUL) {
650 633 BOOT_PRINTF("in INIT *** Error starting TASK_LINK\n")
651 634 }
652 635 }
653 636
654 637 if (status == RTEMS_SUCCESSFUL) // ACTN
655 638 {
656 639 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
657 640 if (status!=RTEMS_SUCCESSFUL) {
658 641 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
659 642 }
660 643 }
661 644
662 645 //******************
663 646 // SPECTRAL MATRICES
664 647 if (status == RTEMS_SUCCESSFUL) // AVF0
665 648 {
666 649 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
667 650 if (status!=RTEMS_SUCCESSFUL) {
668 651 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
669 652 }
670 653 }
671 654 if (status == RTEMS_SUCCESSFUL) // PRC0
672 655 {
673 656 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
674 657 if (status!=RTEMS_SUCCESSFUL) {
675 658 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
676 659 }
677 660 }
678 661 if (status == RTEMS_SUCCESSFUL) // AVF1
679 662 {
680 663 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
681 664 if (status!=RTEMS_SUCCESSFUL) {
682 665 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
683 666 }
684 667 }
685 668 if (status == RTEMS_SUCCESSFUL) // PRC1
686 669 {
687 670 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
688 671 if (status!=RTEMS_SUCCESSFUL) {
689 672 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
690 673 }
691 674 }
692 675 if (status == RTEMS_SUCCESSFUL) // AVF2
693 676 {
694 677 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
695 678 if (status!=RTEMS_SUCCESSFUL) {
696 679 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
697 680 }
698 681 }
699 682 if (status == RTEMS_SUCCESSFUL) // PRC2
700 683 {
701 684 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
702 685 if (status!=RTEMS_SUCCESSFUL) {
703 686 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
704 687 }
705 688 }
706 689
707 690 //****************
708 691 // WAVEFORM PICKER
709 692 if (status == RTEMS_SUCCESSFUL) // WFRM
710 693 {
711 694 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
712 695 if (status!=RTEMS_SUCCESSFUL) {
713 696 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
714 697 }
715 698 }
716 699 if (status == RTEMS_SUCCESSFUL) // CWF3
717 700 {
718 701 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
719 702 if (status!=RTEMS_SUCCESSFUL) {
720 703 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
721 704 }
722 705 }
723 706 if (status == RTEMS_SUCCESSFUL) // CWF2
724 707 {
725 708 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
726 709 if (status!=RTEMS_SUCCESSFUL) {
727 710 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
728 711 }
729 712 }
730 713 if (status == RTEMS_SUCCESSFUL) // CWF1
731 714 {
732 715 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
733 716 if (status!=RTEMS_SUCCESSFUL) {
734 717 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
735 718 }
736 719 }
737 720 if (status == RTEMS_SUCCESSFUL) // SWBD
738 721 {
739 722 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
740 723 if (status!=RTEMS_SUCCESSFUL) {
741 724 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
742 725 }
743 726 }
744 727
745 728 //*****
746 729 // MISC
747 730 if (status == RTEMS_SUCCESSFUL) // HOUS
748 731 {
749 732 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
750 733 if (status!=RTEMS_SUCCESSFUL) {
751 734 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
752 735 }
753 736 }
754 737 if (status == RTEMS_SUCCESSFUL) // AVGV
755 738 {
756 739 status = rtems_task_start( Task_id[TASKID_AVGV], avgv_task, 1 );
757 740 if (status!=RTEMS_SUCCESSFUL) {
758 741 BOOT_PRINTF("in INIT *** Error starting TASK_AVGV\n")
759 742 }
760 743 }
761 744 if (status == RTEMS_SUCCESSFUL) // DUMB
762 745 {
763 746 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
764 747 if (status!=RTEMS_SUCCESSFUL) {
765 748 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
766 749 }
767 750 }
768 751 if (status == RTEMS_SUCCESSFUL) // SCRUBBING
769 752 {
770 753 status = rtems_task_start( Task_id[TASKID_SCRB], scrubbing_task, 1 );
771 754 if (status!=RTEMS_SUCCESSFUL) {
772 755 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
773 756 }
774 757 }
775 758 if (status == RTEMS_SUCCESSFUL) // LOAD
776 759 {
777 760 status = rtems_task_start( Task_id[TASKID_LOAD], load_task, 1 );
778 761 if (status!=RTEMS_SUCCESSFUL) {
779 762 BOOT_PRINTF("in INIT *** Error starting TASK_LOAD\n")
780 763 }
781 764 }
782 765 if (status == RTEMS_SUCCESSFUL) // CALI
783 766 {
784 767 status = rtems_task_start( Task_id[TASKID_CALI], calibration_sweep_task, 1 );
785 768 if (status!=RTEMS_SUCCESSFUL) {
786 769 BOOT_PRINTF("in INIT *** Error starting TASK_LOAD\n")
787 770 }
788 771 }
789 772
790 773 return status;
791 774 }
792 775
793 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
776 rtems_status_code create_message_queues( void ) // create the five message queues used in the software
794 777 {
795 778 rtems_status_code status_recv;
796 779 rtems_status_code status_send;
797 780 rtems_status_code status_q_p0;
798 781 rtems_status_code status_q_p1;
799 782 rtems_status_code status_q_p2;
800 783 rtems_status_code ret;
801 784 rtems_id queue_id;
802 785
803 786 ret = RTEMS_SUCCESSFUL;
804 787 queue_id = RTEMS_ID_NONE;
805 788
806 789 //****************************************
807 790 // create the queue for handling valid TCs
808 791 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
809 792 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
810 793 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
811 794 if ( status_recv != RTEMS_SUCCESSFUL ) {
812 795 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
813 796 }
814 797
815 798 //************************************************
816 799 // create the queue for handling TM packet sending
817 800 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
818 801 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
819 802 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
820 803 if ( status_send != RTEMS_SUCCESSFUL ) {
821 804 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
822 805 }
823 806
824 807 //*****************************************************************************
825 808 // create the queue for handling averaged spectral matrices for processing @ f0
826 809 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
827 810 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
828 811 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
829 812 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
830 813 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
831 814 }
832 815
833 816 //*****************************************************************************
834 817 // create the queue for handling averaged spectral matrices for processing @ f1
835 818 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
836 819 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
837 820 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
838 821 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
839 822 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
840 823 }
841 824
842 825 //*****************************************************************************
843 826 // create the queue for handling averaged spectral matrices for processing @ f2
844 827 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
845 828 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
846 829 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
847 830 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
848 831 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
849 832 }
850 833
851 834 if ( status_recv != RTEMS_SUCCESSFUL )
852 835 {
853 836 ret = status_recv;
854 837 }
855 838 else if( status_send != RTEMS_SUCCESSFUL )
856 839 {
857 840 ret = status_send;
858 841 }
859 842 else if( status_q_p0 != RTEMS_SUCCESSFUL )
860 843 {
861 844 ret = status_q_p0;
862 845 }
863 846 else if( status_q_p1 != RTEMS_SUCCESSFUL )
864 847 {
865 848 ret = status_q_p1;
866 849 }
867 850 else
868 851 {
869 852 ret = status_q_p2;
870 853 }
871 854
872 855 return ret;
873 856 }
874 857
875 858 rtems_status_code create_timecode_timer( void )
876 859 {
877 860 rtems_status_code status;
878 861
879 862 status = rtems_timer_create( timecode_timer_name, &timecode_timer_id );
880 863
881 864 if ( status != RTEMS_SUCCESSFUL )
882 865 {
883 866 PRINTF1("in create_timer_timecode *** ERR creating SPTC timer, %d\n", status)
884 867 }
885 868 else
886 869 {
887 870 PRINTF("in create_timer_timecode *** OK creating SPTC timer\n")
888 871 }
889 872
890 873 return status;
891 874 }
892 875
893 876 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
894 877 {
895 878 rtems_status_code status;
896 879 rtems_name queue_name;
897 880
898 881 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
899 882
900 883 status = rtems_message_queue_ident( queue_name, 0, queue_id );
901 884
902 885 return status;
903 886 }
904 887
905 888 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
906 889 {
907 890 rtems_status_code status;
908 891 rtems_name queue_name;
909 892
910 893 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
911 894
912 895 status = rtems_message_queue_ident( queue_name, 0, queue_id );
913 896
914 897 return status;
915 898 }
916 899
917 900 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
918 901 {
919 902 rtems_status_code status;
920 903 rtems_name queue_name;
921 904
922 905 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
923 906
924 907 status = rtems_message_queue_ident( queue_name, 0, queue_id );
925 908
926 909 return status;
927 910 }
928 911
929 912 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
930 913 {
931 914 rtems_status_code status;
932 915 rtems_name queue_name;
933 916
934 917 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
935 918
936 919 status = rtems_message_queue_ident( queue_name, 0, queue_id );
937 920
938 921 return status;
939 922 }
940 923
941 924 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
942 925 {
943 926 rtems_status_code status;
944 927 rtems_name queue_name;
945 928
946 929 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
947 930
948 931 status = rtems_message_queue_ident( queue_name, 0, queue_id );
949 932
950 933 return status;
951 934 }
952 935
936 /**
937 * @brief update_queue_max_count returns max(fifo_size_max, pending_messages + 1)
938 * @param queue_id
939 * @param fifo_size_max
940 */
953 941 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max )
954 942 {
955 943 u_int32_t count;
956 944 rtems_status_code status;
957 945
958 946 count = 0;
959 947
960 948 status = rtems_message_queue_get_number_pending( queue_id, &count );
961 949
962 950 count = count + 1;
963 951
964 952 if (status != RTEMS_SUCCESSFUL)
965 953 {
966 954 PRINTF1("in update_queue_max_count *** ERR = %d\n", status)
967 955 }
968 956 else
969 957 {
970 958 if (count > *fifo_size_max)
971 959 {
972 960 *fifo_size_max = count;
973 961 }
974 962 }
975 963 }
976 964
965 /**
966 * @brief init_ring initializes given ring buffer
967 * @param ring array of nodes to initialize
968 * @param nbNodes number of node in the ring buffer
969 * @param buffer memory space given to the ring buffer
970 * @param bufferSize size of the whole ring buffer memory space
971 *
972 * @details This function creates a circular buffer from a given number of nodes and a given memory space. It first sets all nodes attributes to thier defaults values
973 * and associates a portion of the given memory space with each node. Then it connects each nodes to build a circular buffer.
974 *
975 * Each node capacity will be bufferSize/nbNodes.
976 *
977 * https://en.wikipedia.org/wiki/Circular_buffer
978 */
977 979 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
978 980 {
979 981 unsigned char i;
980 982
981 983 //***************
982 984 // BUFFER ADDRESS
983 985 for(i=0; i<nbNodes; i++)
984 986 {
985 987 ring[i].coarseTime = INT32_ALL_F;
986 988 ring[i].fineTime = INT32_ALL_F;
987 989 ring[i].sid = INIT_CHAR;
988 990 ring[i].status = INIT_CHAR;
989 991 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
990 992 }
991 993
992 994 //*****
993 995 // NEXT
994 996 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
995 997 for(i=0; i<nbNodes-1; i++)
996 998 {
997 999 ring[i].next = (ring_node*) &ring[ i + 1 ];
998 1000 }
999 1001
1000 1002 //*********
1001 1003 // PREVIOUS
1002 1004 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
1003 1005 for(i=1; i<nbNodes; i++)
1004 1006 {
1005 1007 ring[i].previous = (ring_node*) &ring[ i - 1 ];
1006 1008 }
1007 1009 }
Show file before | Show file after | Hide comments
@@ -1,1633 +1,1608
1 1 /** Functions related to the SpaceWire interface.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle SpaceWire transmissions:
7 7 * - configuration of the SpaceWire link
8 8 * - SpaceWire related interruption requests processing
9 9 * - transmission of TeleMetry packets by a dedicated RTEMS task
10 10 * - reception of TeleCommands by a dedicated RTEMS task
11 11 *
12 12 */
13 13
14 14 #include "fsw_spacewire.h"
15 15
16 16 rtems_name semq_name = 0;
17 17 rtems_id semq_id = RTEMS_ID_NONE;
18 18
19 19 //*****************
20 20 // waveform headers
21 21 Header_TM_LFR_SCIENCE_CWF_t headerCWF = {0};
22 22 Header_TM_LFR_SCIENCE_SWF_t headerSWF = {0};
23 23 Header_TM_LFR_SCIENCE_ASM_t headerASM = {0};
24 24
25 25 unsigned char previousTimecodeCtr = 0;
26 26 unsigned int *grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER);
27 27
28 28 //***********
29 29 // RTEMS TASK
30 30 rtems_task spiq_task(rtems_task_argument unused)
31 31 {
32 32 /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver.
33 33 *
34 34 * @param unused is the starting argument of the RTEMS task
35 35 *
36 36 */
37 37
38 38 rtems_event_set event_out;
39 39 rtems_status_code status;
40 40 int linkStatus;
41 41
42 42 event_out = EVENT_SETS_NONE_PENDING;
43 43 linkStatus = 0;
44 44
45 45 BOOT_PRINTF("in SPIQ *** \n")
46 46
47 47 while(true){
48 48 rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT
49 49 PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n")
50 50
51 51 // [0] SUSPEND RECV AND SEND TASKS
52 52 status = rtems_task_suspend( Task_id[ TASKID_RECV ] );
53 53 if ( status != RTEMS_SUCCESSFUL ) {
54 54 PRINTF("in SPIQ *** ERR suspending RECV Task\n")
55 55 }
56 56 status = rtems_task_suspend( Task_id[ TASKID_SEND ] );
57 57 if ( status != RTEMS_SUCCESSFUL ) {
58 58 PRINTF("in SPIQ *** ERR suspending SEND Task\n")
59 59 }
60 60
61 61 // [1] CHECK THE LINK
62 62 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1)
63 63 if ( linkStatus != SPW_LINK_OK) {
64 64 PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus)
65 65 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
66 66 }
67 67
68 68 // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT
69 69 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2)
70 70 if ( linkStatus != SPW_LINK_OK ) // [2.a] not in run state, reset the link
71 71 {
72 72 spacewire_read_statistics();
73 73 status = spacewire_several_connect_attemps( );
74 74 }
75 75 else // [2.b] in run state, start the link
76 76 {
77 77 status = spacewire_stop_and_start_link( fdSPW ); // start the link
78 78 if ( status != RTEMS_SUCCESSFUL)
79 79 {
80 80 PRINTF1("in SPIQ *** ERR spacewire_stop_and_start_link %d\n", status)
81 81 }
82 82 }
83 83
84 84 // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS
85 85 if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully
86 86 {
87 87 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
88 88 if ( status != RTEMS_SUCCESSFUL ) {
89 89 PRINTF("in SPIQ *** ERR resuming SEND Task\n")
90 90 }
91 91 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
92 92 if ( status != RTEMS_SUCCESSFUL ) {
93 93 PRINTF("in SPIQ *** ERR resuming RECV Task\n")
94 94 }
95 95 }
96 96 else // [3.b] the link is not in run state, go in STANDBY mode
97 97 {
98 98 status = enter_mode_standby();
99 99 if ( status != RTEMS_SUCCESSFUL )
100 100 {
101 101 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
102 102 }
103 103 {
104 104 updateLFRCurrentMode( LFR_MODE_STANDBY );
105 105 }
106 106 // wake the LINK task up to wait for the link recovery
107 107 status = rtems_event_send ( Task_id[TASKID_LINK], RTEMS_EVENT_0 );
108 108 status = rtems_task_suspend( RTEMS_SELF );
109 109 }
110 110 }
111 111 }
112 112
113 113 rtems_task recv_task( rtems_task_argument unused )
114 114 {
115 115 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
116 116 *
117 117 * @param unused is the starting argument of the RTEMS task
118 118 *
119 119 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
120 120 * 1. It reads the incoming data.
121 121 * 2. Launches the acceptance procedure.
122 122 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
123 123 *
124 124 */
125 125
126 126 int len;
127 127 ccsdsTelecommandPacket_t __attribute__((aligned(4))) currentTC;
128 128 unsigned char computed_CRC[ BYTES_PER_CRC ];
129 129 unsigned char currentTC_LEN_RCV[ BYTES_PER_PKT_LEN ];
130 130 unsigned char destinationID;
131 131 unsigned int estimatedPacketLength;
132 132 unsigned int parserCode;
133 133 rtems_status_code status;
134 134 rtems_id queue_recv_id;
135 135 rtems_id queue_send_id;
136 136
137 137 memset( &currentTC, 0, sizeof(ccsdsTelecommandPacket_t) );
138 138 destinationID = 0;
139 139 queue_recv_id = RTEMS_ID_NONE;
140 140 queue_send_id = RTEMS_ID_NONE;
141 141
142 142 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
143 143
144 144 status = get_message_queue_id_recv( &queue_recv_id );
145 145 if (status != RTEMS_SUCCESSFUL)
146 146 {
147 147 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
148 148 }
149 149
150 150 status = get_message_queue_id_send( &queue_send_id );
151 151 if (status != RTEMS_SUCCESSFUL)
152 152 {
153 153 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
154 154 }
155 155
156 156 BOOT_PRINTF("in RECV *** \n")
157 157
158 158 while(1)
159 159 {
160 160 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
161 161 if (len == -1){ // error during the read call
162 162 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
163 163 }
164 164 else {
165 165 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
166 166 PRINTF("in RECV *** packet lenght too short\n")
167 167 }
168 168 else {
169 169 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - PROTID_RES_APP); // => -3 is for Prot ID, Reserved and User App bytes
170 170 PRINTF1("incoming TC with Length (byte): %d\n", len - 3);
171 171 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> SHIFT_1_BYTE);
172 172 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
173 173 // CHECK THE TC
174 174 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
175 175 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
176 176 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
177 177 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
178 178 || (parserCode == WRONG_SRC_ID) )
179 179 { // send TM_LFR_TC_EXE_CORRUPTED
180 180 PRINTF1("TC corrupted received, with code: %d\n", parserCode);
181 181 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
182 182 &&
183 183 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
184 184 )
185 185 {
186 186 if ( parserCode == WRONG_SRC_ID )
187 187 {
188 188 destinationID = SID_TC_GROUND;
189 189 }
190 190 else
191 191 {
192 192 destinationID = currentTC.sourceID;
193 193 }
194 194 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
195 195 computed_CRC, currentTC_LEN_RCV,
196 196 destinationID );
197 197 }
198 198 }
199 199 else
200 200 { // send valid TC to the action launcher
201 201 status = rtems_message_queue_send( queue_recv_id, &currentTC,
202 202 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + PROTID_RES_APP);
203 203 }
204 204 }
205 205 }
206 206
207 207 update_queue_max_count( queue_recv_id, &hk_lfr_q_rv_fifo_size_max );
208 208
209 209 }
210 210 }
211 211
212 212 rtems_task send_task( rtems_task_argument argument)
213 213 {
214 214 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
215 215 *
216 216 * @param unused is the starting argument of the RTEMS task
217 217 *
218 218 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
219 219 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
220 220 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
221 221 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
222 222 * data it contains.
223 223 *
224 224 */
225 225
226 226 rtems_status_code status; // RTEMS status code
227 227 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
228 228 ring_node *incomingRingNodePtr;
229 229 int ring_node_address;
230 230 char *charPtr;
231 231 spw_ioctl_pkt_send *spw_ioctl_send;
232 232 size_t size; // size of the incoming TC packet
233 233 rtems_id queue_send_id;
234 234 unsigned int sid;
235 235 unsigned char sidAsUnsignedChar;
236 236 unsigned char type;
237 237
238 238 incomingRingNodePtr = NULL;
239 239 ring_node_address = 0;
240 240 charPtr = (char *) &ring_node_address;
241 241 size = 0;
242 242 queue_send_id = RTEMS_ID_NONE;
243 243 sid = 0;
244 244 sidAsUnsignedChar = 0;
245 245
246 246 init_header_cwf( &headerCWF );
247 247 init_header_swf( &headerSWF );
248 248 init_header_asm( &headerASM );
249 249
250 250 status = get_message_queue_id_send( &queue_send_id );
251 251 if (status != RTEMS_SUCCESSFUL)
252 252 {
253 253 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
254 254 }
255 255
256 256 BOOT_PRINTF("in SEND *** \n")
257 257
258 258 while(1)
259 259 {
260 260 status = rtems_message_queue_receive( queue_send_id, incomingData, &size,
261 261 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
262 262
263 263 if (status!=RTEMS_SUCCESSFUL)
264 264 {
265 265 PRINTF1("in SEND *** (1) ERR = %d\n", status)
266 266 }
267 267 else
268 268 {
269 269 if ( size == sizeof(ring_node*) )
270 270 {
271 271 charPtr[0] = incomingData[0];
272 272 charPtr[1] = incomingData[1];
273 273 charPtr[BYTE_2] = incomingData[BYTE_2];
274 274 charPtr[BYTE_3] = incomingData[BYTE_3];
275 275 incomingRingNodePtr = (ring_node*) ring_node_address;
276 276 sid = incomingRingNodePtr->sid;
277 277 if ( (sid==SID_NORM_CWF_LONG_F3)
278 278 || (sid==SID_BURST_CWF_F2 )
279 279 || (sid==SID_SBM1_CWF_F1 )
280 280 || (sid==SID_SBM2_CWF_F2 ))
281 281 {
282 282 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
283 283 }
284 284 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
285 285 {
286 286 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
287 287 }
288 else if ( (sid==SID_NORM_CWF_F3) )
288 else if (sid==SID_NORM_CWF_F3)
289 289 {
290 290 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
291 291 }
292 292 else if (sid==SID_NORM_ASM_F0)
293 293 {
294 294 spw_send_asm_f0( incomingRingNodePtr, &headerASM );
295 295 }
296 296 else if (sid==SID_NORM_ASM_F1)
297 297 {
298 298 spw_send_asm_f1( incomingRingNodePtr, &headerASM );
299 299 }
300 300 else if (sid==SID_NORM_ASM_F2)
301 301 {
302 302 spw_send_asm_f2( incomingRingNodePtr, &headerASM );
303 303 }
304 304 else if ( sid==TM_CODE_K_DUMP )
305 305 {
306 306 spw_send_k_dump( incomingRingNodePtr );
307 307 }
308 308 else
309 309 {
310 310 PRINTF1("unexpected sid = %d\n", sid);
311 311 }
312 312 }
313 313 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
314 314 {
315 315 sidAsUnsignedChar = (unsigned char) incomingData[ PACKET_POS_PA_LFR_SID_PKT ];
316 316 sid = sidAsUnsignedChar;
317 317 type = (unsigned char) incomingData[ PACKET_POS_SERVICE_TYPE ];
318 318 if (type == TM_TYPE_LFR_SCIENCE) // this is a BP packet, all other types are handled differently
319 319 // SET THE SEQUENCE_CNT PARAMETER IN CASE OF BP0 OR BP1 PACKETS
320 320 {
321 321 increment_seq_counter_source_id( (unsigned char*) &incomingData[ PACKET_POS_SEQUENCE_CNT ], sid );
322 322 }
323 323
324 324 status = write( fdSPW, incomingData, size );
325 325 if (status == -1){
326 326 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
327 327 }
328 328 }
329 329 else // the incoming message is a spw_ioctl_pkt_send structure
330 330 {
331 331 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
332 332 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
333 333 if (status == -1){
334 334 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
335 335 }
336 336 }
337 337 }
338 338
339 339 update_queue_max_count( queue_send_id, &hk_lfr_q_sd_fifo_size_max );
340 340
341 341 }
342 342 }
343 343
344 344 rtems_task link_task( rtems_task_argument argument )
345 345 {
346 346 rtems_event_set event_out;
347 347 rtems_status_code status;
348 348 int linkStatus;
349 349
350 350 event_out = EVENT_SETS_NONE_PENDING;
351 351 linkStatus = 0;
352 352
353 353 BOOT_PRINTF("in LINK ***\n")
354 354
355 355 while(1)
356 356 {
357 357 // wait for an RTEMS_EVENT
358 358 rtems_event_receive( RTEMS_EVENT_0,
359 359 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
360 360 PRINTF("in LINK *** wait for the link\n")
361 361 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
362 362 while( linkStatus != SPW_LINK_OK) // wait for the link
363 363 {
364 364 status = rtems_task_wake_after( SPW_LINK_WAIT ); // monitor the link each 100ms
365 365 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
366 366 watchdog_reload();
367 367 }
368 368
369 369 spacewire_read_statistics();
370 370 status = spacewire_stop_and_start_link( fdSPW );
371 371
372 372 if (status != RTEMS_SUCCESSFUL)
373 373 {
374 374 PRINTF1("in LINK *** ERR link not started %d\n", status)
375 375 }
376 376 else
377 377 {
378 378 PRINTF("in LINK *** OK link started\n")
379 379 }
380 380
381 381 // restart the SPIQ task
382 382 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
383 383 if ( status != RTEMS_SUCCESSFUL ) {
384 384 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
385 385 }
386 386
387 387 // restart RECV and SEND
388 388 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
389 389 if ( status != RTEMS_SUCCESSFUL ) {
390 390 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
391 391 }
392 392 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
393 393 if ( status != RTEMS_SUCCESSFUL ) {
394 394 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
395 395 }
396 396 }
397 397 }
398 398
399 399 //****************
400 400 // OTHER FUNCTIONS
401 401 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
402 402 {
403 403 /** This function opens the SpaceWire link.
404 404 *
405 405 * @return a valid file descriptor in case of success, -1 in case of a failure
406 406 *
407 407 */
408 408 rtems_status_code status;
409 409
410 410 status = RTEMS_SUCCESSFUL;
411 411
412 412 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
413 413 if ( fdSPW < 0 ) {
414 414 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
415 415 }
416 416 else
417 417 {
418 418 status = RTEMS_SUCCESSFUL;
419 419 }
420 420
421 421 return status;
422 422 }
423 423
424 424 int spacewire_start_link( int fd )
425 425 {
426 426 rtems_status_code status;
427 427
428 428 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
429 429 // -1 default hardcoded driver timeout
430 430
431 431 return status;
432 432 }
433 433
434 434 int spacewire_stop_and_start_link( int fd )
435 435 {
436 436 rtems_status_code status;
437 437
438 438 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
439 439 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
440 440 // -1 default hardcoded driver timeout
441 441
442 442 return status;
443 443 }
444 444
445 445 int spacewire_configure_link( int fd )
446 446 {
447 447 /** This function configures the SpaceWire link.
448 448 *
449 449 * @return GR-RTEMS-DRIVER directive status codes:
450 450 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
451 451 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
452 452 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
453 453 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
454 454 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
455 455 * - 5 EIO - Error when writing to grswp hardware registers.
456 456 * - 2 ENOENT - No such file or directory
457 457 */
458 458
459 459 rtems_status_code status;
460 460
461 461 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
462 462 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
463 463 spw_ioctl_packetsize packetsize;
464 464
465 465 packetsize.rxsize = SPW_RXSIZE;
466 466 packetsize.txdsize = SPW_TXDSIZE;
467 467 packetsize.txhsize = SPW_TXHSIZE;
468 468
469 469 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
470 470 if (status!=RTEMS_SUCCESSFUL) {
471 471 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
472 472 }
473 473 //
474 474 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
475 475 if (status!=RTEMS_SUCCESSFUL) {
476 476 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
477 477 }
478 478 //
479 479 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
480 480 if (status!=RTEMS_SUCCESSFUL) {
481 481 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
482 482 }
483 483 //
484 484 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
485 485 if (status!=RTEMS_SUCCESSFUL) {
486 486 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
487 487 }
488 488 //
489 489 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
490 490 if (status!=RTEMS_SUCCESSFUL) {
491 491 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
492 492 }
493 493 //
494 494 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
495 495 if (status!=RTEMS_SUCCESSFUL) {
496 496 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
497 497 }
498 498 //
499 499 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, CONF_TCODE_CTRL); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
500 500 if (status!=RTEMS_SUCCESSFUL) {
501 501 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
502 502 }
503 503 //
504 504 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_PACKETSIZE, packetsize); // set rxsize, txdsize and txhsize
505 505 if (status!=RTEMS_SUCCESSFUL) {
506 506 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_PACKETSIZE,\n")
507 507 }
508 508
509 509 return status;
510 510 }
511 511
512 512 int spacewire_several_connect_attemps( void )
513 513 {
514 514 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
515 515 *
516 516 * @return RTEMS directive status code:
517 517 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
518 518 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
519 519 *
520 520 */
521 521
522 522 rtems_status_code status_spw;
523 523 rtems_status_code status;
524 524 int i;
525 525
526 526 status_spw = RTEMS_SUCCESSFUL;
527 527
528 528 i = 0;
529 529 while (i < SY_LFR_DPU_CONNECT_ATTEMPT)
530 530 {
531 531 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
532 532
533 533 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
534 534
535 535 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
536 536
537 537 status_spw = spacewire_stop_and_start_link( fdSPW );
538 538
539 539 if ( status_spw != RTEMS_SUCCESSFUL )
540 540 {
541 541 i = i + 1;
542 542 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw);
543 543 }
544 544 else
545 545 {
546 546 i = SY_LFR_DPU_CONNECT_ATTEMPT;
547 547 }
548 548 }
549 549
550 550 return status_spw;
551 551 }
552 552
553 553 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
554 554 {
555 555 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
556 556 *
557 557 * @param val is the value, 0 or 1, used to set the value of the NP bit.
558 558 * @param regAddr is the address of the GRSPW control register.
559 559 *
560 560 * NP is the bit 20 of the GRSPW control register.
561 561 *
562 562 */
563 563
564 564 unsigned int *spwptr = (unsigned int*) regAddr;
565 565
566 566 if (val == 1) {
567 567 *spwptr = *spwptr | SPW_BIT_NP; // [NP] set the No port force bit
568 568 }
569 569 if (val== 0) {
570 570 *spwptr = *spwptr & SPW_BIT_NP_MASK;
571 571 }
572 572 }
573 573
574 574 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
575 575 {
576 576 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
577 577 *
578 578 * @param val is the value, 0 or 1, used to set the value of the RE bit.
579 579 * @param regAddr is the address of the GRSPW control register.
580 580 *
581 581 * RE is the bit 16 of the GRSPW control register.
582 582 *
583 583 */
584 584
585 585 unsigned int *spwptr = (unsigned int*) regAddr;
586 586
587 587 if (val == 1)
588 588 {
589 589 *spwptr = *spwptr | SPW_BIT_RE; // [RE] set the RMAP Enable bit
590 590 }
591 591 if (val== 0)
592 592 {
593 593 *spwptr = *spwptr & SPW_BIT_RE_MASK;
594 594 }
595 595 }
596 596
597 597 void spacewire_read_statistics( void )
598 598 {
599 599 /** This function reads the SpaceWire statistics from the grspw RTEMS driver.
600 600 *
601 601 * @param void
602 602 *
603 603 * @return void
604 604 *
605 605 * Once they are read, the counters are stored in a global variable used during the building of the
606 606 * HK packets.
607 607 *
608 608 */
609 609
610 610 rtems_status_code status;
611 611 spw_stats current;
612 612
613 613 memset(&current, 0, sizeof(spw_stats));
614 614
615 615 spacewire_get_last_error();
616 616
617 617 // read the current statistics
618 618 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &current );
619 619
620 620 // clear the counters
621 621 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_CLR_STATISTICS );
622 622
623 // typedef struct {
624 // unsigned int tx_link_err; // NOT IN HK
625 // unsigned int rx_rmap_header_crc_err; // NOT IN HK
626 // unsigned int rx_rmap_data_crc_err; // NOT IN HK
627 // unsigned int rx_eep_err;
628 // unsigned int rx_truncated;
629 // unsigned int parity_err;
630 // unsigned int escape_err;
631 // unsigned int credit_err;
632 // unsigned int write_sync_err;
633 // unsigned int disconnect_err;
634 // unsigned int early_ep;
635 // unsigned int invalid_address;
636 // unsigned int packets_sent;
637 // unsigned int packets_received;
638 // } spw_stats;
639
640 623 // rx_eep_err
641 624 grspw_stats.rx_eep_err = grspw_stats.rx_eep_err + current.rx_eep_err;
642 625 // rx_truncated
643 626 grspw_stats.rx_truncated = grspw_stats.rx_truncated + current.rx_truncated;
644 627 // parity_err
645 628 grspw_stats.parity_err = grspw_stats.parity_err + current.parity_err;
646 629 // escape_err
647 630 grspw_stats.escape_err = grspw_stats.escape_err + current.escape_err;
648 631 // credit_err
649 632 grspw_stats.credit_err = grspw_stats.credit_err + current.credit_err;
650 633 // write_sync_err
651 634 grspw_stats.write_sync_err = grspw_stats.write_sync_err + current.write_sync_err;
652 635 // disconnect_err
653 636 grspw_stats.disconnect_err = grspw_stats.disconnect_err + current.disconnect_err;
654 637 // early_ep
655 638 grspw_stats.early_ep = grspw_stats.early_ep + current.early_ep;
656 639 // invalid_address
657 640 grspw_stats.invalid_address = grspw_stats.invalid_address + current.invalid_address;
658 641 // packets_sent
659 642 grspw_stats.packets_sent = grspw_stats.packets_sent + current.packets_sent;
660 643 // packets_received
661 644 grspw_stats.packets_received= grspw_stats.packets_received + current.packets_received;
662 645
663 646 }
664 647
665 648 void spacewire_get_last_error( void )
666 649 {
667 650 static spw_stats previous = {0};
668 651 spw_stats current;
669 652 rtems_status_code status;
670 653
671 654 unsigned int hk_lfr_last_er_rid;
672 655 unsigned char hk_lfr_last_er_code;
673 656 int coarseTime;
674 657 int fineTime;
675 658 unsigned char update_hk_lfr_last_er;
676 659
677 660 memset(&current, 0, sizeof(spw_stats));
678 661 hk_lfr_last_er_rid = INIT_CHAR;
679 662 hk_lfr_last_er_code = INIT_CHAR;
680 663 update_hk_lfr_last_er = INIT_CHAR;
681 664
682 665 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &current );
683 666
684 667 // get current time
685 668 coarseTime = time_management_regs->coarse_time;
686 669 fineTime = time_management_regs->fine_time;
687 670
688 // typedef struct {
689 // unsigned int tx_link_err; // NOT IN HK
690 // unsigned int rx_rmap_header_crc_err; // NOT IN HK
691 // unsigned int rx_rmap_data_crc_err; // NOT IN HK
692 // unsigned int rx_eep_err;
693 // unsigned int rx_truncated;
694 // unsigned int parity_err;
695 // unsigned int escape_err;
696 // unsigned int credit_err;
697 // unsigned int write_sync_err;
698 // unsigned int disconnect_err;
699 // unsigned int early_ep;
700 // unsigned int invalid_address;
701 // unsigned int packets_sent;
702 // unsigned int packets_received;
703 // } spw_stats;
704
705 671 // tx_link_err *** no code associated to this field
706 672 // rx_rmap_header_crc_err *** LE *** in HK
707 673 if (previous.rx_rmap_header_crc_err != current.rx_rmap_header_crc_err)
708 674 {
709 675 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
710 676 hk_lfr_last_er_code = CODE_HEADER_CRC;
711 677 update_hk_lfr_last_er = 1;
712 678 }
713 679 // rx_rmap_data_crc_err *** LE *** NOT IN HK
714 680 if (previous.rx_rmap_data_crc_err != current.rx_rmap_data_crc_err)
715 681 {
716 682 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
717 683 hk_lfr_last_er_code = CODE_DATA_CRC;
718 684 update_hk_lfr_last_er = 1;
719 685 }
720 686 // rx_eep_err
721 687 if (previous.rx_eep_err != current.rx_eep_err)
722 688 {
723 689 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
724 690 hk_lfr_last_er_code = CODE_EEP;
725 691 update_hk_lfr_last_er = 1;
726 692 }
727 693 // rx_truncated
728 694 if (previous.rx_truncated != current.rx_truncated)
729 695 {
730 696 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
731 697 hk_lfr_last_er_code = CODE_RX_TOO_BIG;
732 698 update_hk_lfr_last_er = 1;
733 699 }
734 700 // parity_err
735 701 if (previous.parity_err != current.parity_err)
736 702 {
737 703 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
738 704 hk_lfr_last_er_code = CODE_PARITY;
739 705 update_hk_lfr_last_er = 1;
740 706 }
741 707 // escape_err
742 708 if (previous.parity_err != current.parity_err)
743 709 {
744 710 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
745 711 hk_lfr_last_er_code = CODE_ESCAPE;
746 712 update_hk_lfr_last_er = 1;
747 713 }
748 714 // credit_err
749 715 if (previous.credit_err != current.credit_err)
750 716 {
751 717 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
752 718 hk_lfr_last_er_code = CODE_CREDIT;
753 719 update_hk_lfr_last_er = 1;
754 720 }
755 721 // write_sync_err
756 722 if (previous.write_sync_err != current.write_sync_err)
757 723 {
758 724 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
759 725 hk_lfr_last_er_code = CODE_WRITE_SYNC;
760 726 update_hk_lfr_last_er = 1;
761 727 }
762 728 // disconnect_err
763 729 if (previous.disconnect_err != current.disconnect_err)
764 730 {
765 731 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
766 732 hk_lfr_last_er_code = CODE_DISCONNECT;
767 733 update_hk_lfr_last_er = 1;
768 734 }
769 735 // early_ep
770 736 if (previous.early_ep != current.early_ep)
771 737 {
772 738 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
773 739 hk_lfr_last_er_code = CODE_EARLY_EOP_EEP;
774 740 update_hk_lfr_last_er = 1;
775 741 }
776 742 // invalid_address
777 743 if (previous.invalid_address != current.invalid_address)
778 744 {
779 745 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
780 746 hk_lfr_last_er_code = CODE_INVALID_ADDRESS;
781 747 update_hk_lfr_last_er = 1;
782 748 }
783 749
784 750 // if a field has changed, update the hk_last_er fields
785 751 if (update_hk_lfr_last_er == 1)
786 752 {
787 753 update_hk_lfr_last_er_fields( hk_lfr_last_er_rid, hk_lfr_last_er_code );
788 754 }
789 755
790 756 previous = current;
791 757 }
792 758
793 759 void update_hk_lfr_last_er_fields(unsigned int rid, unsigned char code)
794 760 {
795 761 unsigned char *coarseTimePtr;
796 762 unsigned char *fineTimePtr;
797 763
798 764 coarseTimePtr = (unsigned char*) &time_management_regs->coarse_time;
799 765 fineTimePtr = (unsigned char*) &time_management_regs->fine_time;
800 766
801 767 housekeeping_packet.hk_lfr_last_er_rid[0] = (unsigned char) ((rid & BYTE0_MASK) >> SHIFT_1_BYTE );
802 768 housekeeping_packet.hk_lfr_last_er_rid[1] = (unsigned char) (rid & BYTE1_MASK);
803 769 housekeeping_packet.hk_lfr_last_er_code = code;
804 770 housekeeping_packet.hk_lfr_last_er_time[0] = coarseTimePtr[0];
805 771 housekeeping_packet.hk_lfr_last_er_time[1] = coarseTimePtr[1];
806 772 housekeeping_packet.hk_lfr_last_er_time[BYTE_2] = coarseTimePtr[BYTE_2];
807 773 housekeeping_packet.hk_lfr_last_er_time[BYTE_3] = coarseTimePtr[BYTE_3];
808 774 housekeeping_packet.hk_lfr_last_er_time[BYTE_4] = fineTimePtr[BYTE_2];
809 775 housekeeping_packet.hk_lfr_last_er_time[BYTE_5] = fineTimePtr[BYTE_3];
810 776 }
811 777
812 778 void update_hk_with_grspw_stats( void )
813 779 {
814 780 //****************************
815 781 // DPU_SPACEWIRE_IF_STATISTICS
816 782 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (grspw_stats.packets_received >> SHIFT_1_BYTE);
817 783 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (grspw_stats.packets_received);
818 784 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (grspw_stats.packets_sent >> SHIFT_1_BYTE);
819 785 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (grspw_stats.packets_sent);
820 786
821 787 //******************************************
822 788 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
823 789 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) grspw_stats.parity_err;
824 790 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) grspw_stats.disconnect_err;
825 791 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) grspw_stats.escape_err;
826 792 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) grspw_stats.credit_err;
827 793 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) grspw_stats.write_sync_err;
828 794
829 795 //*********************************************
830 796 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
831 797 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) grspw_stats.early_ep;
832 798 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) grspw_stats.invalid_address;
833 799 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) grspw_stats.rx_eep_err;
834 800 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) grspw_stats.rx_truncated;
835 801 }
836 802
837 803 void spacewire_update_hk_lfr_link_state( unsigned char *hk_lfr_status_word_0 )
838 804 {
839 805 unsigned int *statusRegisterPtr;
840 806 unsigned char linkState;
841 807
842 808 statusRegisterPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_STATUS_REGISTER);
843 809 linkState =
844 810 (unsigned char) ( ( (*statusRegisterPtr) >> SPW_LINK_STAT_POS) & STATUS_WORD_LINK_STATE_BITS); // [0000 0111]
845 811
846 812 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 & STATUS_WORD_LINK_STATE_MASK; // [1111 1000] set link state to 0
847 813
848 814 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 | linkState; // update hk_lfr_dpu_spw_link_state
849 815 }
850 816
851 817 void increase_unsigned_char_counter( unsigned char *counter )
852 818 {
853 819 // update the number of valid timecodes that have been received
854 820 if (*counter == UINT8_MAX)
855 821 {
856 822 *counter = 0;
857 823 }
858 824 else
859 825 {
860 826 *counter = *counter + 1;
861 827 }
862 828 }
863 829
864 830 unsigned int check_timecode_and_previous_timecode_coherency(unsigned char currentTimecodeCtr)
865 831 {
866 832 /** This function checks the coherency between the incoming timecode and the last valid timecode.
867 833 *
868 834 * @param currentTimecodeCtr is the incoming timecode
869 835 *
870 836 * @return returned codes::
871 837 * - LFR_DEFAULT
872 838 * - LFR_SUCCESSFUL
873 839 *
874 840 */
875 841
876 842 static unsigned char firstTickout = 1;
877 843 unsigned char ret;
878 844
879 845 ret = LFR_DEFAULT;
880 846
881 847 if (firstTickout == 0)
882 848 {
883 849 if (currentTimecodeCtr == 0)
884 850 {
885 851 if (previousTimecodeCtr == SPW_TIMECODE_MAX)
886 852 {
887 853 ret = LFR_SUCCESSFUL;
888 854 }
889 855 else
890 856 {
891 857 ret = LFR_DEFAULT;
892 858 }
893 859 }
894 860 else
895 861 {
896 862 if (currentTimecodeCtr == (previousTimecodeCtr +1))
897 863 {
898 864 ret = LFR_SUCCESSFUL;
899 865 }
900 866 else
901 867 {
902 868 ret = LFR_DEFAULT;
903 869 }
904 870 }
905 871 }
906 872 else
907 873 {
908 874 firstTickout = 0;
909 875 ret = LFR_SUCCESSFUL;
910 876 }
911 877
912 878 return ret;
913 879 }
914 880
915 881 unsigned int check_timecode_and_internal_time_coherency(unsigned char timecode, unsigned char internalTime)
916 882 {
917 883 unsigned int ret;
918 884
919 885 ret = LFR_DEFAULT;
920 886
921 887 if (timecode == internalTime)
922 888 {
923 889 ret = LFR_SUCCESSFUL;
924 890 }
925 891 else
926 892 {
927 893 ret = LFR_DEFAULT;
928 894 }
929 895
930 896 return ret;
931 897 }
932 898
933 899 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
934 900 {
935 901 // a tickout has been emitted, perform actions on the incoming timecode
936 902
937 903 unsigned char incomingTimecode;
938 904 unsigned char updateTime;
939 905 unsigned char internalTime;
940 906 rtems_status_code status;
941 907
942 908 incomingTimecode = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
943 909 updateTime = time_management_regs->coarse_time_load & TIMECODE_MASK;
944 910 internalTime = time_management_regs->coarse_time & TIMECODE_MASK;
945 911
946 912 housekeeping_packet.hk_lfr_dpu_spw_last_timc = incomingTimecode;
947 913
948 914 // update the number of tickout that have been generated
949 915 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt );
950 916
951 917 //**************************
952 918 // HK_LFR_TIMECODE_ERRONEOUS
953 919 // MISSING and INVALID are handled by the timecode_timer_routine service routine
954 920 if (check_timecode_and_previous_timecode_coherency( incomingTimecode ) == LFR_DEFAULT)
955 921 {
956 922 // this is unexpected but a tickout could have been raised despite of the timecode being erroneous
957 923 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_erroneous );
958 924 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_ERRONEOUS );
959 925 }
960 926
961 927 //************************
962 928 // HK_LFR_TIME_TIMECODE_IT
963 929 // check the coherency between the SpaceWire timecode and the Internal Time
964 930 if (check_timecode_and_internal_time_coherency( incomingTimecode, internalTime ) == LFR_DEFAULT)
965 931 {
966 932 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_it );
967 933 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_IT );
968 934 }
969 935
970 936 //********************
971 937 // HK_LFR_TIMECODE_CTR
972 938 // check the value of the timecode with respect to the last TC_LFR_UPDATE_TIME => SSS-CP-FS-370
973 939 if (oneTcLfrUpdateTimeReceived == 1)
974 940 {
975 941 if ( incomingTimecode != updateTime )
976 942 {
977 943 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_ctr );
978 944 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_CTR );
979 945 }
980 946 }
981 947
982 948 // launch the timecode timer to detect missing or invalid timecodes
983 949 previousTimecodeCtr = incomingTimecode; // update the previousTimecodeCtr value
984 950 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT, timecode_timer_routine, NULL );
985 951 if (status != RTEMS_SUCCESSFUL)
986 952 {
987 953 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_14 );
988 954 }
989 955 }
990 956
991 957 rtems_timer_service_routine timecode_timer_routine( rtems_id timer_id, void *user_data )
992 958 {
993 959 static unsigned char initStep = 1;
994 960
995 961 unsigned char currentTimecodeCtr;
996 962
997 963 currentTimecodeCtr = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
998 964
999 965 if (initStep == 1)
1000 966 {
1001 967 if (currentTimecodeCtr == previousTimecodeCtr)
1002 968 {
1003 969 //************************
1004 970 // HK_LFR_TIMECODE_MISSING
1005 971 // the timecode value has not changed, no valid timecode has been received, the timecode is MISSING
1006 972 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
1007 973 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING );
1008 974 }
1009 975 else if (currentTimecodeCtr == (previousTimecodeCtr+1))
1010 976 {
1011 977 // the timecode value has changed and the value is valid, this is unexpected because
1012 978 // the timer should not have fired, the timecode_irq_handler should have been raised
1013 979 }
1014 980 else
1015 981 {
1016 982 //************************
1017 983 // HK_LFR_TIMECODE_INVALID
1018 984 // the timecode value has changed and the value is not valid, no tickout has been generated
1019 985 // this is why the timer has fired
1020 986 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_invalid );
1021 987 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_INVALID );
1022 988 }
1023 989 }
1024 990 else
1025 991 {
1026 992 initStep = 1;
1027 993 //************************
1028 994 // HK_LFR_TIMECODE_MISSING
1029 995 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
1030 996 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING );
1031 997 }
1032 998
1033 999 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_13 );
1034 1000 }
1035 1001
1036 1002 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
1037 1003 {
1038 1004 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1039 1005 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1040 1006 header->reserved = DEFAULT_RESERVED;
1041 1007 header->userApplication = CCSDS_USER_APP;
1042 1008 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
1043 1009 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
1044 1010 header->packetLength[0] = INIT_CHAR;
1045 1011 header->packetLength[1] = INIT_CHAR;
1046 1012 // DATA FIELD HEADER
1047 1013 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1048 1014 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1049 1015 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
1050 1016 header->destinationID = TM_DESTINATION_ID_GROUND;
1051 1017 header->time[BYTE_0] = INIT_CHAR;
1052 1018 header->time[BYTE_1] = INIT_CHAR;
1053 1019 header->time[BYTE_2] = INIT_CHAR;
1054 1020 header->time[BYTE_3] = INIT_CHAR;
1055 1021 header->time[BYTE_4] = INIT_CHAR;
1056 1022 header->time[BYTE_5] = INIT_CHAR;
1057 1023 // AUXILIARY DATA HEADER
1058 1024 header->sid = INIT_CHAR;
1059 1025 header->pa_bia_status_info = DEFAULT_HKBIA;
1060 1026 header->blkNr[0] = INIT_CHAR;
1061 1027 header->blkNr[1] = INIT_CHAR;
1062 1028 }
1063 1029
1064 1030 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
1065 1031 {
1066 1032 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1067 1033 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1068 1034 header->reserved = DEFAULT_RESERVED;
1069 1035 header->userApplication = CCSDS_USER_APP;
1070 1036 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1071 1037 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1072 1038 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1073 1039 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1074 1040 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> SHIFT_1_BYTE);
1075 1041 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
1076 1042 // DATA FIELD HEADER
1077 1043 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1078 1044 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1079 1045 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
1080 1046 header->destinationID = TM_DESTINATION_ID_GROUND;
1081 1047 header->time[BYTE_0] = INIT_CHAR;
1082 1048 header->time[BYTE_1] = INIT_CHAR;
1083 1049 header->time[BYTE_2] = INIT_CHAR;
1084 1050 header->time[BYTE_3] = INIT_CHAR;
1085 1051 header->time[BYTE_4] = INIT_CHAR;
1086 1052 header->time[BYTE_5] = INIT_CHAR;
1087 1053 // AUXILIARY DATA HEADER
1088 1054 header->sid = INIT_CHAR;
1089 1055 header->pa_bia_status_info = DEFAULT_HKBIA;
1090 1056 header->pktCnt = PKTCNT_SWF; // PKT_CNT
1091 1057 header->pktNr = INIT_CHAR;
1092 1058 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> SHIFT_1_BYTE);
1093 1059 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
1094 1060 }
1095 1061
1096 1062 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
1097 1063 {
1098 1064 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1099 1065 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1100 1066 header->reserved = DEFAULT_RESERVED;
1101 1067 header->userApplication = CCSDS_USER_APP;
1102 1068 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1103 1069 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1104 1070 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1105 1071 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1106 1072 header->packetLength[0] = INIT_CHAR;
1107 1073 header->packetLength[1] = INIT_CHAR;
1108 1074 // DATA FIELD HEADER
1109 1075 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1110 1076 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1111 1077 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
1112 1078 header->destinationID = TM_DESTINATION_ID_GROUND;
1113 1079 header->time[BYTE_0] = INIT_CHAR;
1114 1080 header->time[BYTE_1] = INIT_CHAR;
1115 1081 header->time[BYTE_2] = INIT_CHAR;
1116 1082 header->time[BYTE_3] = INIT_CHAR;
1117 1083 header->time[BYTE_4] = INIT_CHAR;
1118 1084 header->time[BYTE_5] = INIT_CHAR;
1119 1085 // AUXILIARY DATA HEADER
1120 1086 header->sid = INIT_CHAR;
1121 1087 header->pa_bia_status_info = INIT_CHAR;
1122 1088 header->pa_lfr_pkt_cnt_asm = INIT_CHAR;
1123 1089 header->pa_lfr_pkt_nr_asm = INIT_CHAR;
1124 1090 header->pa_lfr_asm_blk_nr[0] = INIT_CHAR;
1125 1091 header->pa_lfr_asm_blk_nr[1] = INIT_CHAR;
1126 1092 }
1127 1093
1128 1094 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
1129 1095 Header_TM_LFR_SCIENCE_CWF_t *header )
1130 1096 {
1131 1097 /** This function sends CWF CCSDS packets (F2, F1 or F0).
1132 1098 *
1133 1099 * @param waveform points to the buffer containing the data that will be send.
1134 1100 * @param sid is the source identifier of the data that will be sent.
1135 1101 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1136 1102 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1137 1103 * contain information to setup the transmission of the data packets.
1138 1104 *
1139 1105 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1140 1106 *
1141 1107 */
1142 1108
1143 1109 unsigned int i;
1144 1110 int ret;
1145 1111 unsigned int coarseTime;
1146 1112 unsigned int fineTime;
1147 1113 rtems_status_code status;
1148 1114 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1149 1115 int *dataPtr;
1150 1116 unsigned char sid;
1151 1117
1152 1118 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1153 1119 spw_ioctl_send_CWF.options = 0;
1154 1120
1155 1121 ret = LFR_DEFAULT;
1156 1122 sid = (unsigned char) ring_node_to_send->sid;
1157 1123
1158 1124 coarseTime = ring_node_to_send->coarseTime;
1159 1125 fineTime = ring_node_to_send->fineTime;
1160 1126 dataPtr = (int*) ring_node_to_send->buffer_address;
1161 1127
1162 1128 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> SHIFT_1_BYTE);
1163 1129 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
1164 1130 header->pa_bia_status_info = pa_bia_status_info;
1165 1131 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1166 1132 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> SHIFT_1_BYTE);
1167 1133 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
1168 1134
1169 1135 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
1170 1136 {
1171 1137 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
1172 1138 spw_ioctl_send_CWF.hdr = (char*) header;
1173 1139 // BUILD THE DATA
1174 1140 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
1175 1141
1176 1142 // SET PACKET SEQUENCE CONTROL
1177 1143 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1178 1144
1179 1145 // SET SID
1180 1146 header->sid = sid;
1181 1147
1182 1148 // SET PACKET TIME
1183 1149 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
1184 1150 //
1185 1151 header->time[0] = header->acquisitionTime[0];
1186 1152 header->time[1] = header->acquisitionTime[1];
1187 1153 header->time[BYTE_2] = header->acquisitionTime[BYTE_2];
1188 1154 header->time[BYTE_3] = header->acquisitionTime[BYTE_3];
1189 1155 header->time[BYTE_4] = header->acquisitionTime[BYTE_4];
1190 1156 header->time[BYTE_5] = header->acquisitionTime[BYTE_5];
1191 1157
1192 1158 // SET PACKET ID
1193 1159 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
1194 1160 {
1195 1161 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> SHIFT_1_BYTE);
1196 1162 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
1197 1163 }
1198 1164 else
1199 1165 {
1200 1166 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1201 1167 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1202 1168 }
1203 1169
1204 1170 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1205 1171 if (status != RTEMS_SUCCESSFUL) {
1206 1172 ret = LFR_DEFAULT;
1207 1173 }
1208 1174 }
1209 1175
1210 1176 return ret;
1211 1177 }
1212 1178
1213 1179 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
1214 1180 Header_TM_LFR_SCIENCE_SWF_t *header )
1215 1181 {
1216 1182 /** This function sends SWF CCSDS packets (F2, F1 or F0).
1217 1183 *
1218 1184 * @param waveform points to the buffer containing the data that will be send.
1219 1185 * @param sid is the source identifier of the data that will be sent.
1220 1186 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
1221 1187 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1222 1188 * contain information to setup the transmission of the data packets.
1223 1189 *
1224 1190 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1225 1191 *
1226 1192 */
1227 1193
1228 1194 unsigned int i;
1229 1195 int ret;
1230 1196 unsigned int coarseTime;
1231 1197 unsigned int fineTime;
1232 1198 rtems_status_code status;
1233 1199 spw_ioctl_pkt_send spw_ioctl_send_SWF;
1234 1200 int *dataPtr;
1235 1201 unsigned char sid;
1236 1202
1237 1203 spw_ioctl_send_SWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_SWF;
1238 1204 spw_ioctl_send_SWF.options = 0;
1239 1205
1240 1206 ret = LFR_DEFAULT;
1241 1207
1242 1208 coarseTime = ring_node_to_send->coarseTime;
1243 1209 fineTime = ring_node_to_send->fineTime;
1244 1210 dataPtr = (int*) ring_node_to_send->buffer_address;
1245 1211 sid = ring_node_to_send->sid;
1246 1212
1247 1213 header->pa_bia_status_info = pa_bia_status_info;
1248 1214 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1249 1215
1250 1216 for (i=0; i<PKTCNT_SWF; i++) // send waveform
1251 1217 {
1252 1218 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
1253 1219 spw_ioctl_send_SWF.hdr = (char*) header;
1254 1220
1255 1221 // SET PACKET SEQUENCE CONTROL
1256 1222 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1257 1223
1258 1224 // SET PACKET LENGTH AND BLKNR
1259 1225 if (i == (PKTCNT_SWF-1))
1260 1226 {
1261 1227 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
1262 1228 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> SHIFT_1_BYTE);
1263 1229 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
1264 1230 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> SHIFT_1_BYTE);
1265 1231 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
1266 1232 }
1267 1233 else
1268 1234 {
1269 1235 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
1270 1236 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> SHIFT_1_BYTE);
1271 1237 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
1272 1238 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> SHIFT_1_BYTE);
1273 1239 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
1274 1240 }
1275 1241
1276 1242 // SET PACKET TIME
1277 1243 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
1278 1244 //
1279 1245 header->time[BYTE_0] = header->acquisitionTime[BYTE_0];
1280 1246 header->time[BYTE_1] = header->acquisitionTime[BYTE_1];
1281 1247 header->time[BYTE_2] = header->acquisitionTime[BYTE_2];
1282 1248 header->time[BYTE_3] = header->acquisitionTime[BYTE_3];
1283 1249 header->time[BYTE_4] = header->acquisitionTime[BYTE_4];
1284 1250 header->time[BYTE_5] = header->acquisitionTime[BYTE_5];
1285 1251
1286 1252 // SET SID
1287 1253 header->sid = sid;
1288 1254
1289 1255 // SET PKTNR
1290 1256 header->pktNr = i+1; // PKT_NR
1291 1257
1292 1258 // SEND PACKET
1293 1259 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
1294 1260 if (status != RTEMS_SUCCESSFUL) {
1295 1261 ret = LFR_DEFAULT;
1296 1262 }
1297 1263 }
1298 1264
1299 1265 return ret;
1300 1266 }
1301 1267
1302 1268 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
1303 1269 Header_TM_LFR_SCIENCE_CWF_t *header )
1304 1270 {
1305 1271 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
1306 1272 *
1307 1273 * @param waveform points to the buffer containing the data that will be send.
1308 1274 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1309 1275 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1310 1276 * contain information to setup the transmission of the data packets.
1311 1277 *
1312 1278 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
1313 1279 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
1314 1280 *
1315 1281 */
1316 1282
1317 1283 unsigned int i;
1318 1284 int ret;
1319 1285 unsigned int coarseTime;
1320 1286 unsigned int fineTime;
1321 1287 rtems_status_code status;
1322 1288 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1323 1289 char *dataPtr;
1324 1290 unsigned char sid;
1325 1291
1326 1292 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1327 1293 spw_ioctl_send_CWF.options = 0;
1328 1294
1329 1295 ret = LFR_DEFAULT;
1330 1296 sid = ring_node_to_send->sid;
1331 1297
1332 1298 coarseTime = ring_node_to_send->coarseTime;
1333 1299 fineTime = ring_node_to_send->fineTime;
1334 1300 dataPtr = (char*) ring_node_to_send->buffer_address;
1335 1301
1336 1302 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> SHIFT_1_BYTE);
1337 1303 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
1338 1304 header->pa_bia_status_info = pa_bia_status_info;
1339 1305 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1340 1306 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> SHIFT_1_BYTE);
1341 1307 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
1342 1308
1343 1309 //*********************
1344 1310 // SEND CWF3_light DATA
1345 1311 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
1346 1312 {
1347 1313 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
1348 1314 spw_ioctl_send_CWF.hdr = (char*) header;
1349 1315 // BUILD THE DATA
1350 1316 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
1351 1317
1352 1318 // SET PACKET SEQUENCE COUNTER
1353 1319 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1354 1320
1355 1321 // SET SID
1356 1322 header->sid = sid;
1357 1323
1358 1324 // SET PACKET TIME
1359 1325 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1360 1326 //
1361 1327 header->time[BYTE_0] = header->acquisitionTime[BYTE_0];
1362 1328 header->time[BYTE_1] = header->acquisitionTime[BYTE_1];
1363 1329 header->time[BYTE_2] = header->acquisitionTime[BYTE_2];
1364 1330 header->time[BYTE_3] = header->acquisitionTime[BYTE_3];
1365 1331 header->time[BYTE_4] = header->acquisitionTime[BYTE_4];
1366 1332 header->time[BYTE_5] = header->acquisitionTime[BYTE_5];
1367 1333
1368 1334 // SET PACKET ID
1369 1335 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1370 1336 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1371 1337
1372 1338 // SEND PACKET
1373 1339 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1374 1340 if (status != RTEMS_SUCCESSFUL) {
1375 1341 ret = LFR_DEFAULT;
1376 1342 }
1377 1343 }
1378 1344
1379 1345 return ret;
1380 1346 }
1381 1347
1382 1348 void spw_send_asm_f0( ring_node *ring_node_to_send,
1383 1349 Header_TM_LFR_SCIENCE_ASM_t *header )
1384 1350 {
1385 1351 unsigned int i;
1386 1352 unsigned int length = 0;
1387 1353 rtems_status_code status;
1388 1354 unsigned int sid;
1389 1355 float *spectral_matrix;
1390 1356 int coarseTime;
1391 1357 int fineTime;
1392 1358 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1393 1359
1394 1360 sid = ring_node_to_send->sid;
1395 1361 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1396 1362 coarseTime = ring_node_to_send->coarseTime;
1397 1363 fineTime = ring_node_to_send->fineTime;
1398 1364
1399 1365 header->pa_bia_status_info = pa_bia_status_info;
1400 1366 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1401 1367
1402 1368 for (i=0; i<PKTCNT_ASM; i++)
1403 1369 {
1404 1370 if ((i==0) || (i==1))
1405 1371 {
1406 1372 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_1;
1407 1373 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1408 1374 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1409 1375 ];
1410 1376 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_1;
1411 1377 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1412 1378 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> SHIFT_1_BYTE ); // BLK_NR MSB
1413 1379 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_1); // BLK_NR LSB
1414 1380 }
1415 1381 else
1416 1382 {
1417 1383 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_2;
1418 1384 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1419 1385 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1420 1386 ];
1421 1387 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_2;
1422 1388 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1423 1389 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> SHIFT_1_BYTE ); // BLK_NR MSB
1424 1390 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_2); // BLK_NR LSB
1425 1391 }
1426 1392
1427 1393 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1428 1394 spw_ioctl_send_ASM.hdr = (char *) header;
1429 1395 spw_ioctl_send_ASM.options = 0;
1430 1396
1431 1397 // (2) BUILD THE HEADER
1432 1398 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1433 1399 header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE);
1434 1400 header->packetLength[1] = (unsigned char) (length);
1435 1401 header->sid = (unsigned char) sid; // SID
1436 1402 header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM;
1437 1403 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1438 1404
1439 1405 // (3) SET PACKET TIME
1440 1406 header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES);
1441 1407 header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES);
1442 1408 header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE);
1443 1409 header->time[BYTE_3] = (unsigned char) (coarseTime);
1444 1410 header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE);
1445 1411 header->time[BYTE_5] = (unsigned char) (fineTime);
1446 1412 //
1447 1413 header->acquisitionTime[BYTE_0] = header->time[BYTE_0];
1448 1414 header->acquisitionTime[BYTE_1] = header->time[BYTE_1];
1449 1415 header->acquisitionTime[BYTE_2] = header->time[BYTE_2];
1450 1416 header->acquisitionTime[BYTE_3] = header->time[BYTE_3];
1451 1417 header->acquisitionTime[BYTE_4] = header->time[BYTE_4];
1452 1418 header->acquisitionTime[BYTE_5] = header->time[BYTE_5];
1453 1419
1454 1420 // (4) SEND PACKET
1455 1421 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1456 1422 if (status != RTEMS_SUCCESSFUL) {
1457 1423 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1458 1424 }
1459 1425 }
1460 1426 }
1461 1427
1462 1428 void spw_send_asm_f1( ring_node *ring_node_to_send,
1463 1429 Header_TM_LFR_SCIENCE_ASM_t *header )
1464 1430 {
1465 1431 unsigned int i;
1466 1432 unsigned int length = 0;
1467 1433 rtems_status_code status;
1468 1434 unsigned int sid;
1469 1435 float *spectral_matrix;
1470 1436 int coarseTime;
1471 1437 int fineTime;
1472 1438 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1473 1439
1474 1440 sid = ring_node_to_send->sid;
1475 1441 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1476 1442 coarseTime = ring_node_to_send->coarseTime;
1477 1443 fineTime = ring_node_to_send->fineTime;
1478 1444
1479 1445 header->pa_bia_status_info = pa_bia_status_info;
1480 1446 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1481 1447
1482 1448 for (i=0; i<PKTCNT_ASM; i++)
1483 1449 {
1484 1450 if ((i==0) || (i==1))
1485 1451 {
1486 1452 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_1;
1487 1453 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1488 1454 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1489 1455 ];
1490 1456 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_1;
1491 1457 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1492 1458 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> SHIFT_1_BYTE ); // BLK_NR MSB
1493 1459 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_1); // BLK_NR LSB
1494 1460 }
1495 1461 else
1496 1462 {
1497 1463 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_2;
1498 1464 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1499 1465 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1500 1466 ];
1501 1467 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_2;
1502 1468 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1503 1469 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> SHIFT_1_BYTE ); // BLK_NR MSB
1504 1470 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_2); // BLK_NR LSB
1505 1471 }
1506 1472
1507 1473 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1508 1474 spw_ioctl_send_ASM.hdr = (char *) header;
1509 1475 spw_ioctl_send_ASM.options = 0;
1510 1476
1511 1477 // (2) BUILD THE HEADER
1512 1478 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1513 1479 header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE);
1514 1480 header->packetLength[1] = (unsigned char) (length);
1515 1481 header->sid = (unsigned char) sid; // SID
1516 1482 header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM;
1517 1483 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1518 1484
1519 1485 // (3) SET PACKET TIME
1520 1486 header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES);
1521 1487 header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES);
1522 1488 header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE);
1523 1489 header->time[BYTE_3] = (unsigned char) (coarseTime);
1524 1490 header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE);
1525 1491 header->time[BYTE_5] = (unsigned char) (fineTime);
1526 1492 //
1527 1493 header->acquisitionTime[BYTE_0] = header->time[BYTE_0];
1528 1494 header->acquisitionTime[BYTE_1] = header->time[BYTE_1];
1529 1495 header->acquisitionTime[BYTE_2] = header->time[BYTE_2];
1530 1496 header->acquisitionTime[BYTE_3] = header->time[BYTE_3];
1531 1497 header->acquisitionTime[BYTE_4] = header->time[BYTE_4];
1532 1498 header->acquisitionTime[BYTE_5] = header->time[BYTE_5];
1533 1499
1534 1500 // (4) SEND PACKET
1535 1501 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1536 1502 if (status != RTEMS_SUCCESSFUL) {
1537 1503 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1538 1504 }
1539 1505 }
1540 1506 }
1541 1507
1508 /**
1509 * @brief spw_send_asm_f2 Sends an ASM packet at F2 over spacewire
1510 * @param ring_node_to_send node pointing to the actual buffer to send
1511 * @param header
1512 */
1542 1513 void spw_send_asm_f2( ring_node *ring_node_to_send,
1543 1514 Header_TM_LFR_SCIENCE_ASM_t *header )
1544 1515 {
1545 1516 unsigned int i;
1546 1517 unsigned int length = 0;
1547 1518 rtems_status_code status;
1548 1519 unsigned int sid;
1549 1520 float *spectral_matrix;
1550 1521 int coarseTime;
1551 1522 int fineTime;
1552 1523 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1553 1524
1554 1525 sid = ring_node_to_send->sid;
1555 1526 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1556 1527 coarseTime = ring_node_to_send->coarseTime;
1557 1528 fineTime = ring_node_to_send->fineTime;
1558 1529
1559 1530 header->pa_bia_status_info = pa_bia_status_info;
1560 1531 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1561 1532
1562 1533 for (i=0; i<PKTCNT_ASM; i++)
1563 1534 {
1564 1535
1565 1536 spw_ioctl_send_ASM.dlen = DLEN_ASM_F2_PKT;
1566 1537 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1567 1538 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM )
1568 1539 ];
1569 1540 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1570 1541 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1571 1542 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> SHIFT_1_BYTE ); // BLK_NR MSB
1572 1543 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1573 1544
1574 1545 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1575 1546 spw_ioctl_send_ASM.hdr = (char *) header;
1576 1547 spw_ioctl_send_ASM.options = 0;
1577 1548
1578 1549 // (2) BUILD THE HEADER
1579 1550 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1580 1551 header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE);
1581 1552 header->packetLength[1] = (unsigned char) (length);
1582 1553 header->sid = (unsigned char) sid; // SID
1583 1554 header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM;
1584 1555 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1585 1556
1586 1557 // (3) SET PACKET TIME
1587 1558 header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES);
1588 1559 header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES);
1589 1560 header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE);
1590 1561 header->time[BYTE_3] = (unsigned char) (coarseTime);
1591 1562 header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE);
1592 1563 header->time[BYTE_5] = (unsigned char) (fineTime);
1593 1564 //
1594 1565 header->acquisitionTime[BYTE_0] = header->time[BYTE_0];
1595 1566 header->acquisitionTime[BYTE_1] = header->time[BYTE_1];
1596 1567 header->acquisitionTime[BYTE_2] = header->time[BYTE_2];
1597 1568 header->acquisitionTime[BYTE_3] = header->time[BYTE_3];
1598 1569 header->acquisitionTime[BYTE_4] = header->time[BYTE_4];
1599 1570 header->acquisitionTime[BYTE_5] = header->time[BYTE_5];
1600 1571
1601 1572 // (4) SEND PACKET
1602 1573 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1603 1574 if (status != RTEMS_SUCCESSFUL) {
1604 1575 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1605 1576 }
1606 1577 }
1607 1578 }
1608 1579
1580 /**
1581 * @brief spw_send_k_dump Sends k coefficients dump packet over spacewire
1582 * @param ring_node_to_send node pointing to the actual buffer to send
1583 */
1609 1584 void spw_send_k_dump( ring_node *ring_node_to_send )
1610 1585 {
1611 1586 rtems_status_code status;
1612 1587 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump;
1613 1588 unsigned int packetLength;
1614 1589 unsigned int size;
1615 1590
1616 1591 PRINTF("spw_send_k_dump\n")
1617 1592
1618 1593 kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address;
1619 1594
1620 1595 packetLength = (kcoefficients_dump->packetLength[0] * CONST_256) + kcoefficients_dump->packetLength[1];
1621 1596
1622 1597 size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
1623 1598
1624 1599 PRINTF2("packetLength %d, size %d\n", packetLength, size )
1625 1600
1626 1601 status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size );
1627 1602
1628 1603 if (status == -1){
1629 1604 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
1630 1605 }
1631 1606
1632 1607 ring_node_to_send->status = INIT_CHAR;
1633 1608 }
Show file before | Show file after | Hide comments
@@ -1,2068 +1,2064
1 1 /** Functions to load and dump parameters in the LFR registers.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TC related to parameter loading and dumping.\n
7 7 * TC_LFR_LOAD_COMMON_PAR\n
8 8 * TC_LFR_LOAD_NORMAL_PAR\n
9 9 * TC_LFR_LOAD_BURST_PAR\n
10 10 * TC_LFR_LOAD_SBM1_PAR\n
11 11 * TC_LFR_LOAD_SBM2_PAR\n
12 12 *
13 13 */
14 14
15 15 #include "tc_load_dump_parameters.h"
16 16
17 17 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_1 = {0};
18 18 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2 = {0};
19 19 ring_node kcoefficient_node_1 = {0};
20 20 ring_node kcoefficient_node_2 = {0};
21 21
22 22 int action_load_common_par(ccsdsTelecommandPacket_t *TC)
23 23 {
24 24 /** This function updates the LFR registers with the incoming common parameters.
25 25 *
26 26 * @param TC points to the TeleCommand packet that is being processed
27 27 *
28 28 *
29 29 */
30 30
31 31 parameter_dump_packet.sy_lfr_common_parameters_spare = TC->dataAndCRC[0];
32 32 parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
33 33 set_wfp_data_shaping( );
34 34 return LFR_SUCCESSFUL;
35 35 }
36 36
37 37 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
38 38 {
39 39 /** This function updates the LFR registers with the incoming normal parameters.
40 40 *
41 41 * @param TC points to the TeleCommand packet that is being processed
42 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 46 int result;
47 47 int flag;
48 48 rtems_status_code status;
49 49
50 50 flag = LFR_SUCCESSFUL;
51 51
52 52 if ( (lfrCurrentMode == LFR_MODE_NORMAL) ||
53 53 (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) {
54 54 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
55 55 flag = LFR_DEFAULT;
56 56 }
57 57
58 58 // CHECK THE PARAMETERS SET CONSISTENCY
59 59 if (flag == LFR_SUCCESSFUL)
60 60 {
61 61 flag = check_normal_par_consistency( TC, queue_id );
62 62 }
63 63
64 64 // SET THE PARAMETERS IF THEY ARE CONSISTENT
65 65 if (flag == LFR_SUCCESSFUL)
66 66 {
67 67 result = set_sy_lfr_n_swf_l( TC );
68 68 result = set_sy_lfr_n_swf_p( TC );
69 69 result = set_sy_lfr_n_bp_p0( TC );
70 70 result = set_sy_lfr_n_bp_p1( TC );
71 71 result = set_sy_lfr_n_asm_p( TC );
72 72 result = set_sy_lfr_n_cwf_long_f3( TC );
73 73 }
74 74
75 75 return flag;
76 76 }
77 77
78 78 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
79 79 {
80 80 /** This function updates the LFR registers with the incoming burst parameters.
81 81 *
82 82 * @param TC points to the TeleCommand packet that is being processed
83 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 87 int flag;
88 88 rtems_status_code status;
89 89 unsigned char sy_lfr_b_bp_p0;
90 90 unsigned char sy_lfr_b_bp_p1;
91 91 float aux;
92 92
93 93 flag = LFR_SUCCESSFUL;
94 94
95 95 if ( lfrCurrentMode == LFR_MODE_BURST ) {
96 96 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
97 97 flag = LFR_DEFAULT;
98 98 }
99 99
100 100 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
101 101 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
102 102
103 103 // sy_lfr_b_bp_p0 shall not be lower than its default value
104 104 if (flag == LFR_SUCCESSFUL)
105 105 {
106 106 if (sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0 )
107 107 {
108 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 109 flag = WRONG_APP_DATA;
110 110 }
111 111 }
112 112 // sy_lfr_b_bp_p1 shall not be lower than its default value
113 113 if (flag == LFR_SUCCESSFUL)
114 114 {
115 115 if (sy_lfr_b_bp_p1 < DEFAULT_SY_LFR_B_BP_P1 )
116 116 {
117 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 118 flag = WRONG_APP_DATA;
119 119 }
120 120 }
121 121 //****************************************************************
122 122 // check the consistency between sy_lfr_b_bp_p0 and sy_lfr_b_bp_p1
123 123 if (flag == LFR_SUCCESSFUL)
124 124 {
125 125 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
126 126 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
127 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 128 if (aux > FLOAT_EQUAL_ZERO)
129 129 {
130 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 131 flag = LFR_DEFAULT;
132 132 }
133 133 }
134 134
135 135 // SET THE PARAMETERS
136 136 if (flag == LFR_SUCCESSFUL)
137 137 {
138 138 flag = set_sy_lfr_b_bp_p0( TC );
139 139 flag = set_sy_lfr_b_bp_p1( TC );
140 140 }
141 141
142 142 return flag;
143 143 }
144 144
145 145 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
146 146 {
147 147 /** This function updates the LFR registers with the incoming sbm1 parameters.
148 148 *
149 149 * @param TC points to the TeleCommand packet that is being processed
150 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 154 int flag;
155 155 rtems_status_code status;
156 156 unsigned char sy_lfr_s1_bp_p0;
157 157 unsigned char sy_lfr_s1_bp_p1;
158 158 float aux;
159 159
160 160 flag = LFR_SUCCESSFUL;
161 161
162 162 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
163 163 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
164 164 flag = LFR_DEFAULT;
165 165 }
166 166
167 167 sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
168 168 sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
169 169
170 170 // sy_lfr_s1_bp_p0
171 171 if (flag == LFR_SUCCESSFUL)
172 172 {
173 173 if (sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0 )
174 174 {
175 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 176 flag = WRONG_APP_DATA;
177 177 }
178 178 }
179 179 // sy_lfr_s1_bp_p1
180 180 if (flag == LFR_SUCCESSFUL)
181 181 {
182 182 if (sy_lfr_s1_bp_p1 < DEFAULT_SY_LFR_S1_BP_P1 )
183 183 {
184 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 185 flag = WRONG_APP_DATA;
186 186 }
187 187 }
188 188 //******************************************************************
189 189 // check the consistency between sy_lfr_s1_bp_p0 and sy_lfr_s1_bp_p1
190 190 if (flag == LFR_SUCCESSFUL)
191 191 {
192 192 aux = ( (float ) sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE) )
193 193 - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE));
194 194 if (aux > FLOAT_EQUAL_ZERO)
195 195 {
196 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 197 flag = LFR_DEFAULT;
198 198 }
199 199 }
200 200
201 201 // SET THE PARAMETERS
202 202 if (flag == LFR_SUCCESSFUL)
203 203 {
204 204 flag = set_sy_lfr_s1_bp_p0( TC );
205 205 flag = set_sy_lfr_s1_bp_p1( TC );
206 206 }
207 207
208 208 return flag;
209 209 }
210 210
211 211 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
212 212 {
213 213 /** This function updates the LFR registers with the incoming sbm2 parameters.
214 214 *
215 215 * @param TC points to the TeleCommand packet that is being processed
216 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 220 int flag;
221 221 rtems_status_code status;
222 222 unsigned char sy_lfr_s2_bp_p0;
223 223 unsigned char sy_lfr_s2_bp_p1;
224 224 float aux;
225 225
226 226 flag = LFR_SUCCESSFUL;
227 227
228 228 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
229 229 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
230 230 flag = LFR_DEFAULT;
231 231 }
232 232
233 233 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
234 234 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
235 235
236 236 // sy_lfr_s2_bp_p0
237 237 if (flag == LFR_SUCCESSFUL)
238 238 {
239 239 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
240 240 {
241 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 242 flag = WRONG_APP_DATA;
243 243 }
244 244 }
245 245 // sy_lfr_s2_bp_p1
246 246 if (flag == LFR_SUCCESSFUL)
247 247 {
248 248 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
249 249 {
250 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 251 flag = WRONG_APP_DATA;
252 252 }
253 253 }
254 254 //******************************************************************
255 255 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
256 256 if (flag == LFR_SUCCESSFUL)
257 257 {
258 258 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
259 259 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
260 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 261 if (aux > FLOAT_EQUAL_ZERO)
262 262 {
263 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 264 flag = LFR_DEFAULT;
265 265 }
266 266 }
267 267
268 268 // SET THE PARAMETERS
269 269 if (flag == LFR_SUCCESSFUL)
270 270 {
271 271 flag = set_sy_lfr_s2_bp_p0( TC );
272 272 flag = set_sy_lfr_s2_bp_p1( TC );
273 273 }
274 274
275 275 return flag;
276 276 }
277 277
278 278 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
279 279 {
280 280 /** This function updates the LFR registers with the incoming sbm2 parameters.
281 281 *
282 282 * @param TC points to the TeleCommand packet that is being processed
283 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 287 int flag;
288 288
289 289 flag = LFR_DEFAULT;
290 290
291 291 flag = set_sy_lfr_kcoeff( TC, queue_id );
292 292
293 293 return flag;
294 294 }
295 295
296 296 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
297 297 {
298 298 /** This function updates the LFR registers with the incoming sbm2 parameters.
299 299 *
300 300 * @param TC points to the TeleCommand packet that is being processed
301 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 305 int flag;
306 306
307 307 flag = LFR_DEFAULT;
308 308
309 309 flag = set_sy_lfr_fbins( TC );
310 310
311 311 // once the fbins masks have been stored, they have to be merged with the masks which handle the reaction wheels frequencies filtering
312 312 merge_fbins_masks();
313 313
314 314 return flag;
315 315 }
316 316
317 317 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
318 318 {
319 319 /** This function updates the LFR registers with the incoming sbm2 parameters.
320 320 *
321 321 * @param TC points to the TeleCommand packet that is being processed
322 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 326 int flag;
327 327 unsigned char k;
328 328
329 329 flag = LFR_DEFAULT;
330 330 k = INIT_CHAR;
331 331
332 332 flag = check_sy_lfr_filter_parameters( TC, queue_id );
333 333
334 334 if (flag == LFR_SUCCESSFUL)
335 335 {
336 336 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ];
337 337 parameter_dump_packet.sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
338 338 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_0 ];
339 339 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_1 ];
340 340 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_2 ];
341 341 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_3 ];
342 342 parameter_dump_packet.sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
343 343 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_0 ];
344 344 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_1 ];
345 345 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_2 ];
346 346 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_3 ];
347 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 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 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 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 353 // store PAS filter parameters
354 354
355 355 // sy_lfr_pas_filter_enabled
356 356 filterPar.spare_sy_lfr_pas_filter_enabled = parameter_dump_packet.spare_sy_lfr_pas_filter_enabled;
357 357 set_sy_lfr_pas_filter_enabled( parameter_dump_packet.spare_sy_lfr_pas_filter_enabled & BIT_PAS_FILTER_ENABLED );
358 358
359 359 // sy_lfr_pas_filter_modulus
360 360 filterPar.modulus_in_finetime = ((uint64_t) parameter_dump_packet.sy_lfr_pas_filter_modulus) * CONST_65536;
361 361
362 362 // sy_lfr_pas_filter_tbad
363 363 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_tbad,
364 364 parameter_dump_packet.sy_lfr_pas_filter_tbad );
365 365 filterPar.tbad_in_finetime = (uint64_t) (filterPar.sy_lfr_pas_filter_tbad * CONST_65536);
366 366
367 367 // sy_lfr_pas_filter_offset
368 368 filterPar.offset_in_finetime = ((uint64_t) parameter_dump_packet.sy_lfr_pas_filter_offset) * CONST_65536;
369 369
370 370 // sy_lfr_pas_filter_shift
371 371 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_shift,
372 372 parameter_dump_packet.sy_lfr_pas_filter_shift );
373 373 filterPar.shift_in_finetime = (uint64_t) (filterPar.sy_lfr_pas_filter_shift * CONST_65536);
374 374
375 375 //****************************************************
376 376 // store the parameter sy_lfr_sc_rw_delta_f as a float
377 377 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f,
378 378 parameter_dump_packet.sy_lfr_sc_rw_delta_f );
379 379
380 380 // copy rw.._k.. from the incoming TC to the local parameter_dump_packet
381 381 for (k = 0; k < NB_RW_K_COEFFS * NB_BYTES_PER_RW_K_COEFF; k++)
382 382 {
383 383 parameter_dump_packet.sy_lfr_rw1_k1[k] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_RW1_K1 + k ];
384 384 }
385 385
386 386 //***********************************************
387 387 // store the parameter sy_lfr_rw.._k.. as a float
388 388 // rw1_k
389 389 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k1, parameter_dump_packet.sy_lfr_rw1_k1 );
390 390 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k2, parameter_dump_packet.sy_lfr_rw1_k2 );
391 391 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k3, parameter_dump_packet.sy_lfr_rw1_k3 );
392 392 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k4, parameter_dump_packet.sy_lfr_rw1_k4 );
393 393 // rw2_k
394 394 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k1, parameter_dump_packet.sy_lfr_rw2_k1 );
395 395 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k2, parameter_dump_packet.sy_lfr_rw2_k2 );
396 396 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k3, parameter_dump_packet.sy_lfr_rw2_k3 );
397 397 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k4, parameter_dump_packet.sy_lfr_rw2_k4 );
398 398 // rw3_k
399 399 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k1, parameter_dump_packet.sy_lfr_rw3_k1 );
400 400 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k2, parameter_dump_packet.sy_lfr_rw3_k2 );
401 401 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k3, parameter_dump_packet.sy_lfr_rw3_k3 );
402 402 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k4, parameter_dump_packet.sy_lfr_rw3_k4 );
403 403 // rw4_k
404 404 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k1, parameter_dump_packet.sy_lfr_rw4_k1 );
405 405 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k2, parameter_dump_packet.sy_lfr_rw4_k2 );
406 406 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k3, parameter_dump_packet.sy_lfr_rw4_k3 );
407 407 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k4, parameter_dump_packet.sy_lfr_rw4_k4 );
408 408
409 409 }
410 410
411 411 return flag;
412 412 }
413 413
414 414 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
415 415 {
416 416 /** This function updates the LFR registers with the incoming sbm2 parameters.
417 417 *
418 418 * @param TC points to the TeleCommand packet that is being processed
419 419 * @param queue_id is the id of the queue which handles TM related to this execution step
420 420 *
421 421 */
422 422
423 423 unsigned int address;
424 424 rtems_status_code status;
425 425 unsigned int freq;
426 426 unsigned int bin;
427 427 unsigned int coeff;
428 428 unsigned char *kCoeffPtr;
429 429 unsigned char *kCoeffDumpPtr;
430 430
431 431 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
432 432 // F0 => 11 bins
433 433 // F1 => 13 bins
434 434 // F2 => 12 bins
435 435 // 36 bins to dump in two packets (30 bins max per packet)
436 436
437 437 //*********
438 438 // PACKET 1
439 439 // 11 F0 bins, 13 F1 bins and 6 F2 bins
440 440 kcoefficients_dump_1.destinationID = TC->sourceID;
441 441 increment_seq_counter_destination_id_dump( kcoefficients_dump_1.packetSequenceControl, TC->sourceID );
442 442 for( freq = 0;
443 443 freq < NB_BINS_COMPRESSED_SM_F0;
444 444 freq++ )
445 445 {
446 446 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1] = freq;
447 447 bin = freq;
448 // printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
449 448 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
450 449 {
451 450 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
452 451 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
453 452 ]; // 2 for the kcoeff_frequency
454 453 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
455 454 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
456 455 }
457 456 }
458 457 for( freq = NB_BINS_COMPRESSED_SM_F0;
459 458 freq < ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
460 459 freq++ )
461 460 {
462 461 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = freq;
463 462 bin = freq - NB_BINS_COMPRESSED_SM_F0;
464 // printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
465 463 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
466 464 {
467 465 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
468 466 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
469 467 ]; // 2 for the kcoeff_frequency
470 468 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
471 469 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
472 470 }
473 471 }
474 472 for( freq = ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
475 473 freq < KCOEFF_BLK_NR_PKT1 ;
476 474 freq++ )
477 475 {
478 476 kcoefficients_dump_1.kcoeff_blks[ (freq * KCOEFF_BLK_SIZE) + 1 ] = freq;
479 477 bin = freq - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
480 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
481 478 for ( coeff = 0; coeff <NB_K_COEFF_PER_BIN; coeff++ )
482 479 {
483 480 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
484 481 (freq * KCOEFF_BLK_SIZE) + (coeff * NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
485 482 ]; // 2 for the kcoeff_frequency
486 483 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
487 484 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
488 485 }
489 486 }
490 487 kcoefficients_dump_1.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
491 488 kcoefficients_dump_1.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
492 489 kcoefficients_dump_1.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
493 490 kcoefficients_dump_1.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
494 491 kcoefficients_dump_1.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
495 492 kcoefficients_dump_1.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
496 493 // SEND DATA
497 494 kcoefficient_node_1.status = 1;
498 495 address = (unsigned int) &kcoefficient_node_1;
499 496 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
500 497 if (status != RTEMS_SUCCESSFUL) {
501 498 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
502 499 }
503 500
504 501 //********
505 502 // PACKET 2
506 503 // 6 F2 bins
507 504 kcoefficients_dump_2.destinationID = TC->sourceID;
508 505 increment_seq_counter_destination_id_dump( kcoefficients_dump_2.packetSequenceControl, TC->sourceID );
509 506 for( freq = 0;
510 507 freq < KCOEFF_BLK_NR_PKT2;
511 508 freq++ )
512 509 {
513 510 kcoefficients_dump_2.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = KCOEFF_BLK_NR_PKT1 + freq;
514 511 bin = freq + KCOEFF_BLK_NR_PKT2;
515 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
516 512 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
517 513 {
518 514 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[
519 515 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ ]; // 2 for the kcoeff_frequency
520 516 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
521 517 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
522 518 }
523 519 }
524 520 kcoefficients_dump_2.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
525 521 kcoefficients_dump_2.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
526 522 kcoefficients_dump_2.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
527 523 kcoefficients_dump_2.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
528 524 kcoefficients_dump_2.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
529 525 kcoefficients_dump_2.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
530 526 // SEND DATA
531 527 kcoefficient_node_2.status = 1;
532 528 address = (unsigned int) &kcoefficient_node_2;
533 529 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
534 530 if (status != RTEMS_SUCCESSFUL) {
535 531 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
536 532 }
537 533
538 534 return status;
539 535 }
540 536
541 537 int action_dump_par( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
542 538 {
543 539 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
544 540 *
545 541 * @param queue_id is the id of the queue which handles TM related to this execution step.
546 542 *
547 543 * @return RTEMS directive status codes:
548 544 * - RTEMS_SUCCESSFUL - message sent successfully
549 545 * - RTEMS_INVALID_ID - invalid queue id
550 546 * - RTEMS_INVALID_SIZE - invalid message size
551 547 * - RTEMS_INVALID_ADDRESS - buffer is NULL
552 548 * - RTEMS_UNSATISFIED - out of message buffers
553 549 * - RTEMS_TOO_MANY - queue s limit has been reached
554 550 *
555 551 */
556 552
557 553 int status;
558 554
559 555 increment_seq_counter_destination_id_dump( parameter_dump_packet.packetSequenceControl, TC->sourceID );
560 556 parameter_dump_packet.destinationID = TC->sourceID;
561 557
562 558 // UPDATE TIME
563 559 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
564 560 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
565 561 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
566 562 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
567 563 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
568 564 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
569 565 // SEND DATA
570 566 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
571 567 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
572 568 if (status != RTEMS_SUCCESSFUL) {
573 569 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
574 570 }
575 571
576 572 return status;
577 573 }
578 574
579 575 //***********************
580 576 // NORMAL MODE PARAMETERS
581 577
582 578 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
583 579 {
584 580 unsigned char msb;
585 581 unsigned char lsb;
586 582 int flag;
587 583 float aux;
588 584 rtems_status_code status;
589 585
590 586 unsigned int sy_lfr_n_swf_l;
591 587 unsigned int sy_lfr_n_swf_p;
592 588 unsigned int sy_lfr_n_asm_p;
593 589 unsigned char sy_lfr_n_bp_p0;
594 590 unsigned char sy_lfr_n_bp_p1;
595 591 unsigned char sy_lfr_n_cwf_long_f3;
596 592
597 593 flag = LFR_SUCCESSFUL;
598 594
599 595 //***************
600 596 // get parameters
601 597 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
602 598 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
603 599 sy_lfr_n_swf_l = (msb * CONST_256) + lsb;
604 600
605 601 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
606 602 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
607 603 sy_lfr_n_swf_p = (msb * CONST_256) + lsb;
608 604
609 605 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
610 606 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
611 607 sy_lfr_n_asm_p = (msb * CONST_256) + lsb;
612 608
613 609 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
614 610
615 611 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
616 612
617 613 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
618 614
619 615 //******************
620 616 // check consistency
621 617 // sy_lfr_n_swf_l
622 618 if (sy_lfr_n_swf_l != DFLT_SY_LFR_N_SWF_L)
623 619 {
624 620 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L + DATAFIELD_OFFSET, sy_lfr_n_swf_l );
625 621 flag = WRONG_APP_DATA;
626 622 }
627 623 // sy_lfr_n_swf_p
628 624 if (flag == LFR_SUCCESSFUL)
629 625 {
630 626 if ( sy_lfr_n_swf_p < MIN_SY_LFR_N_SWF_P )
631 627 {
632 628 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P + DATAFIELD_OFFSET, sy_lfr_n_swf_p );
633 629 flag = WRONG_APP_DATA;
634 630 }
635 631 }
636 632 // sy_lfr_n_bp_p0
637 633 if (flag == LFR_SUCCESSFUL)
638 634 {
639 635 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
640 636 {
641 637 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0 + DATAFIELD_OFFSET, sy_lfr_n_bp_p0 );
642 638 flag = WRONG_APP_DATA;
643 639 }
644 640 }
645 641 // sy_lfr_n_asm_p
646 642 if (flag == LFR_SUCCESSFUL)
647 643 {
648 644 if (sy_lfr_n_asm_p == 0)
649 645 {
650 646 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
651 647 flag = WRONG_APP_DATA;
652 648 }
653 649 }
654 650 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
655 651 if (flag == LFR_SUCCESSFUL)
656 652 {
657 653 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
658 654 if (aux > FLOAT_EQUAL_ZERO)
659 655 {
660 656 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
661 657 flag = WRONG_APP_DATA;
662 658 }
663 659 }
664 660 // sy_lfr_n_bp_p1
665 661 if (flag == LFR_SUCCESSFUL)
666 662 {
667 663 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
668 664 {
669 665 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
670 666 flag = WRONG_APP_DATA;
671 667 }
672 668 }
673 669 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
674 670 if (flag == LFR_SUCCESSFUL)
675 671 {
676 672 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
677 673 if (aux > FLOAT_EQUAL_ZERO)
678 674 {
679 675 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
680 676 flag = LFR_DEFAULT;
681 677 }
682 678 }
683 679 // sy_lfr_n_cwf_long_f3
684 680
685 681 return flag;
686 682 }
687 683
688 684 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
689 685 {
690 686 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
691 687 *
692 688 * @param TC points to the TeleCommand packet that is being processed
693 689 * @param queue_id is the id of the queue which handles TM related to this execution step
694 690 *
695 691 */
696 692
697 693 int result;
698 694
699 695 result = LFR_SUCCESSFUL;
700 696
701 697 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
702 698 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
703 699
704 700 return result;
705 701 }
706 702
707 703 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
708 704 {
709 705 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
710 706 *
711 707 * @param TC points to the TeleCommand packet that is being processed
712 708 * @param queue_id is the id of the queue which handles TM related to this execution step
713 709 *
714 710 */
715 711
716 712 int result;
717 713
718 714 result = LFR_SUCCESSFUL;
719 715
720 716 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
721 717 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
722 718
723 719 return result;
724 720 }
725 721
726 722 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
727 723 {
728 724 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
729 725 *
730 726 * @param TC points to the TeleCommand packet that is being processed
731 727 * @param queue_id is the id of the queue which handles TM related to this execution step
732 728 *
733 729 */
734 730
735 731 int result;
736 732
737 733 result = LFR_SUCCESSFUL;
738 734
739 735 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
740 736 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
741 737
742 738 return result;
743 739 }
744 740
745 741 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
746 742 {
747 743 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
748 744 *
749 745 * @param TC points to the TeleCommand packet that is being processed
750 746 * @param queue_id is the id of the queue which handles TM related to this execution step
751 747 *
752 748 */
753 749
754 750 int status;
755 751
756 752 status = LFR_SUCCESSFUL;
757 753
758 754 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
759 755
760 756 return status;
761 757 }
762 758
763 759 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
764 760 {
765 761 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
766 762 *
767 763 * @param TC points to the TeleCommand packet that is being processed
768 764 * @param queue_id is the id of the queue which handles TM related to this execution step
769 765 *
770 766 */
771 767
772 768 int status;
773 769
774 770 status = LFR_SUCCESSFUL;
775 771
776 772 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
777 773
778 774 return status;
779 775 }
780 776
781 777 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
782 778 {
783 779 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
784 780 *
785 781 * @param TC points to the TeleCommand packet that is being processed
786 782 * @param queue_id is the id of the queue which handles TM related to this execution step
787 783 *
788 784 */
789 785
790 786 int status;
791 787
792 788 status = LFR_SUCCESSFUL;
793 789
794 790 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
795 791
796 792 return status;
797 793 }
798 794
799 795 //**********************
800 796 // BURST MODE PARAMETERS
801 797
802 798 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
803 799 {
804 800 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
805 801 *
806 802 * @param TC points to the TeleCommand packet that is being processed
807 803 * @param queue_id is the id of the queue which handles TM related to this execution step
808 804 *
809 805 */
810 806
811 807 int status;
812 808
813 809 status = LFR_SUCCESSFUL;
814 810
815 811 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
816 812
817 813 return status;
818 814 }
819 815
820 816 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
821 817 {
822 818 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
823 819 *
824 820 * @param TC points to the TeleCommand packet that is being processed
825 821 * @param queue_id is the id of the queue which handles TM related to this execution step
826 822 *
827 823 */
828 824
829 825 int status;
830 826
831 827 status = LFR_SUCCESSFUL;
832 828
833 829 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
834 830
835 831 return status;
836 832 }
837 833
838 834 //*********************
839 835 // SBM1 MODE PARAMETERS
840 836
841 837 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
842 838 {
843 839 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
844 840 *
845 841 * @param TC points to the TeleCommand packet that is being processed
846 842 * @param queue_id is the id of the queue which handles TM related to this execution step
847 843 *
848 844 */
849 845
850 846 int status;
851 847
852 848 status = LFR_SUCCESSFUL;
853 849
854 850 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
855 851
856 852 return status;
857 853 }
858 854
859 855 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
860 856 {
861 857 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
862 858 *
863 859 * @param TC points to the TeleCommand packet that is being processed
864 860 * @param queue_id is the id of the queue which handles TM related to this execution step
865 861 *
866 862 */
867 863
868 864 int status;
869 865
870 866 status = LFR_SUCCESSFUL;
871 867
872 868 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
873 869
874 870 return status;
875 871 }
876 872
877 873 //*********************
878 874 // SBM2 MODE PARAMETERS
879 875
880 876 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC )
881 877 {
882 878 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
883 879 *
884 880 * @param TC points to the TeleCommand packet that is being processed
885 881 * @param queue_id is the id of the queue which handles TM related to this execution step
886 882 *
887 883 */
888 884
889 885 int status;
890 886
891 887 status = LFR_SUCCESSFUL;
892 888
893 889 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
894 890
895 891 return status;
896 892 }
897 893
898 894 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
899 895 {
900 896 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
901 897 *
902 898 * @param TC points to the TeleCommand packet that is being processed
903 899 * @param queue_id is the id of the queue which handles TM related to this execution step
904 900 *
905 901 */
906 902
907 903 int status;
908 904
909 905 status = LFR_SUCCESSFUL;
910 906
911 907 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
912 908
913 909 return status;
914 910 }
915 911
916 912 //*******************
917 913 // TC_LFR_UPDATE_INFO
918 914
919 915 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
920 916 {
921 917 unsigned int status;
922 918
923 919 status = LFR_DEFAULT;
924 920
925 921 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
926 922 || (mode == LFR_MODE_BURST)
927 923 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
928 924 {
929 925 status = LFR_SUCCESSFUL;
930 926 }
931 927 else
932 928 {
933 929 status = LFR_DEFAULT;
934 930 }
935 931
936 932 return status;
937 933 }
938 934
939 935 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
940 936 {
941 937 unsigned int status;
942 938
943 939 status = LFR_DEFAULT;
944 940
945 941 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
946 942 || (mode == TDS_MODE_BURST)
947 943 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
948 944 || (mode == TDS_MODE_LFM))
949 945 {
950 946 status = LFR_SUCCESSFUL;
951 947 }
952 948 else
953 949 {
954 950 status = LFR_DEFAULT;
955 951 }
956 952
957 953 return status;
958 954 }
959 955
960 956 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
961 957 {
962 958 unsigned int status;
963 959
964 960 status = LFR_DEFAULT;
965 961
966 962 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
967 963 || (mode == THR_MODE_BURST))
968 964 {
969 965 status = LFR_SUCCESSFUL;
970 966 }
971 967 else
972 968 {
973 969 status = LFR_DEFAULT;
974 970 }
975 971
976 972 return status;
977 973 }
978 974
979 975 void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value )
980 976 {
981 977 unsigned char flag;
982 978 unsigned char flagPosInByte;
983 979 unsigned char newFlag;
984 980 unsigned char flagMask;
985 981
986 982 // if the frequency value is not a number, the flag is set to 0 and the frequency RWx_Fy is not filtered
987 983 if (isnan(value))
988 984 {
989 985 flag = FLAG_NAN;
990 986 }
991 987 else
992 988 {
993 989 flag = FLAG_IAN;
994 990 }
995 991
996 992 switch(wheel)
997 993 {
998 994 case WHEEL_1:
999 995 flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
1000 996 flagMask = ~(1 << flagPosInByte);
1001 997 newFlag = flag << flagPosInByte;
1002 998 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
1003 999 break;
1004 1000 case WHEEL_2:
1005 1001 flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
1006 1002 flagMask = ~(1 << flagPosInByte);
1007 1003 newFlag = flag << flagPosInByte;
1008 1004 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
1009 1005 break;
1010 1006 case WHEEL_3:
1011 1007 flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
1012 1008 flagMask = ~(1 << flagPosInByte);
1013 1009 newFlag = flag << flagPosInByte;
1014 1010 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1015 1011 break;
1016 1012 case WHEEL_4:
1017 1013 flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
1018 1014 flagMask = ~(1 << flagPosInByte);
1019 1015 newFlag = flag << flagPosInByte;
1020 1016 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1021 1017 break;
1022 1018 default:
1023 1019 break;
1024 1020 }
1025 1021 }
1026 1022
1027 1023 void set_hk_lfr_sc_rw_f_flags( void )
1028 1024 {
1029 1025 // RW1
1030 1026 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_1, rw_f.cp_rpw_sc_rw1_f1 );
1031 1027 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_2, rw_f.cp_rpw_sc_rw1_f2 );
1032 1028 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_3, rw_f.cp_rpw_sc_rw1_f3 );
1033 1029 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_4, rw_f.cp_rpw_sc_rw1_f4 );
1034 1030
1035 1031 // RW2
1036 1032 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_1, rw_f.cp_rpw_sc_rw2_f1 );
1037 1033 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_2, rw_f.cp_rpw_sc_rw2_f2 );
1038 1034 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_3, rw_f.cp_rpw_sc_rw2_f3 );
1039 1035 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_4, rw_f.cp_rpw_sc_rw2_f4 );
1040 1036
1041 1037 // RW3
1042 1038 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_1, rw_f.cp_rpw_sc_rw3_f1 );
1043 1039 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_2, rw_f.cp_rpw_sc_rw3_f2 );
1044 1040 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_3, rw_f.cp_rpw_sc_rw3_f3 );
1045 1041 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_4, rw_f.cp_rpw_sc_rw3_f4 );
1046 1042
1047 1043 // RW4
1048 1044 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_1, rw_f.cp_rpw_sc_rw4_f1 );
1049 1045 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_2, rw_f.cp_rpw_sc_rw4_f2 );
1050 1046 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_3, rw_f.cp_rpw_sc_rw4_f3 );
1051 1047 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_4, rw_f.cp_rpw_sc_rw4_f4 );
1052 1048 }
1053 1049
1054 1050 int check_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value )
1055 1051 {
1056 1052 float rw_k;
1057 1053 int ret;
1058 1054
1059 1055 ret = LFR_SUCCESSFUL;
1060 1056 rw_k = INIT_FLOAT;
1061 1057
1062 1058 copyFloatByChar( (unsigned char*) &rw_k, (unsigned char*) &TC->packetID[ offset ] );
1063 1059
1064 1060 *pos = offset;
1065 1061 *value = rw_k;
1066 1062
1067 1063 if (rw_k < MIN_SY_LFR_RW_F)
1068 1064 {
1069 1065 ret = WRONG_APP_DATA;
1070 1066 }
1071 1067
1072 1068 return ret;
1073 1069 }
1074 1070
1075 1071 int check_all_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int *pos, float*value )
1076 1072 {
1077 1073 int ret;
1078 1074
1079 1075 ret = LFR_SUCCESSFUL;
1080 1076
1081 1077 //****
1082 1078 //****
1083 1079 // RW1
1084 1080 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1, pos, value ); // F1
1085 1081 if (ret == LFR_SUCCESSFUL) // F2
1086 1082 {
1087 1083 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2, pos, value );
1088 1084 }
1089 1085 if (ret == LFR_SUCCESSFUL) // F3
1090 1086 {
1091 1087 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3, pos, value );
1092 1088 }
1093 1089 if (ret == LFR_SUCCESSFUL) // F4
1094 1090 {
1095 1091 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4, pos, value );
1096 1092 }
1097 1093
1098 1094 //****
1099 1095 //****
1100 1096 // RW2
1101 1097 if (ret == LFR_SUCCESSFUL) // F1
1102 1098 {
1103 1099 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1, pos, value );
1104 1100 }
1105 1101 if (ret == LFR_SUCCESSFUL) // F2
1106 1102 {
1107 1103 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2, pos, value );
1108 1104 }
1109 1105 if (ret == LFR_SUCCESSFUL) // F3
1110 1106 {
1111 1107 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3, pos, value );
1112 1108 }
1113 1109 if (ret == LFR_SUCCESSFUL) // F4
1114 1110 {
1115 1111 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4, pos, value );
1116 1112 }
1117 1113
1118 1114 //****
1119 1115 //****
1120 1116 // RW3
1121 1117 if (ret == LFR_SUCCESSFUL) // F1
1122 1118 {
1123 1119 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1, pos, value );
1124 1120 }
1125 1121 if (ret == LFR_SUCCESSFUL) // F2
1126 1122 {
1127 1123 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2, pos, value );
1128 1124 }
1129 1125 if (ret == LFR_SUCCESSFUL) // F3
1130 1126 {
1131 1127 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3, pos, value );
1132 1128 }
1133 1129 if (ret == LFR_SUCCESSFUL) // F4
1134 1130 {
1135 1131 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4, pos, value );
1136 1132 }
1137 1133
1138 1134 //****
1139 1135 //****
1140 1136 // RW4
1141 1137 if (ret == LFR_SUCCESSFUL) // F1
1142 1138 {
1143 1139 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1, pos, value );
1144 1140 }
1145 1141 if (ret == LFR_SUCCESSFUL) // F2
1146 1142 {
1147 1143 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2, pos, value );
1148 1144 }
1149 1145 if (ret == LFR_SUCCESSFUL) // F3
1150 1146 {
1151 1147 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3, pos, value );
1152 1148 }
1153 1149 if (ret == LFR_SUCCESSFUL) // F4
1154 1150 {
1155 1151 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4, pos, value );
1156 1152 }
1157 1153
1158 1154 return ret;
1159 1155 }
1160 1156
1161 1157 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC )
1162 1158 {
1163 1159 /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally.
1164 1160 *
1165 1161 * @param TC points to the TeleCommand packet that is being processed
1166 1162 *
1167 1163 */
1168 1164
1169 1165 unsigned char * bytePosPtr; // pointer to the beginning of the incoming TC packet
1170 1166
1171 1167 bytePosPtr = (unsigned char *) &TC->packetID;
1172 1168
1173 1169 // rw1_f
1174 1170 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] );
1175 1171 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] );
1176 1172 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3 ] );
1177 1173 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4 ] );
1178 1174
1179 1175 // rw2_f
1180 1176 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] );
1181 1177 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] );
1182 1178 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3 ] );
1183 1179 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4 ] );
1184 1180
1185 1181 // rw3_f
1186 1182 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] );
1187 1183 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] );
1188 1184 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3 ] );
1189 1185 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4 ] );
1190 1186
1191 1187 // rw4_f
1192 1188 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] );
1193 1189 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] );
1194 1190 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3 ] );
1195 1191 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4 ] );
1196 1192
1197 1193 // test each reaction wheel frequency value. NaN means that the frequency is not filtered
1198 1194
1199 1195 }
1200 1196
1201 1197 void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, float sy_lfr_rw_k )
1202 1198 {
1203 1199 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
1204 1200 *
1205 1201 * @param fbins_mask
1206 1202 * @param rw_f is the reaction wheel frequency to filter
1207 1203 * @param delta_f is the frequency step between the frequency bins, it depends on the frequency channel
1208 1204 * @param flag [true] filtering enabled [false] filtering disabled
1209 1205 *
1210 1206 * @return void
1211 1207 *
1212 1208 */
1213 1209
1214 1210 float f_RW_min;
1215 1211 float f_RW_MAX;
1216 1212 float fi_min;
1217 1213 float fi_MAX;
1218 1214 float fi;
1219 1215 float deltaBelow;
1220 1216 float deltaAbove;
1221 1217 float freqToFilterOut;
1222 1218 int binBelow;
1223 1219 int binAbove;
1224 1220 int closestBin;
1225 1221 unsigned int whichByte;
1226 1222 int selectedByte;
1227 1223 int bin;
1228 1224 int binToRemove[NB_BINS_TO_REMOVE];
1229 1225 int k;
1230 1226 bool filteringSet;
1231 1227
1232 1228 closestBin = 0;
1233 1229 whichByte = 0;
1234 1230 bin = 0;
1235 1231 filteringSet = false;
1236 1232
1237 1233 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1238 1234 {
1239 1235 binToRemove[k] = -1;
1240 1236 }
1241 1237
1242 1238 if (!isnan(rw_f))
1243 1239 {
1244 1240 // compute the frequency range to filter [ rw_f - delta_f; rw_f + delta_f ]
1245 1241 f_RW_min = rw_f - ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
1246 1242 f_RW_MAX = rw_f + ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
1247 1243
1248 1244 freqToFilterOut = f_RW_min;
1249 1245 while ( filteringSet == false )
1250 1246 {
1251 1247 // compute the index of the frequency bin immediately below rw_f
1252 1248 binBelow = (int) ( floor( ((double) freqToFilterOut) / ((double) deltaFreq)) );
1253 1249 deltaBelow = freqToFilterOut - binBelow * deltaFreq;
1254 1250
1255 1251 // compute the index of the frequency bin immediately above rw_f
1256 1252 binAbove = (int) ( ceil( ((double) freqToFilterOut) / ((double) deltaFreq)) );
1257 1253 deltaAbove = binAbove * deltaFreq - freqToFilterOut;
1258 1254
1259 1255 // search the closest bin
1260 1256 if (deltaAbove > deltaBelow)
1261 1257 {
1262 1258 closestBin = binBelow;
1263 1259 }
1264 1260 else
1265 1261 {
1266 1262 closestBin = binAbove;
1267 1263 }
1268 1264
1269 1265 // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285]
1270 1266 fi = closestBin * deltaFreq;
1271 1267 fi_min = fi - (deltaFreq * FI_INTERVAL_COEFF);
1272 1268 fi_MAX = fi + (deltaFreq * FI_INTERVAL_COEFF);
1273 1269
1274 1270 //**************************************************************************************
1275 1271 // be careful here, one shall take into account that the bin 0 IS DROPPED in the spectra
1276 1272 // thus, the index 0 in a mask corresponds to the bin 1 of the spectrum
1277 1273 //**************************************************************************************
1278 1274
1279 1275 // 1. IF freqToFilterOut is included in [ fi_min; fi_MAX ]
1280 1276 // => remove f_(i), f_(i-1) and f_(i+1)
1281 1277 if ( ( freqToFilterOut > fi_min ) && ( freqToFilterOut < fi_MAX ) )
1282 1278 {
1283 1279 binToRemove[0] = (closestBin - 1) - 1;
1284 1280 binToRemove[1] = (closestBin) - 1;
1285 1281 binToRemove[2] = (closestBin + 1) - 1;
1286 1282 }
1287 1283 // 2. ELSE
1288 1284 // => remove the two f_(i) which are around f_RW
1289 1285 else
1290 1286 {
1291 1287 binToRemove[0] = (binBelow) - 1;
1292 1288 binToRemove[1] = (binAbove) - 1;
1293 1289 binToRemove[2] = (-1);
1294 1290 }
1295 1291
1296 1292 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1297 1293 {
1298 1294 bin = binToRemove[k];
1299 1295 if ( (bin >= BIN_MIN) && (bin <= BIN_MAX) )
1300 1296 {
1301 1297 whichByte = (bin >> SHIFT_3_BITS); // division by 8
1302 1298 selectedByte = ( 1 << (bin - (whichByte * BITS_PER_BYTE)) );
1303 1299 fbins_mask[BYTES_PER_MASK - 1 - whichByte] =
1304 1300 fbins_mask[BYTES_PER_MASK - 1 - whichByte] & ((unsigned char) (~selectedByte)); // bytes are ordered MSB first in the packets
1305 1301
1306 1302 }
1307 1303 }
1308 1304
1309 1305 // update freqToFilterOut
1310 1306 if ( freqToFilterOut == f_RW_MAX )
1311 1307 {
1312 1308 filteringSet = true; // end of the loop
1313 1309 }
1314 1310 else
1315 1311 {
1316 1312 freqToFilterOut = freqToFilterOut + deltaFreq;
1317 1313 }
1318 1314
1319 1315 if ( freqToFilterOut > f_RW_MAX)
1320 1316 {
1321 1317 freqToFilterOut = f_RW_MAX;
1322 1318 }
1323 1319 }
1324 1320 }
1325 1321 }
1326 1322
1327 1323 void build_sy_lfr_rw_mask( unsigned int channel )
1328 1324 {
1329 1325 unsigned char local_rw_fbins_mask[BYTES_PER_MASK];
1330 1326 unsigned char *maskPtr;
1331 1327 double deltaF;
1332 1328 unsigned k;
1333 1329
1334 1330 maskPtr = NULL;
1335 1331 deltaF = DELTAF_F2;
1336 1332
1337 1333 switch (channel)
1338 1334 {
1339 1335 case CHANNELF0:
1340 1336 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1341 1337 deltaF = DELTAF_F0;
1342 1338 break;
1343 1339 case CHANNELF1:
1344 1340 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1345 1341 deltaF = DELTAF_F1;
1346 1342 break;
1347 1343 case CHANNELF2:
1348 1344 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1349 1345 deltaF = DELTAF_F2;
1350 1346 break;
1351 1347 default:
1352 1348 break;
1353 1349 }
1354 1350
1355 1351 for (k = 0; k < BYTES_PER_MASK; k++)
1356 1352 {
1357 1353 local_rw_fbins_mask[k] = INT8_ALL_F;
1358 1354 }
1359 1355
1360 1356 // RW1
1361 1357 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, filterPar.sy_lfr_rw1_k1 );
1362 1358 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, filterPar.sy_lfr_rw1_k2 );
1363 1359 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f3, deltaF, filterPar.sy_lfr_rw1_k3 );
1364 1360 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f4, deltaF, filterPar.sy_lfr_rw1_k4 );
1365 1361
1366 1362 // RW2
1367 1363 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, filterPar.sy_lfr_rw2_k1 );
1368 1364 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, filterPar.sy_lfr_rw2_k2 );
1369 1365 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f3, deltaF, filterPar.sy_lfr_rw2_k3 );
1370 1366 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f4, deltaF, filterPar.sy_lfr_rw2_k4 );
1371 1367
1372 1368 // RW3
1373 1369 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, filterPar.sy_lfr_rw3_k1 );
1374 1370 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, filterPar.sy_lfr_rw3_k2 );
1375 1371 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f3, deltaF, filterPar.sy_lfr_rw3_k3 );
1376 1372 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f4, deltaF, filterPar.sy_lfr_rw3_k4 );
1377 1373
1378 1374 // RW4
1379 1375 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, filterPar.sy_lfr_rw4_k1 );
1380 1376 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, filterPar.sy_lfr_rw4_k2 );
1381 1377 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f3, deltaF, filterPar.sy_lfr_rw4_k3 );
1382 1378 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f4, deltaF, filterPar.sy_lfr_rw4_k4 );
1383 1379
1384 1380 // update the value of the fbins related to reaction wheels frequency filtering
1385 1381 if (maskPtr != NULL)
1386 1382 {
1387 1383 for (k = 0; k < BYTES_PER_MASK; k++)
1388 1384 {
1389 1385 maskPtr[k] = local_rw_fbins_mask[k];
1390 1386 }
1391 1387 }
1392 1388 }
1393 1389
1394 1390 void build_sy_lfr_rw_masks( void )
1395 1391 {
1396 1392 build_sy_lfr_rw_mask( CHANNELF0 );
1397 1393 build_sy_lfr_rw_mask( CHANNELF1 );
1398 1394 build_sy_lfr_rw_mask( CHANNELF2 );
1399 1395 }
1400 1396
1401 1397 void merge_fbins_masks( void )
1402 1398 {
1403 1399 unsigned char k;
1404 1400
1405 1401 unsigned char *fbins_f0;
1406 1402 unsigned char *fbins_f1;
1407 1403 unsigned char *fbins_f2;
1408 1404 unsigned char *rw_mask_f0;
1409 1405 unsigned char *rw_mask_f1;
1410 1406 unsigned char *rw_mask_f2;
1411 1407
1412 1408 fbins_f0 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1413 1409 fbins_f1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
1414 1410 fbins_f2 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
1415 1411 rw_mask_f0 = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1416 1412 rw_mask_f1 = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1417 1413 rw_mask_f2 = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1418 1414
1419 1415 for( k=0; k < BYTES_PER_MASK; k++ )
1420 1416 {
1421 1417 fbins_masks.merged_fbins_mask_f0[k] = fbins_f0[k] & rw_mask_f0[k];
1422 1418 fbins_masks.merged_fbins_mask_f1[k] = fbins_f1[k] & rw_mask_f1[k];
1423 1419 fbins_masks.merged_fbins_mask_f2[k] = fbins_f2[k] & rw_mask_f2[k];
1424 1420 }
1425 1421 }
1426 1422
1427 1423 //***********
1428 1424 // FBINS MASK
1429 1425
1430 1426 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
1431 1427 {
1432 1428 int status;
1433 1429 unsigned int k;
1434 1430 unsigned char *fbins_mask_dump;
1435 1431 unsigned char *fbins_mask_TC;
1436 1432
1437 1433 status = LFR_SUCCESSFUL;
1438 1434
1439 1435 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1440 1436 fbins_mask_TC = TC->dataAndCRC;
1441 1437
1442 1438 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1443 1439 {
1444 1440 fbins_mask_dump[k] = fbins_mask_TC[k];
1445 1441 }
1446 1442
1447 1443 return status;
1448 1444 }
1449 1445
1450 1446 //***************************
1451 1447 // TC_LFR_LOAD_PAS_FILTER_PAR
1452 1448
1453 1449 int check_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value )
1454 1450 {
1455 1451 float rw_k;
1456 1452 int ret;
1457 1453
1458 1454 ret = LFR_SUCCESSFUL;
1459 1455 rw_k = INIT_FLOAT;
1460 1456
1461 1457 copyFloatByChar( (unsigned char*) &rw_k, (unsigned char*) &TC->dataAndCRC[ offset ] );
1462 1458
1463 1459 *pos = offset;
1464 1460 *value = rw_k;
1465 1461
1466 1462 if (rw_k < MIN_SY_LFR_RW_F)
1467 1463 {
1468 1464 ret = WRONG_APP_DATA;
1469 1465 }
1470 1466
1471 1467 return ret;
1472 1468 }
1473 1469
1474 1470 int check_all_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int *pos, float *value )
1475 1471 {
1476 1472 int ret;
1477 1473
1478 1474 ret = LFR_SUCCESSFUL;
1479 1475
1480 1476 //****
1481 1477 //****
1482 1478 // RW1
1483 1479 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K1, pos, value ); // K1
1484 1480 if (ret == LFR_SUCCESSFUL) // K2
1485 1481 {
1486 1482 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K2, pos, value );
1487 1483 }
1488 1484 if (ret == LFR_SUCCESSFUL) // K3
1489 1485 {
1490 1486 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K3, pos, value );
1491 1487 }
1492 1488 if (ret == LFR_SUCCESSFUL) // K4
1493 1489 {
1494 1490 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K4, pos, value );
1495 1491 }
1496 1492
1497 1493 //****
1498 1494 //****
1499 1495 // RW2
1500 1496 if (ret == LFR_SUCCESSFUL) // K1
1501 1497 {
1502 1498 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K1, pos, value );
1503 1499 }
1504 1500 if (ret == LFR_SUCCESSFUL) // K2
1505 1501 {
1506 1502 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K2, pos, value );
1507 1503 }
1508 1504 if (ret == LFR_SUCCESSFUL) // K3
1509 1505 {
1510 1506 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K3, pos, value );
1511 1507 }
1512 1508 if (ret == LFR_SUCCESSFUL) // K4
1513 1509 {
1514 1510 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K4, pos, value );
1515 1511 }
1516 1512
1517 1513 //****
1518 1514 //****
1519 1515 // RW3
1520 1516 if (ret == LFR_SUCCESSFUL) // K1
1521 1517 {
1522 1518 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K1, pos, value );
1523 1519 }
1524 1520 if (ret == LFR_SUCCESSFUL) // K2
1525 1521 {
1526 1522 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K2, pos, value );
1527 1523 }
1528 1524 if (ret == LFR_SUCCESSFUL) // K3
1529 1525 {
1530 1526 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K3, pos, value );
1531 1527 }
1532 1528 if (ret == LFR_SUCCESSFUL) // K4
1533 1529 {
1534 1530 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K4, pos, value );
1535 1531 }
1536 1532
1537 1533 //****
1538 1534 //****
1539 1535 // RW4
1540 1536 if (ret == LFR_SUCCESSFUL) // K1
1541 1537 {
1542 1538 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K1, pos, value );
1543 1539 }
1544 1540 if (ret == LFR_SUCCESSFUL) // K2
1545 1541 {
1546 1542 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K2, pos, value );
1547 1543 }
1548 1544 if (ret == LFR_SUCCESSFUL) // K3
1549 1545 {
1550 1546 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K3, pos, value );
1551 1547 }
1552 1548 if (ret == LFR_SUCCESSFUL) // K4
1553 1549 {
1554 1550 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K4, pos, value );
1555 1551 }
1556 1552
1557 1553 return ret;
1558 1554 }
1559 1555
1560 1556 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
1561 1557 {
1562 1558 int flag;
1563 1559 rtems_status_code status;
1564 1560
1565 1561 unsigned char sy_lfr_pas_filter_enabled;
1566 1562 unsigned char sy_lfr_pas_filter_modulus;
1567 1563 float sy_lfr_pas_filter_tbad;
1568 1564 unsigned char sy_lfr_pas_filter_offset;
1569 1565 float sy_lfr_pas_filter_shift;
1570 1566 float sy_lfr_sc_rw_delta_f;
1571 1567 char *parPtr;
1572 1568 int datafield_pos;
1573 1569 float rw_k;
1574 1570
1575 1571 flag = LFR_SUCCESSFUL;
1576 1572 sy_lfr_pas_filter_tbad = INIT_FLOAT;
1577 1573 sy_lfr_pas_filter_shift = INIT_FLOAT;
1578 1574 sy_lfr_sc_rw_delta_f = INIT_FLOAT;
1579 1575 parPtr = NULL;
1580 1576 datafield_pos = INIT_INT;
1581 1577 rw_k = INIT_FLOAT;
1582 1578
1583 1579 //***************
1584 1580 // get parameters
1585 1581 sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & BIT_PAS_FILTER_ENABLED; // [0000 0001]
1586 1582 sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
1587 1583 copyFloatByChar(
1588 1584 (unsigned char*) &sy_lfr_pas_filter_tbad,
1589 1585 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD ]
1590 1586 );
1591 1587 sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
1592 1588 copyFloatByChar(
1593 1589 (unsigned char*) &sy_lfr_pas_filter_shift,
1594 1590 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT ]
1595 1591 );
1596 1592 copyFloatByChar(
1597 1593 (unsigned char*) &sy_lfr_sc_rw_delta_f,
1598 1594 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F ]
1599 1595 );
1600 1596
1601 1597 //******************
1602 1598 // CHECK CONSISTENCY
1603 1599
1604 1600 //**************************
1605 1601 // sy_lfr_pas_filter_enabled
1606 1602 // nothing to check, value is 0 or 1
1607 1603
1608 1604 //**************************
1609 1605 // sy_lfr_pas_filter_modulus
1610 1606 if ( (sy_lfr_pas_filter_modulus < MIN_PAS_FILTER_MODULUS) || (sy_lfr_pas_filter_modulus > MAX_PAS_FILTER_MODULUS) )
1611 1607 {
1612 1608 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1613 1609 flag = WRONG_APP_DATA;
1614 1610 }
1615 1611
1616 1612 //***********************
1617 1613 // sy_lfr_pas_filter_tbad
1618 1614 if (flag == LFR_SUCCESSFUL)
1619 1615 {
1620 1616 if ( (sy_lfr_pas_filter_tbad < MIN_PAS_FILTER_TBAD) || (sy_lfr_pas_filter_tbad > MAX_PAS_FILTER_TBAD) )
1621 1617 {
1622 1618 parPtr = (char*) &sy_lfr_pas_filter_tbad;
1623 1619 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1624 1620 flag = WRONG_APP_DATA;
1625 1621 }
1626 1622 }
1627 1623
1628 1624 //*************************
1629 1625 // sy_lfr_pas_filter_offset
1630 1626 if (flag == LFR_SUCCESSFUL)
1631 1627 {
1632 1628 if ( (sy_lfr_pas_filter_offset < MIN_PAS_FILTER_OFFSET) || (sy_lfr_pas_filter_offset > MAX_PAS_FILTER_OFFSET) )
1633 1629 {
1634 1630 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET + DATAFIELD_OFFSET, sy_lfr_pas_filter_offset );
1635 1631 flag = WRONG_APP_DATA;
1636 1632 }
1637 1633 }
1638 1634
1639 1635 //************************
1640 1636 // sy_lfr_pas_filter_shift
1641 1637 if (flag == LFR_SUCCESSFUL)
1642 1638 {
1643 1639 if ( (sy_lfr_pas_filter_shift < MIN_PAS_FILTER_SHIFT) || (sy_lfr_pas_filter_shift > MAX_PAS_FILTER_SHIFT) )
1644 1640 {
1645 1641 parPtr = (char*) &sy_lfr_pas_filter_shift;
1646 1642 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1647 1643 flag = WRONG_APP_DATA;
1648 1644 }
1649 1645 }
1650 1646
1651 1647 //*************************************
1652 1648 // check global coherency of the values
1653 1649 if (flag == LFR_SUCCESSFUL)
1654 1650 {
1655 1651 if ( (sy_lfr_pas_filter_offset + sy_lfr_pas_filter_shift) >= sy_lfr_pas_filter_modulus )
1656 1652 {
1657 1653 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1658 1654 flag = WRONG_APP_DATA;
1659 1655 }
1660 1656 }
1661 1657
1662 1658 //*********************
1663 1659 // sy_lfr_sc_rw_delta_f
1664 1660 if (flag == LFR_SUCCESSFUL)
1665 1661 {
1666 1662 if ( sy_lfr_sc_rw_delta_f < MIN_SY_LFR_SC_RW_DELTA_F )
1667 1663 {
1668 1664 parPtr = (char*) &sy_lfr_sc_rw_delta_f;
1669 1665 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1670 1666 flag = WRONG_APP_DATA;
1671 1667 }
1672 1668 }
1673 1669
1674 1670 //************
1675 1671 // sy_lfr_rw_k
1676 1672 if (flag == LFR_SUCCESSFUL)
1677 1673 {
1678 1674 flag = check_all_sy_lfr_rw_k( TC, &datafield_pos, &rw_k );
1679 1675 if (flag != LFR_SUCCESSFUL)
1680 1676 {
1681 1677 parPtr = (char*) &sy_lfr_pas_filter_shift;
1682 1678 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, datafield_pos + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1683 1679 }
1684 1680 }
1685 1681
1686 1682 return flag;
1687 1683 }
1688 1684
1689 1685 //**************
1690 1686 // KCOEFFICIENTS
1691 1687 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC,rtems_id queue_id )
1692 1688 {
1693 1689 unsigned int kcoeff;
1694 1690 unsigned short sy_lfr_kcoeff_frequency;
1695 1691 unsigned short bin;
1696 1692 float *kcoeffPtr_norm;
1697 1693 float *kcoeffPtr_sbm;
1698 1694 int status;
1699 1695 unsigned char *kcoeffLoadPtr;
1700 1696 unsigned char *kcoeffNormPtr;
1701 1697 unsigned char *kcoeffSbmPtr_a;
1702 1698 unsigned char *kcoeffSbmPtr_b;
1703 1699
1704 1700 sy_lfr_kcoeff_frequency = 0;
1705 1701 bin = 0;
1706 1702 kcoeffPtr_norm = NULL;
1707 1703 kcoeffPtr_sbm = NULL;
1708 1704 status = LFR_SUCCESSFUL;
1709 1705
1710 1706 // copy the value of the frequency byte by byte DO NOT USE A SHORT* POINTER
1711 1707 copyInt16ByChar( (unsigned char*) &sy_lfr_kcoeff_frequency, &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY] );
1712 1708
1713 1709
1714 1710 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
1715 1711 {
1716 1712 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
1717 1713 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + DATAFIELD_OFFSET,
1718 1714 TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 1] ); // +1 to get the LSB instead of the MSB
1719 1715 status = LFR_DEFAULT;
1720 1716 }
1721 1717 else
1722 1718 {
1723 1719 if ( ( sy_lfr_kcoeff_frequency >= 0 )
1724 1720 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
1725 1721 {
1726 1722 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
1727 1723 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
1728 1724 bin = sy_lfr_kcoeff_frequency;
1729 1725 }
1730 1726 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
1731 1727 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
1732 1728 {
1733 1729 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
1734 1730 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
1735 1731 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
1736 1732 }
1737 1733 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
1738 1734 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
1739 1735 {
1740 1736 kcoeffPtr_norm = k_coeff_intercalib_f2;
1741 1737 kcoeffPtr_sbm = NULL;
1742 1738 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
1743 1739 }
1744 1740 }
1745 1741
1746 1742 if (kcoeffPtr_norm != NULL ) // update K coefficient for NORMAL data products
1747 1743 {
1748 1744 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1749 1745 {
1750 1746 // destination
1751 1747 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + kcoeff ];
1752 1748 // source
1753 1749 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1754 1750 // copy source to destination
1755 1751 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
1756 1752 }
1757 1753 }
1758 1754
1759 1755 if (kcoeffPtr_sbm != NULL ) // update K coefficient for SBM data products
1760 1756 {
1761 1757 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1762 1758 {
1763 1759 // destination
1764 1760 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ];
1765 1761 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ (((bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_KCOEFF_PER_NORM_KCOEFF) + 1 ];
1766 1762 // source
1767 1763 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1768 1764 // copy source to destination
1769 1765 copyFloatByChar( kcoeffSbmPtr_a, kcoeffLoadPtr );
1770 1766 copyFloatByChar( kcoeffSbmPtr_b, kcoeffLoadPtr );
1771 1767 }
1772 1768 }
1773 1769
1774 1770 // print_k_coeff();
1775 1771
1776 1772 return status;
1777 1773 }
1778 1774
1779 1775 void copyFloatByChar( unsigned char *destination, unsigned char *source )
1780 1776 {
1781 1777 destination[BYTE_0] = source[BYTE_0];
1782 1778 destination[BYTE_1] = source[BYTE_1];
1783 1779 destination[BYTE_2] = source[BYTE_2];
1784 1780 destination[BYTE_3] = source[BYTE_3];
1785 1781 }
1786 1782
1787 1783 void copyInt32ByChar( unsigned char *destination, unsigned char *source )
1788 1784 {
1789 1785 destination[BYTE_0] = source[BYTE_0];
1790 1786 destination[BYTE_1] = source[BYTE_1];
1791 1787 destination[BYTE_2] = source[BYTE_2];
1792 1788 destination[BYTE_3] = source[BYTE_3];
1793 1789 }
1794 1790
1795 1791 void copyInt16ByChar( unsigned char *destination, unsigned char *source )
1796 1792 {
1797 1793 destination[BYTE_0] = source[BYTE_0];
1798 1794 destination[BYTE_1] = source[BYTE_1];
1799 1795 }
1800 1796
1801 1797 void floatToChar( float value, unsigned char* ptr)
1802 1798 {
1803 1799 unsigned char* valuePtr;
1804 1800
1805 1801 valuePtr = (unsigned char*) &value;
1806 1802
1807 1803 ptr[BYTE_0] = valuePtr[BYTE_0];
1808 1804 ptr[BYTE_1] = valuePtr[BYTE_1];
1809 1805 ptr[BYTE_2] = valuePtr[BYTE_2];
1810 1806 ptr[BYTE_3] = valuePtr[BYTE_3];
1811 1807 }
1812 1808
1813 1809 //**********
1814 1810 // init dump
1815 1811
1816 1812 void init_parameter_dump( void )
1817 1813 {
1818 1814 /** This function initialize the parameter_dump_packet global variable with default values.
1819 1815 *
1820 1816 */
1821 1817
1822 1818 unsigned int k;
1823 1819
1824 1820 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
1825 1821 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
1826 1822 parameter_dump_packet.reserved = CCSDS_RESERVED;
1827 1823 parameter_dump_packet.userApplication = CCSDS_USER_APP;
1828 1824 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1829 1825 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1830 1826 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1831 1827 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1832 1828 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> SHIFT_1_BYTE);
1833 1829 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
1834 1830 // DATA FIELD HEADER
1835 1831 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1836 1832 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
1837 1833 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
1838 1834 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
1839 1835 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
1840 1836 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
1841 1837 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
1842 1838 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
1843 1839 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
1844 1840 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
1845 1841 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
1846 1842
1847 1843 //******************
1848 1844 // COMMON PARAMETERS
1849 1845 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
1850 1846 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
1851 1847
1852 1848 //******************
1853 1849 // NORMAL PARAMETERS
1854 1850 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> SHIFT_1_BYTE);
1855 1851 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1856 1852 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> SHIFT_1_BYTE);
1857 1853 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1858 1854 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> SHIFT_1_BYTE);
1859 1855 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1860 1856 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1861 1857 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1862 1858 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1863 1859
1864 1860 //*****************
1865 1861 // BURST PARAMETERS
1866 1862 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1867 1863 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1868 1864
1869 1865 //****************
1870 1866 // SBM1 PARAMETERS
1871 1867 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
1872 1868 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1873 1869
1874 1870 //****************
1875 1871 // SBM2 PARAMETERS
1876 1872 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1877 1873 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1878 1874
1879 1875 //************
1880 1876 // FBINS MASKS
1881 1877 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1882 1878 {
1883 1879 parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = INT8_ALL_F;
1884 1880 }
1885 1881
1886 1882 // PAS FILTER PARAMETERS
1887 1883 parameter_dump_packet.pa_rpw_spare8_2 = INIT_CHAR;
1888 1884 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = INIT_CHAR;
1889 1885 parameter_dump_packet.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
1890 1886 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_TBAD, parameter_dump_packet.sy_lfr_pas_filter_tbad );
1891 1887 parameter_dump_packet.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
1892 1888 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift );
1893 1889 floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f );
1894 1890
1895 1891 // RW1_K
1896 1892 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw1_k1);
1897 1893 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw1_k2);
1898 1894 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw1_k3);
1899 1895 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw1_k4);
1900 1896 // RW2_K
1901 1897 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw2_k1);
1902 1898 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw2_k2);
1903 1899 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw2_k3);
1904 1900 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw2_k4);
1905 1901 // RW3_K
1906 1902 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw3_k1);
1907 1903 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw3_k2);
1908 1904 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw3_k3);
1909 1905 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw3_k4);
1910 1906 // RW4_K
1911 1907 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw4_k1);
1912 1908 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw4_k2);
1913 1909 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw4_k3);
1914 1910 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw4_k4);
1915 1911
1916 1912 // LFR_RW_MASK
1917 1913 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1918 1914 {
1919 1915 parameter_dump_packet.sy_lfr_rw_mask_f0_word1[k] = INT8_ALL_F;
1920 1916 }
1921 1917
1922 1918 // once the reaction wheels masks have been initialized, they have to be merged with the fbins masks
1923 1919 merge_fbins_masks();
1924 1920 }
1925 1921
1926 1922 void init_kcoefficients_dump( void )
1927 1923 {
1928 1924 init_kcoefficients_dump_packet( &kcoefficients_dump_1, PKTNR_1, KCOEFF_BLK_NR_PKT1 );
1929 1925 init_kcoefficients_dump_packet( &kcoefficients_dump_2, PKTNR_2, KCOEFF_BLK_NR_PKT2 );
1930 1926
1931 1927 kcoefficient_node_1.previous = NULL;
1932 1928 kcoefficient_node_1.next = NULL;
1933 1929 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1934 1930 kcoefficient_node_1.coarseTime = INIT_CHAR;
1935 1931 kcoefficient_node_1.fineTime = INIT_CHAR;
1936 1932 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1937 1933 kcoefficient_node_1.status = INIT_CHAR;
1938 1934
1939 1935 kcoefficient_node_2.previous = NULL;
1940 1936 kcoefficient_node_2.next = NULL;
1941 1937 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1942 1938 kcoefficient_node_2.coarseTime = INIT_CHAR;
1943 1939 kcoefficient_node_2.fineTime = INIT_CHAR;
1944 1940 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1945 1941 kcoefficient_node_2.status = INIT_CHAR;
1946 1942 }
1947 1943
1948 1944 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1949 1945 {
1950 1946 unsigned int k;
1951 1947 unsigned int packetLength;
1952 1948
1953 1949 packetLength =
1954 1950 ((blk_nr * KCOEFF_BLK_SIZE) + BYTE_POS_KCOEFFICIENTS_PARAMETES) - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1955 1951
1956 1952 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1957 1953 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1958 1954 kcoefficients_dump->reserved = CCSDS_RESERVED;
1959 1955 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1960 1956 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1961 1957 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1962 1958 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1963 1959 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1964 1960 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
1965 1961 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1966 1962 // DATA FIELD HEADER
1967 1963 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1968 1964 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1969 1965 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1970 1966 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1971 1967 kcoefficients_dump->time[BYTE_0] = INIT_CHAR;
1972 1968 kcoefficients_dump->time[BYTE_1] = INIT_CHAR;
1973 1969 kcoefficients_dump->time[BYTE_2] = INIT_CHAR;
1974 1970 kcoefficients_dump->time[BYTE_3] = INIT_CHAR;
1975 1971 kcoefficients_dump->time[BYTE_4] = INIT_CHAR;
1976 1972 kcoefficients_dump->time[BYTE_5] = INIT_CHAR;
1977 1973 kcoefficients_dump->sid = SID_K_DUMP;
1978 1974
1979 1975 kcoefficients_dump->pkt_cnt = KCOEFF_PKTCNT;
1980 1976 kcoefficients_dump->pkt_nr = PKTNR_1;
1981 1977 kcoefficients_dump->blk_nr = blk_nr;
1982 1978
1983 1979 //******************
1984 1980 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1985 1981 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1986 1982 for (k=0; k<(KCOEFF_BLK_NR_PKT1 * KCOEFF_BLK_SIZE); k++)
1987 1983 {
1988 1984 kcoefficients_dump->kcoeff_blks[k] = INIT_CHAR;
1989 1985 }
1990 1986 }
1991 1987
1992 1988 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id )
1993 1989 {
1994 1990 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
1995 1991 *
1996 1992 * @param packet_sequence_control points to the packet sequence control which will be incremented
1997 1993 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
1998 1994 *
1999 1995 * If the destination ID is not known, a dedicated counter is incremented.
2000 1996 *
2001 1997 */
2002 1998
2003 1999 unsigned short sequence_cnt;
2004 2000 unsigned short segmentation_grouping_flag;
2005 2001 unsigned short new_packet_sequence_control;
2006 2002 unsigned char i;
2007 2003
2008 2004 switch (destination_id)
2009 2005 {
2010 2006 case SID_TC_GROUND:
2011 2007 i = GROUND;
2012 2008 break;
2013 2009 case SID_TC_MISSION_TIMELINE:
2014 2010 i = MISSION_TIMELINE;
2015 2011 break;
2016 2012 case SID_TC_TC_SEQUENCES:
2017 2013 i = TC_SEQUENCES;
2018 2014 break;
2019 2015 case SID_TC_RECOVERY_ACTION_CMD:
2020 2016 i = RECOVERY_ACTION_CMD;
2021 2017 break;
2022 2018 case SID_TC_BACKUP_MISSION_TIMELINE:
2023 2019 i = BACKUP_MISSION_TIMELINE;
2024 2020 break;
2025 2021 case SID_TC_DIRECT_CMD:
2026 2022 i = DIRECT_CMD;
2027 2023 break;
2028 2024 case SID_TC_SPARE_GRD_SRC1:
2029 2025 i = SPARE_GRD_SRC1;
2030 2026 break;
2031 2027 case SID_TC_SPARE_GRD_SRC2:
2032 2028 i = SPARE_GRD_SRC2;
2033 2029 break;
2034 2030 case SID_TC_OBCP:
2035 2031 i = OBCP;
2036 2032 break;
2037 2033 case SID_TC_SYSTEM_CONTROL:
2038 2034 i = SYSTEM_CONTROL;
2039 2035 break;
2040 2036 case SID_TC_AOCS:
2041 2037 i = AOCS;
2042 2038 break;
2043 2039 case SID_TC_RPW_INTERNAL:
2044 2040 i = RPW_INTERNAL;
2045 2041 break;
2046 2042 default:
2047 2043 i = GROUND;
2048 2044 break;
2049 2045 }
2050 2046
2051 2047 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
2052 2048 sequence_cnt = sequenceCounters_TM_DUMP[ i ] & SEQ_CNT_MASK;
2053 2049
2054 2050 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
2055 2051
2056 2052 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> SHIFT_1_BYTE);
2057 2053 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
2058 2054
2059 2055 // increment the sequence counter
2060 2056 if ( sequenceCounters_TM_DUMP[ i ] < SEQ_CNT_MAX )
2061 2057 {
2062 2058 sequenceCounters_TM_DUMP[ i ] = sequenceCounters_TM_DUMP[ i ] + 1;
2063 2059 }
2064 2060 else
2065 2061 {
2066 2062 sequenceCounters_TM_DUMP[ i ] = 0;
2067 2063 }
2068 2064 }
General Comments 0
You need to be logged in to leave comments. Login now