##// END OF EJS Templates
3.1.0.2...
paul -
r293:e6dce572ae0e R3_plus draft
parent child
Show More
@@ -1,2 +1,2
1 1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
2 2450d4935652a4d0370245cc7fc60a4c51e6fc9b header/lfr_common_headers
2 c378fa14eadd80b3b873ca7c8f9f387893c07692 header/lfr_common_headers
@@ -1,123 +1,123
1 1 TEMPLATE = app
2 2 # CONFIG += console v8 sim
3 3 # CONFIG options =
4 4 # verbose
5 5 # boot_messages
6 6 # debug_messages
7 7 # cpu_usage_report
8 8 # stack_report
9 9 # vhdl_dev
10 10 # debug_tch
11 11 # lpp_dpu_destid /!\ REMOVE BEFORE DELIVERY TO LESIA /!\
12 12 # debug_watchdog
13 13 CONFIG += console verbose lpp_dpu_destid cpu_usage_report
14 14 CONFIG -= qt
15 15
16 16 include(./sparc.pri)
17 17
18 18 # flight software version
19 19 SWVERSION=-1-0
20 20 DEFINES += SW_VERSION_N1=3 # major
21 21 DEFINES += SW_VERSION_N2=1 # minor
22 22 DEFINES += SW_VERSION_N3=0 # patch
23 DEFINES += SW_VERSION_N4=1 # internal
23 DEFINES += SW_VERSION_N4=2 # internal
24 24
25 25 # <GCOV>
26 26 #QMAKE_CFLAGS_RELEASE += -fprofile-arcs -ftest-coverage
27 27 #LIBS += -lgcov /opt/GCOV/01A/lib/overload.o -lc
28 28 # </GCOV>
29 29
30 30 # <CHANGE BEFORE FLIGHT>
31 31 contains( CONFIG, lpp_dpu_destid ) {
32 32 DEFINES += LPP_DPU_DESTID
33 33 }
34 34 # </CHANGE BEFORE FLIGHT>
35 35
36 36 contains( CONFIG, debug_tch ) {
37 37 DEFINES += DEBUG_TCH
38 38 }
39 39 DEFINES += MSB_FIRST_TCH
40 40
41 41 contains( CONFIG, vhdl_dev ) {
42 42 DEFINES += VHDL_DEV
43 43 }
44 44
45 45 contains( CONFIG, verbose ) {
46 46 DEFINES += PRINT_MESSAGES_ON_CONSOLE
47 47 }
48 48
49 49 contains( CONFIG, debug_messages ) {
50 50 DEFINES += DEBUG_MESSAGES
51 51 }
52 52
53 53 contains( CONFIG, cpu_usage_report ) {
54 54 DEFINES += PRINT_TASK_STATISTICS
55 55 }
56 56
57 57 contains( CONFIG, stack_report ) {
58 58 DEFINES += PRINT_STACK_REPORT
59 59 }
60 60
61 61 contains( CONFIG, boot_messages ) {
62 62 DEFINES += BOOT_MESSAGES
63 63 }
64 64
65 65 contains( CONFIG, debug_watchdog ) {
66 66 DEFINES += DEBUG_WATCHDOG
67 67 }
68 68
69 69 #doxygen.target = doxygen
70 70 #doxygen.commands = doxygen ../doc/Doxyfile
71 71 #QMAKE_EXTRA_TARGETS += doxygen
72 72
73 73 TARGET = fsw
74 74
75 75 INCLUDEPATH += \
76 76 $${PWD}/../src \
77 77 $${PWD}/../header \
78 78 $${PWD}/../header/lfr_common_headers \
79 79 $${PWD}/../header/processing \
80 80 $${PWD}/../LFR_basic-parameters
81 81
82 82 SOURCES += \
83 83 ../src/wf_handler.c \
84 84 ../src/tc_handler.c \
85 85 ../src/fsw_misc.c \
86 86 ../src/fsw_init.c \
87 87 ../src/fsw_globals.c \
88 88 ../src/fsw_spacewire.c \
89 89 ../src/tc_load_dump_parameters.c \
90 90 ../src/tm_lfr_tc_exe.c \
91 91 ../src/tc_acceptance.c \
92 92 ../src/processing/fsw_processing.c \
93 93 ../src/processing/avf0_prc0.c \
94 94 ../src/processing/avf1_prc1.c \
95 95 ../src/processing/avf2_prc2.c \
96 96 ../src/lfr_cpu_usage_report.c \
97 97 ../LFR_basic-parameters/basic_parameters.c
98 98
99 99 HEADERS += \
100 100 ../header/wf_handler.h \
101 101 ../header/tc_handler.h \
102 102 ../header/grlib_regs.h \
103 103 ../header/fsw_misc.h \
104 104 ../header/fsw_init.h \
105 105 ../header/fsw_spacewire.h \
106 106 ../header/tc_load_dump_parameters.h \
107 107 ../header/tm_lfr_tc_exe.h \
108 108 ../header/tc_acceptance.h \
109 109 ../header/processing/fsw_processing.h \
110 110 ../header/processing/avf0_prc0.h \
111 111 ../header/processing/avf1_prc1.h \
112 112 ../header/processing/avf2_prc2.h \
113 113 ../header/fsw_params_wf_handler.h \
114 114 ../header/lfr_cpu_usage_report.h \
115 115 ../header/lfr_common_headers/ccsds_types.h \
116 116 ../header/lfr_common_headers/fsw_params.h \
117 117 ../header/lfr_common_headers/fsw_params_nb_bytes.h \
118 118 ../header/lfr_common_headers/fsw_params_processing.h \
119 119 ../header/lfr_common_headers/tm_byte_positions.h \
120 120 ../LFR_basic-parameters/basic_parameters.h \
121 121 ../LFR_basic-parameters/basic_parameters_params.h \
122 122 ../header/GscMemoryLPP.hpp
123 123
@@ -1,64 +1,64
1 1 #ifndef FSW_INIT_H_INCLUDED
2 2 #define FSW_INIT_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <leon.h>
6 6
7 7 #include "fsw_params.h"
8 8 #include "fsw_misc.h"
9 9 #include "fsw_processing.h"
10 10
11 11 #include "tc_handler.h"
12 12 #include "wf_handler.h"
13 13 #include "fsw_spacewire.h"
14 14
15 15 #include "avf0_prc0.h"
16 16 #include "avf1_prc1.h"
17 17 #include "avf2_prc2.h"
18 18
19 19 extern rtems_name Task_name[20]; /* array of task names */
20 20 extern rtems_id Task_id[20]; /* array of task ids */
21 21 extern rtems_name timecode_timer_name;
22 22 extern rtems_id timecode_timer_id;
23 23 extern unsigned char pa_bia_status_info;
24 24 extern unsigned char cp_rpw_sc_rw_f_flags;
25 25 extern float cp_rpw_sc_rw1_f1;
26 26 extern float cp_rpw_sc_rw1_f2;
27 27 extern float cp_rpw_sc_rw2_f1;
28 28 extern float cp_rpw_sc_rw2_f2;
29 29 extern float cp_rpw_sc_rw3_f1;
30 30 extern float cp_rpw_sc_rw3_f2;
31 31 extern float cp_rpw_sc_rw4_f1;
32 32 extern float cp_rpw_sc_rw4_f2;
33 extern float sy_lfr_sc_rw_delta_f;
33 extern filterPar_t filterPar;
34 34
35 35 // RTEMS TASKS
36 36 rtems_task Init( rtems_task_argument argument);
37 37
38 38 // OTHER functions
39 39 void create_names( void );
40 40 int create_all_tasks( void );
41 41 int start_all_tasks( void );
42 42 //
43 43 rtems_status_code create_message_queues( void );
44 44 rtems_status_code create_timecode_timer( void );
45 45 rtems_status_code get_message_queue_id_send( rtems_id *queue_id );
46 46 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id );
47 47 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id );
48 48 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
49 49 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
50 50 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max );
51 51 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize );
52 52 //
53 53 int start_recv_send_tasks( void );
54 54 //
55 55 void init_local_mode_parameters( void );
56 56 void reset_local_time( void );
57 57
58 58 extern void rtems_cpu_usage_report( void );
59 59 extern void rtems_cpu_usage_reset( void );
60 60 extern void rtems_stack_checker_report_usage( void );
61 61
62 62 extern int sched_yield( void );
63 63
64 64 #endif // FSW_INIT_H_INCLUDED
@@ -1,96 +1,98
1 1 /** Global variables of the LFR flight software.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * Among global variables, there are:
7 7 * - RTEMS names and id.
8 8 * - APB configuration registers.
9 9 * - waveforms global buffers, used by the waveform picker hardware module to store data.
10 10 * - spectral matrices buffesr, used by the hardware module to store data.
11 11 * - variable related to LFR modes parameters.
12 12 * - the global HK packet buffer.
13 13 * - the global dump parameter buffer.
14 14 *
15 15 */
16 16
17 17 #include <rtems.h>
18 18 #include <grspw.h>
19 19
20 20 #include "ccsds_types.h"
21 21 #include "grlib_regs.h"
22 22 #include "fsw_params.h"
23 23 #include "fsw_params_wf_handler.h"
24 24
25 25 // RTEMS GLOBAL VARIABLES
26 26 rtems_name misc_name[5];
27 27 rtems_name Task_name[20]; /* array of task names */
28 28 rtems_id Task_id[20]; /* array of task ids */
29 29 rtems_name timecode_timer_name;
30 30 rtems_id timecode_timer_id;
31 31 int fdSPW = 0;
32 32 int fdUART = 0;
33 33 unsigned char lfrCurrentMode;
34 34 unsigned char pa_bia_status_info;
35 35 unsigned char thisIsAnASMRestart = 0;
36 36 unsigned char oneTcLfrUpdateTimeReceived = 0;
37 37
38 38 // WAVEFORMS GLOBAL VARIABLES // 2048 * 3 * 4 + 2 * 4 = 24576 + 8 bytes = 24584
39 39 // 97 * 256 = 24832 => delta = 248 bytes = 62 words
40 40 // WAVEFORMS GLOBAL VARIABLES // 2688 * 3 * 4 + 2 * 4 = 32256 + 8 bytes = 32264
41 41 // 127 * 256 = 32512 => delta = 248 bytes = 62 words
42 42 // F0 F1 F2 F3
43 43 volatile int wf_buffer_f0[ NB_RING_NODES_F0 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
44 44 volatile int wf_buffer_f1[ NB_RING_NODES_F1 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
45 45 volatile int wf_buffer_f2[ NB_RING_NODES_F2 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
46 46 volatile int wf_buffer_f3[ NB_RING_NODES_F3 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
47 47
48 48 //***********************************
49 49 // SPECTRAL MATRICES GLOBAL VARIABLES
50 50
51 51 // alignment constraints for the spectral matrices buffers => the first data after the time (8 bytes) shall be aligned on 0x00
52 52 volatile int sm_f0[ NB_RING_NODES_SM_F0 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
53 53 volatile int sm_f1[ NB_RING_NODES_SM_F1 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
54 54 volatile int sm_f2[ NB_RING_NODES_SM_F2 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
55 55
56 56 // APB CONFIGURATION REGISTERS
57 57 time_management_regs_t *time_management_regs = (time_management_regs_t*) REGS_ADDR_TIME_MANAGEMENT;
58 58 gptimer_regs_t *gptimer_regs = (gptimer_regs_t *) REGS_ADDR_GPTIMER;
59 59 waveform_picker_regs_0_1_18_t *waveform_picker_regs = (waveform_picker_regs_0_1_18_t*) REGS_ADDR_WAVEFORM_PICKER;
60 60 spectral_matrix_regs_t *spectral_matrix_regs = (spectral_matrix_regs_t*) REGS_ADDR_SPECTRAL_MATRIX;
61 61
62 62 // MODE PARAMETERS
63 63 Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
64 64 struct param_local_str param_local;
65 65 unsigned int lastValidEnterModeTime;
66 66
67 67 // HK PACKETS
68 68 Packet_TM_LFR_HK_t housekeeping_packet;
69 69 unsigned char cp_rpw_sc_rw_f_flags;
70 70 // message queues occupancy
71 71 unsigned char hk_lfr_q_sd_fifo_size_max;
72 72 unsigned char hk_lfr_q_rv_fifo_size_max;
73 73 unsigned char hk_lfr_q_p0_fifo_size_max;
74 74 unsigned char hk_lfr_q_p1_fifo_size_max;
75 75 unsigned char hk_lfr_q_p2_fifo_size_max;
76 76 // sequence counters are incremented by APID (PID + CAT) and destination ID
77 77 unsigned short sequenceCounters_SCIENCE_NORMAL_BURST;
78 78 unsigned short sequenceCounters_SCIENCE_SBM1_SBM2;
79 79 unsigned short sequenceCounters_TC_EXE[SEQ_CNT_NB_DEST_ID];
80 80 unsigned short sequenceCounters_TM_DUMP[SEQ_CNT_NB_DEST_ID];
81 81 unsigned short sequenceCounterHK;
82 82 spw_stats grspw_stats;
83 83
84 84 // TC_LFR_UPDATE_INFO
85 85 float cp_rpw_sc_rw1_f1;
86 86 float cp_rpw_sc_rw1_f2;
87 87 float cp_rpw_sc_rw2_f1;
88 88 float cp_rpw_sc_rw2_f2;
89 89 float cp_rpw_sc_rw3_f1;
90 90 float cp_rpw_sc_rw3_f2;
91 91 float cp_rpw_sc_rw4_f1;
92 92 float cp_rpw_sc_rw4_f2;
93 float sy_lfr_sc_rw_delta_f;
93
94 // TC_LFR_LOAD_FILTER_PAR
95 filterPar_t filterPar;
94 96
95 97 fbins_masks_t fbins_masks;
96 98 unsigned int acquisitionDurations[3] = {ACQUISITION_DURATION_F0, ACQUISITION_DURATION_F1, ACQUISITION_DURATION_F2};
@@ -1,928 +1,934
1 1 /** This is the RTEMS initialization module.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * This module contains two very different information:
7 7 * - specific instructions to configure the compilation of the RTEMS executive
8 8 * - functions related to the fligth softwre initialization, especially the INIT RTEMS task
9 9 *
10 10 */
11 11
12 12 //*************************
13 13 // GPL reminder to be added
14 14 //*************************
15 15
16 16 #include <rtems.h>
17 17
18 18 /* configuration information */
19 19
20 20 #define CONFIGURE_INIT
21 21
22 22 #include <bsp.h> /* for device driver prototypes */
23 23
24 24 /* configuration information */
25 25
26 26 #define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
27 27 #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
28 28
29 29 #define CONFIGURE_MAXIMUM_TASKS 20
30 30 #define CONFIGURE_RTEMS_INIT_TASKS_TABLE
31 31 #define CONFIGURE_EXTRA_TASK_STACKS (3 * RTEMS_MINIMUM_STACK_SIZE)
32 32 #define CONFIGURE_LIBIO_MAXIMUM_FILE_DESCRIPTORS 32
33 33 #define CONFIGURE_INIT_TASK_PRIORITY 1 // instead of 100
34 34 #define CONFIGURE_INIT_TASK_MODE (RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT)
35 35 #define CONFIGURE_INIT_TASK_ATTRIBUTES (RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT)
36 36 #define CONFIGURE_MAXIMUM_DRIVERS 16
37 37 #define CONFIGURE_MAXIMUM_PERIODS 5
38 38 #define CONFIGURE_MAXIMUM_TIMERS 5 // [spiq] [link] [spacewire_reset_link]
39 39 #define CONFIGURE_MAXIMUM_MESSAGE_QUEUES 5
40 40 #ifdef PRINT_STACK_REPORT
41 41 #define CONFIGURE_STACK_CHECKER_ENABLED
42 42 #endif
43 43
44 44 #include <rtems/confdefs.h>
45 45
46 46 /* If --drvmgr was enabled during the configuration of the RTEMS kernel */
47 47 #ifdef RTEMS_DRVMGR_STARTUP
48 48 #ifdef LEON3
49 49 /* Add Timer and UART Driver */
50 50 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
51 51 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
52 52 #endif
53 53 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
54 54 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
55 55 #endif
56 56 #endif
57 57 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
58 58 #include <drvmgr/drvmgr_confdefs.h>
59 59 #endif
60 60
61 61 #include "fsw_init.h"
62 62 #include "fsw_config.c"
63 63 #include "GscMemoryLPP.hpp"
64 64
65 65 void initCache()
66 66 {
67 67 // ASI 2 contains a few control registers that have not been assigned as ancillary state registers.
68 68 // These should only be read and written using 32-bit LDA/STA instructions.
69 69 // All cache registers are accessed through load/store operations to the alternate address space (LDA/STA), using ASI = 2.
70 70 // The table below shows the register addresses:
71 71 // 0x00 Cache control register
72 72 // 0x04 Reserved
73 73 // 0x08 Instruction cache configuration register
74 74 // 0x0C Data cache configuration register
75 75
76 76 // Cache Control Register Leon3 / Leon3FT
77 77 // 31..30 29 28 27..24 23 22 21 20..19 18 17 16
78 78 // RFT PS TB DS FD FI FT ST IB
79 79 // 15 14 13..12 11..10 9..8 7..6 5 4 3..2 1..0
80 80 // IP DP ITE IDE DTE DDE DF IF DCS ICS
81 81
82 82 unsigned int cacheControlRegister;
83 83
84 84 CCR_resetCacheControlRegister();
85 85 ASR16_resetRegisterProtectionControlRegister();
86 86
87 87 cacheControlRegister = CCR_getValue();
88 88 PRINTF1("(0) CCR - Cache Control Register = %x\n", cacheControlRegister);
89 89 PRINTF1("(0) ASR16 = %x\n", *asr16Ptr);
90 90
91 91 CCR_enableInstructionCache(); // ICS bits
92 92 CCR_enableDataCache(); // DCS bits
93 93 CCR_enableInstructionBurstFetch(); // IB bit
94 94
95 95 faultTolerantScheme();
96 96
97 97 cacheControlRegister = CCR_getValue();
98 98 PRINTF1("(1) CCR - Cache Control Register = %x\n", cacheControlRegister);
99 99 PRINTF1("(1) ASR16 Register protection control register = %x\n", *asr16Ptr);
100 100
101 101 PRINTF("\n");
102 102 }
103 103
104 104 rtems_task Init( rtems_task_argument ignored )
105 105 {
106 106 /** This is the RTEMS INIT taks, it is the first task launched by the system.
107 107 *
108 108 * @param unused is the starting argument of the RTEMS task
109 109 *
110 110 * The INIT task create and run all other RTEMS tasks.
111 111 *
112 112 */
113 113
114 114 //***********
115 115 // INIT CACHE
116 116
117 117 unsigned char *vhdlVersion;
118 118
119 119 reset_lfr();
120 120
121 121 reset_local_time();
122 122
123 123 rtems_cpu_usage_reset();
124 124
125 125 rtems_status_code status;
126 126 rtems_status_code status_spw;
127 127 rtems_isr_entry old_isr_handler;
128 128
129 129 // UART settings
130 130 enable_apbuart_transmitter();
131 131 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
132 132
133 133 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
134 134
135 135
136 136 PRINTF("\n\n\n\n\n")
137 137
138 138 initCache();
139 139
140 140 PRINTF("*************************\n")
141 141 PRINTF("** LFR Flight Software **\n")
142 142 PRINTF1("** %d.", SW_VERSION_N1)
143 143 PRINTF1("%d." , SW_VERSION_N2)
144 144 PRINTF1("%d." , SW_VERSION_N3)
145 145 PRINTF1("%d **\n", SW_VERSION_N4)
146 146
147 147 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
148 148 PRINTF("** VHDL **\n")
149 149 PRINTF1("** %d.", vhdlVersion[1])
150 150 PRINTF1("%d." , vhdlVersion[2])
151 151 PRINTF1("%d **\n", vhdlVersion[3])
152 152 PRINTF("*************************\n")
153 153 PRINTF("\n\n")
154 154
155 155 init_parameter_dump();
156 156 init_kcoefficients_dump();
157 157 init_local_mode_parameters();
158 158 init_housekeeping_parameters();
159 159 init_k_coefficients_prc0();
160 160 init_k_coefficients_prc1();
161 161 init_k_coefficients_prc2();
162 162 pa_bia_status_info = 0x00;
163 163 cp_rpw_sc_rw_f_flags = 0x00;
164 164 cp_rpw_sc_rw1_f1 = 0.0;
165 165 cp_rpw_sc_rw1_f2 = 0.0;
166 166 cp_rpw_sc_rw2_f1 = 0.0;
167 167 cp_rpw_sc_rw2_f2 = 0.0;
168 168 cp_rpw_sc_rw3_f1 = 0.0;
169 169 cp_rpw_sc_rw3_f2 = 0.0;
170 170 cp_rpw_sc_rw4_f1 = 0.0;
171 171 cp_rpw_sc_rw4_f2 = 0.0;
172 sy_lfr_sc_rw_delta_f = 0.0;
172 // initialize filtering parameters
173 filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED;
174 filterPar.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
175 filterPar.sy_lfr_pas_filter_tbad = DEFAULT_SY_LFR_PAS_FILTER_TBAD;
176 filterPar.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
177 filterPar.sy_lfr_pas_filter_shift = DEFAULT_SY_LFR_PAS_FILTER_SHIFT;
178 filterPar.sy_lfr_sc_rw_delta_f = DEFAULT_SY_LFR_SC_RW_DELTA_F;
173 179 update_last_valid_transition_date( DEFAULT_LAST_VALID_TRANSITION_DATE );
174 180
175 181 // waveform picker initialization
176 182 WFP_init_rings();
177 183 LEON_Clear_interrupt( IRQ_SPARC_GPTIMER_WATCHDOG ); // initialize the waveform rings
178 184 WFP_reset_current_ring_nodes();
179 185 reset_waveform_picker_regs();
180 186
181 187 // spectral matrices initialization
182 188 SM_init_rings(); // initialize spectral matrices rings
183 189 SM_reset_current_ring_nodes();
184 190 reset_spectral_matrix_regs();
185 191
186 192 // configure calibration
187 193 configureCalibration( false ); // true means interleaved mode, false is for normal mode
188 194
189 195 updateLFRCurrentMode( LFR_MODE_STANDBY );
190 196
191 197 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
192 198
193 199 create_names(); // create all names
194 200
195 201 status = create_timecode_timer(); // create the timer used by timecode_irq_handler
196 202 if (status != RTEMS_SUCCESSFUL)
197 203 {
198 204 PRINTF1("in INIT *** ERR in create_timer_timecode, code %d", status)
199 205 }
200 206
201 207 status = create_message_queues(); // create message queues
202 208 if (status != RTEMS_SUCCESSFUL)
203 209 {
204 210 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
205 211 }
206 212
207 213 status = create_all_tasks(); // create all tasks
208 214 if (status != RTEMS_SUCCESSFUL)
209 215 {
210 216 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
211 217 }
212 218
213 219 // **************************
214 220 // <SPACEWIRE INITIALIZATION>
215 221 status_spw = spacewire_open_link(); // (1) open the link
216 222 if ( status_spw != RTEMS_SUCCESSFUL )
217 223 {
218 224 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
219 225 }
220 226
221 227 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
222 228 {
223 229 status_spw = spacewire_configure_link( fdSPW );
224 230 if ( status_spw != RTEMS_SUCCESSFUL )
225 231 {
226 232 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
227 233 }
228 234 }
229 235
230 236 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
231 237 {
232 238 status_spw = spacewire_start_link( fdSPW );
233 239 if ( status_spw != RTEMS_SUCCESSFUL )
234 240 {
235 241 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
236 242 }
237 243 }
238 244 // </SPACEWIRE INITIALIZATION>
239 245 // ***************************
240 246
241 247 status = start_all_tasks(); // start all tasks
242 248 if (status != RTEMS_SUCCESSFUL)
243 249 {
244 250 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
245 251 }
246 252
247 253 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
248 254 status = start_recv_send_tasks();
249 255 if ( status != RTEMS_SUCCESSFUL )
250 256 {
251 257 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
252 258 }
253 259
254 260 // suspend science tasks, they will be restarted later depending on the mode
255 261 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
256 262 if (status != RTEMS_SUCCESSFUL)
257 263 {
258 264 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
259 265 }
260 266
261 267 // configure IRQ handling for the waveform picker unit
262 268 status = rtems_interrupt_catch( waveforms_isr,
263 269 IRQ_SPARC_WAVEFORM_PICKER,
264 270 &old_isr_handler) ;
265 271 // configure IRQ handling for the spectral matrices unit
266 272 status = rtems_interrupt_catch( spectral_matrices_isr,
267 273 IRQ_SPARC_SPECTRAL_MATRIX,
268 274 &old_isr_handler) ;
269 275
270 276 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
271 277 if ( status_spw != RTEMS_SUCCESSFUL )
272 278 {
273 279 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
274 280 if ( status != RTEMS_SUCCESSFUL ) {
275 281 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
276 282 }
277 283 }
278 284
279 285 BOOT_PRINTF("delete INIT\n")
280 286
281 287 set_hk_lfr_sc_potential_flag( true );
282 288
283 289 // start the timer to detect a missing spacewire timecode
284 290 // the timeout is larger because the spw IP needs to receive several valid timecodes before generating a tickout
285 291 // if a tickout is generated, the timer is restarted
286 292 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT_INIT, timecode_timer_routine, NULL );
287 293
288 294 grspw_timecode_callback = &timecode_irq_handler;
289 295
290 296 status = rtems_task_delete(RTEMS_SELF);
291 297
292 298 }
293 299
294 300 void init_local_mode_parameters( void )
295 301 {
296 302 /** This function initialize the param_local global variable with default values.
297 303 *
298 304 */
299 305
300 306 unsigned int i;
301 307
302 308 // LOCAL PARAMETERS
303 309
304 310 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
305 311 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
306 312 BOOT_PRINTF1("nb_interrupt_f0_MAX = %d\n", param_local.local_nb_interrupt_f0_MAX)
307 313
308 314 // init sequence counters
309 315
310 316 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
311 317 {
312 318 sequenceCounters_TC_EXE[i] = 0x00;
313 319 sequenceCounters_TM_DUMP[i] = 0x00;
314 320 }
315 321 sequenceCounters_SCIENCE_NORMAL_BURST = 0x00;
316 322 sequenceCounters_SCIENCE_SBM1_SBM2 = 0x00;
317 323 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
318 324 }
319 325
320 326 void reset_local_time( void )
321 327 {
322 328 time_management_regs->ctrl = time_management_regs->ctrl | 0x02; // [0010] software reset, coarse time = 0x80000000
323 329 }
324 330
325 331 void create_names( void ) // create all names for tasks and queues
326 332 {
327 333 /** This function creates all RTEMS names used in the software for tasks and queues.
328 334 *
329 335 * @return RTEMS directive status codes:
330 336 * - RTEMS_SUCCESSFUL - successful completion
331 337 *
332 338 */
333 339
334 340 // task names
335 341 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
336 342 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
337 343 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
338 344 Task_name[TASKID_LOAD] = rtems_build_name( 'L', 'O', 'A', 'D' );
339 345 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
340 346 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
341 347 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
342 348 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
343 349 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
344 350 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
345 351 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
346 352 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
347 353 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
348 354 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
349 355 Task_name[TASKID_LINK] = rtems_build_name( 'L', 'I', 'N', 'K' );
350 356 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
351 357 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
352 358 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
353 359 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
354 360
355 361 // rate monotonic period names
356 362 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
357 363
358 364 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
359 365 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
360 366 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
361 367 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
362 368 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
363 369
364 370 timecode_timer_name = rtems_build_name( 'S', 'P', 'T', 'C' );
365 371 }
366 372
367 373 int create_all_tasks( void ) // create all tasks which run in the software
368 374 {
369 375 /** This function creates all RTEMS tasks used in the software.
370 376 *
371 377 * @return RTEMS directive status codes:
372 378 * - RTEMS_SUCCESSFUL - task created successfully
373 379 * - RTEMS_INVALID_ADDRESS - id is NULL
374 380 * - RTEMS_INVALID_NAME - invalid task name
375 381 * - RTEMS_INVALID_PRIORITY - invalid task priority
376 382 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
377 383 * - RTEMS_TOO_MANY - too many tasks created
378 384 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
379 385 * - RTEMS_TOO_MANY - too many global objects
380 386 *
381 387 */
382 388
383 389 rtems_status_code status;
384 390
385 391 //**********
386 392 // SPACEWIRE
387 393 // RECV
388 394 status = rtems_task_create(
389 395 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
390 396 RTEMS_DEFAULT_MODES,
391 397 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
392 398 );
393 399 if (status == RTEMS_SUCCESSFUL) // SEND
394 400 {
395 401 status = rtems_task_create(
396 402 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * 2,
397 403 RTEMS_DEFAULT_MODES,
398 404 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
399 405 );
400 406 }
401 407 if (status == RTEMS_SUCCESSFUL) // LINK
402 408 {
403 409 status = rtems_task_create(
404 410 Task_name[TASKID_LINK], TASK_PRIORITY_LINK, RTEMS_MINIMUM_STACK_SIZE,
405 411 RTEMS_DEFAULT_MODES,
406 412 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LINK]
407 413 );
408 414 }
409 415 if (status == RTEMS_SUCCESSFUL) // ACTN
410 416 {
411 417 status = rtems_task_create(
412 418 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
413 419 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
414 420 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
415 421 );
416 422 }
417 423 if (status == RTEMS_SUCCESSFUL) // SPIQ
418 424 {
419 425 status = rtems_task_create(
420 426 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
421 427 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
422 428 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
423 429 );
424 430 }
425 431
426 432 //******************
427 433 // SPECTRAL MATRICES
428 434 if (status == RTEMS_SUCCESSFUL) // AVF0
429 435 {
430 436 status = rtems_task_create(
431 437 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
432 438 RTEMS_DEFAULT_MODES,
433 439 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
434 440 );
435 441 }
436 442 if (status == RTEMS_SUCCESSFUL) // PRC0
437 443 {
438 444 status = rtems_task_create(
439 445 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * 2,
440 446 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
441 447 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
442 448 );
443 449 }
444 450 if (status == RTEMS_SUCCESSFUL) // AVF1
445 451 {
446 452 status = rtems_task_create(
447 453 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
448 454 RTEMS_DEFAULT_MODES,
449 455 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
450 456 );
451 457 }
452 458 if (status == RTEMS_SUCCESSFUL) // PRC1
453 459 {
454 460 status = rtems_task_create(
455 461 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * 2,
456 462 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
457 463 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
458 464 );
459 465 }
460 466 if (status == RTEMS_SUCCESSFUL) // AVF2
461 467 {
462 468 status = rtems_task_create(
463 469 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
464 470 RTEMS_DEFAULT_MODES,
465 471 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
466 472 );
467 473 }
468 474 if (status == RTEMS_SUCCESSFUL) // PRC2
469 475 {
470 476 status = rtems_task_create(
471 477 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * 2,
472 478 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
473 479 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
474 480 );
475 481 }
476 482
477 483 //****************
478 484 // WAVEFORM PICKER
479 485 if (status == RTEMS_SUCCESSFUL) // WFRM
480 486 {
481 487 status = rtems_task_create(
482 488 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
483 489 RTEMS_DEFAULT_MODES,
484 490 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
485 491 );
486 492 }
487 493 if (status == RTEMS_SUCCESSFUL) // CWF3
488 494 {
489 495 status = rtems_task_create(
490 496 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
491 497 RTEMS_DEFAULT_MODES,
492 498 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
493 499 );
494 500 }
495 501 if (status == RTEMS_SUCCESSFUL) // CWF2
496 502 {
497 503 status = rtems_task_create(
498 504 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
499 505 RTEMS_DEFAULT_MODES,
500 506 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
501 507 );
502 508 }
503 509 if (status == RTEMS_SUCCESSFUL) // CWF1
504 510 {
505 511 status = rtems_task_create(
506 512 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
507 513 RTEMS_DEFAULT_MODES,
508 514 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
509 515 );
510 516 }
511 517 if (status == RTEMS_SUCCESSFUL) // SWBD
512 518 {
513 519 status = rtems_task_create(
514 520 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
515 521 RTEMS_DEFAULT_MODES,
516 522 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
517 523 );
518 524 }
519 525
520 526 //*****
521 527 // MISC
522 528 if (status == RTEMS_SUCCESSFUL) // LOAD
523 529 {
524 530 status = rtems_task_create(
525 531 Task_name[TASKID_LOAD], TASK_PRIORITY_LOAD, RTEMS_MINIMUM_STACK_SIZE,
526 532 RTEMS_DEFAULT_MODES,
527 533 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LOAD]
528 534 );
529 535 }
530 536 if (status == RTEMS_SUCCESSFUL) // DUMB
531 537 {
532 538 status = rtems_task_create(
533 539 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
534 540 RTEMS_DEFAULT_MODES,
535 541 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
536 542 );
537 543 }
538 544 if (status == RTEMS_SUCCESSFUL) // HOUS
539 545 {
540 546 status = rtems_task_create(
541 547 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
542 548 RTEMS_DEFAULT_MODES,
543 549 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
544 550 );
545 551 }
546 552
547 553 return status;
548 554 }
549 555
550 556 int start_recv_send_tasks( void )
551 557 {
552 558 rtems_status_code status;
553 559
554 560 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
555 561 if (status!=RTEMS_SUCCESSFUL) {
556 562 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
557 563 }
558 564
559 565 if (status == RTEMS_SUCCESSFUL) // SEND
560 566 {
561 567 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
562 568 if (status!=RTEMS_SUCCESSFUL) {
563 569 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
564 570 }
565 571 }
566 572
567 573 return status;
568 574 }
569 575
570 576 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
571 577 {
572 578 /** This function starts all RTEMS tasks used in the software.
573 579 *
574 580 * @return RTEMS directive status codes:
575 581 * - RTEMS_SUCCESSFUL - ask started successfully
576 582 * - RTEMS_INVALID_ADDRESS - invalid task entry point
577 583 * - RTEMS_INVALID_ID - invalid task id
578 584 * - RTEMS_INCORRECT_STATE - task not in the dormant state
579 585 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
580 586 *
581 587 */
582 588 // starts all the tasks fot eh flight software
583 589
584 590 rtems_status_code status;
585 591
586 592 //**********
587 593 // SPACEWIRE
588 594 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
589 595 if (status!=RTEMS_SUCCESSFUL) {
590 596 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
591 597 }
592 598
593 599 if (status == RTEMS_SUCCESSFUL) // LINK
594 600 {
595 601 status = rtems_task_start( Task_id[TASKID_LINK], link_task, 1 );
596 602 if (status!=RTEMS_SUCCESSFUL) {
597 603 BOOT_PRINTF("in INIT *** Error starting TASK_LINK\n")
598 604 }
599 605 }
600 606
601 607 if (status == RTEMS_SUCCESSFUL) // ACTN
602 608 {
603 609 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
604 610 if (status!=RTEMS_SUCCESSFUL) {
605 611 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
606 612 }
607 613 }
608 614
609 615 //******************
610 616 // SPECTRAL MATRICES
611 617 if (status == RTEMS_SUCCESSFUL) // AVF0
612 618 {
613 619 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
614 620 if (status!=RTEMS_SUCCESSFUL) {
615 621 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
616 622 }
617 623 }
618 624 if (status == RTEMS_SUCCESSFUL) // PRC0
619 625 {
620 626 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
621 627 if (status!=RTEMS_SUCCESSFUL) {
622 628 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
623 629 }
624 630 }
625 631 if (status == RTEMS_SUCCESSFUL) // AVF1
626 632 {
627 633 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
628 634 if (status!=RTEMS_SUCCESSFUL) {
629 635 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
630 636 }
631 637 }
632 638 if (status == RTEMS_SUCCESSFUL) // PRC1
633 639 {
634 640 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
635 641 if (status!=RTEMS_SUCCESSFUL) {
636 642 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
637 643 }
638 644 }
639 645 if (status == RTEMS_SUCCESSFUL) // AVF2
640 646 {
641 647 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
642 648 if (status!=RTEMS_SUCCESSFUL) {
643 649 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
644 650 }
645 651 }
646 652 if (status == RTEMS_SUCCESSFUL) // PRC2
647 653 {
648 654 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
649 655 if (status!=RTEMS_SUCCESSFUL) {
650 656 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
651 657 }
652 658 }
653 659
654 660 //****************
655 661 // WAVEFORM PICKER
656 662 if (status == RTEMS_SUCCESSFUL) // WFRM
657 663 {
658 664 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
659 665 if (status!=RTEMS_SUCCESSFUL) {
660 666 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
661 667 }
662 668 }
663 669 if (status == RTEMS_SUCCESSFUL) // CWF3
664 670 {
665 671 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
666 672 if (status!=RTEMS_SUCCESSFUL) {
667 673 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
668 674 }
669 675 }
670 676 if (status == RTEMS_SUCCESSFUL) // CWF2
671 677 {
672 678 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
673 679 if (status!=RTEMS_SUCCESSFUL) {
674 680 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
675 681 }
676 682 }
677 683 if (status == RTEMS_SUCCESSFUL) // CWF1
678 684 {
679 685 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
680 686 if (status!=RTEMS_SUCCESSFUL) {
681 687 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
682 688 }
683 689 }
684 690 if (status == RTEMS_SUCCESSFUL) // SWBD
685 691 {
686 692 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
687 693 if (status!=RTEMS_SUCCESSFUL) {
688 694 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
689 695 }
690 696 }
691 697
692 698 //*****
693 699 // MISC
694 700 if (status == RTEMS_SUCCESSFUL) // HOUS
695 701 {
696 702 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
697 703 if (status!=RTEMS_SUCCESSFUL) {
698 704 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
699 705 }
700 706 }
701 707 if (status == RTEMS_SUCCESSFUL) // DUMB
702 708 {
703 709 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
704 710 if (status!=RTEMS_SUCCESSFUL) {
705 711 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
706 712 }
707 713 }
708 714 if (status == RTEMS_SUCCESSFUL) // LOAD
709 715 {
710 716 status = rtems_task_start( Task_id[TASKID_LOAD], load_task, 1 );
711 717 if (status!=RTEMS_SUCCESSFUL) {
712 718 BOOT_PRINTF("in INIT *** Error starting TASK_LOAD\n")
713 719 }
714 720 }
715 721
716 722 return status;
717 723 }
718 724
719 725 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
720 726 {
721 727 rtems_status_code status_recv;
722 728 rtems_status_code status_send;
723 729 rtems_status_code status_q_p0;
724 730 rtems_status_code status_q_p1;
725 731 rtems_status_code status_q_p2;
726 732 rtems_status_code ret;
727 733 rtems_id queue_id;
728 734
729 735 //****************************************
730 736 // create the queue for handling valid TCs
731 737 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
732 738 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
733 739 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
734 740 if ( status_recv != RTEMS_SUCCESSFUL ) {
735 741 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
736 742 }
737 743
738 744 //************************************************
739 745 // create the queue for handling TM packet sending
740 746 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
741 747 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
742 748 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
743 749 if ( status_send != RTEMS_SUCCESSFUL ) {
744 750 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
745 751 }
746 752
747 753 //*****************************************************************************
748 754 // create the queue for handling averaged spectral matrices for processing @ f0
749 755 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
750 756 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
751 757 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
752 758 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
753 759 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
754 760 }
755 761
756 762 //*****************************************************************************
757 763 // create the queue for handling averaged spectral matrices for processing @ f1
758 764 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
759 765 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
760 766 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
761 767 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
762 768 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
763 769 }
764 770
765 771 //*****************************************************************************
766 772 // create the queue for handling averaged spectral matrices for processing @ f2
767 773 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
768 774 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
769 775 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
770 776 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
771 777 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
772 778 }
773 779
774 780 if ( status_recv != RTEMS_SUCCESSFUL )
775 781 {
776 782 ret = status_recv;
777 783 }
778 784 else if( status_send != RTEMS_SUCCESSFUL )
779 785 {
780 786 ret = status_send;
781 787 }
782 788 else if( status_q_p0 != RTEMS_SUCCESSFUL )
783 789 {
784 790 ret = status_q_p0;
785 791 }
786 792 else if( status_q_p1 != RTEMS_SUCCESSFUL )
787 793 {
788 794 ret = status_q_p1;
789 795 }
790 796 else
791 797 {
792 798 ret = status_q_p2;
793 799 }
794 800
795 801 return ret;
796 802 }
797 803
798 804 rtems_status_code create_timecode_timer( void )
799 805 {
800 806 rtems_status_code status;
801 807
802 808 status = rtems_timer_create( timecode_timer_name, &timecode_timer_id );
803 809
804 810 if ( status != RTEMS_SUCCESSFUL )
805 811 {
806 812 PRINTF1("in create_timer_timecode *** ERR creating SPTC timer, %d\n", status)
807 813 }
808 814 else
809 815 {
810 816 PRINTF("in create_timer_timecode *** OK creating SPTC timer\n")
811 817 }
812 818
813 819 return status;
814 820 }
815 821
816 822 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
817 823 {
818 824 rtems_status_code status;
819 825 rtems_name queue_name;
820 826
821 827 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
822 828
823 829 status = rtems_message_queue_ident( queue_name, 0, queue_id );
824 830
825 831 return status;
826 832 }
827 833
828 834 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
829 835 {
830 836 rtems_status_code status;
831 837 rtems_name queue_name;
832 838
833 839 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
834 840
835 841 status = rtems_message_queue_ident( queue_name, 0, queue_id );
836 842
837 843 return status;
838 844 }
839 845
840 846 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
841 847 {
842 848 rtems_status_code status;
843 849 rtems_name queue_name;
844 850
845 851 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
846 852
847 853 status = rtems_message_queue_ident( queue_name, 0, queue_id );
848 854
849 855 return status;
850 856 }
851 857
852 858 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
853 859 {
854 860 rtems_status_code status;
855 861 rtems_name queue_name;
856 862
857 863 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
858 864
859 865 status = rtems_message_queue_ident( queue_name, 0, queue_id );
860 866
861 867 return status;
862 868 }
863 869
864 870 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
865 871 {
866 872 rtems_status_code status;
867 873 rtems_name queue_name;
868 874
869 875 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
870 876
871 877 status = rtems_message_queue_ident( queue_name, 0, queue_id );
872 878
873 879 return status;
874 880 }
875 881
876 882 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max )
877 883 {
878 884 u_int32_t count;
879 885 rtems_status_code status;
880 886
881 887 status = rtems_message_queue_get_number_pending( queue_id, &count );
882 888
883 889 count = count + 1;
884 890
885 891 if (status != RTEMS_SUCCESSFUL)
886 892 {
887 893 PRINTF1("in update_queue_max_count *** ERR = %d\n", status)
888 894 }
889 895 else
890 896 {
891 897 if (count > *fifo_size_max)
892 898 {
893 899 *fifo_size_max = count;
894 900 }
895 901 }
896 902 }
897 903
898 904 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
899 905 {
900 906 unsigned char i;
901 907
902 908 //***************
903 909 // BUFFER ADDRESS
904 910 for(i=0; i<nbNodes; i++)
905 911 {
906 912 ring[i].coarseTime = 0xffffffff;
907 913 ring[i].fineTime = 0xffffffff;
908 914 ring[i].sid = 0x00;
909 915 ring[i].status = 0x00;
910 916 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
911 917 }
912 918
913 919 //*****
914 920 // NEXT
915 921 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
916 922 for(i=0; i<nbNodes-1; i++)
917 923 {
918 924 ring[i].next = (ring_node*) &ring[ i + 1 ];
919 925 }
920 926
921 927 //*********
922 928 // PREVIOUS
923 929 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
924 930 for(i=1; i<nbNodes; i++)
925 931 {
926 932 ring[i].previous = (ring_node*) &ring[ i - 1 ];
927 933 }
928 934 }
@@ -1,795 +1,786
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "fsw_processing.h"
11 11 #include "fsw_processing_globals.c"
12 12 #include "fsw_init.h"
13 13
14 14 unsigned int nb_sm_f0;
15 15 unsigned int nb_sm_f0_aux_f1;
16 16 unsigned int nb_sm_f1;
17 17 unsigned int nb_sm_f0_aux_f2;
18 18
19 19 typedef enum restartState_t
20 20 {
21 21 WAIT_FOR_F2,
22 22 WAIT_FOR_F1,
23 23 WAIT_FOR_F0
24 24 } restartState;
25 25
26 26 //************************
27 27 // spectral matrices rings
28 28 ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ];
29 29 ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ];
30 30 ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ];
31 31 ring_node *current_ring_node_sm_f0;
32 32 ring_node *current_ring_node_sm_f1;
33 33 ring_node *current_ring_node_sm_f2;
34 34 ring_node *ring_node_for_averaging_sm_f0;
35 35 ring_node *ring_node_for_averaging_sm_f1;
36 36 ring_node *ring_node_for_averaging_sm_f2;
37 37
38 38 //
39 39 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
40 40 {
41 41 ring_node *node;
42 42
43 43 node = NULL;
44 44 switch ( frequencyChannel ) {
45 45 case 0:
46 46 node = ring_node_for_averaging_sm_f0;
47 47 break;
48 48 case 1:
49 49 node = ring_node_for_averaging_sm_f1;
50 50 break;
51 51 case 2:
52 52 node = ring_node_for_averaging_sm_f2;
53 53 break;
54 54 default:
55 55 break;
56 56 }
57 57
58 58 return node;
59 59 }
60 60
61 61 //***********************************************************
62 62 // Interrupt Service Routine for spectral matrices processing
63 63
64 64 void spectral_matrices_isr_f0( int statusReg )
65 65 {
66 66 unsigned char status;
67 67 rtems_status_code status_code;
68 68 ring_node *full_ring_node;
69 69
70 70 status = (unsigned char) (statusReg & 0x03); // [0011] get the status_ready_matrix_f0_x bits
71 71
72 72 switch(status)
73 73 {
74 74 case 0:
75 75 break;
76 76 case 3:
77 77 // UNEXPECTED VALUE
78 78 spectral_matrix_regs->status = 0x03; // [0011]
79 79 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
80 80 break;
81 81 case 1:
82 82 full_ring_node = current_ring_node_sm_f0->previous;
83 83 full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
84 84 full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time;
85 85 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
86 86 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
87 87 // if there are enough ring nodes ready, wake up an AVFx task
88 88 nb_sm_f0 = nb_sm_f0 + 1;
89 89 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
90 90 {
91 91 ring_node_for_averaging_sm_f0 = full_ring_node;
92 92 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
93 93 {
94 94 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
95 95 }
96 96 nb_sm_f0 = 0;
97 97 }
98 98 spectral_matrix_regs->status = 0x01; // [0000 0001]
99 99 break;
100 100 case 2:
101 101 full_ring_node = current_ring_node_sm_f0->previous;
102 102 full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
103 103 full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time;
104 104 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
105 105 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
106 106 // if there are enough ring nodes ready, wake up an AVFx task
107 107 nb_sm_f0 = nb_sm_f0 + 1;
108 108 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
109 109 {
110 110 ring_node_for_averaging_sm_f0 = full_ring_node;
111 111 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
112 112 {
113 113 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
114 114 }
115 115 nb_sm_f0 = 0;
116 116 }
117 117 spectral_matrix_regs->status = 0x02; // [0000 0010]
118 118 break;
119 119 }
120 120 }
121 121
122 122 void spectral_matrices_isr_f1( int statusReg )
123 123 {
124 124 rtems_status_code status_code;
125 125 unsigned char status;
126 126 ring_node *full_ring_node;
127 127
128 128 status = (unsigned char) ((statusReg & 0x0c) >> 2); // [1100] get the status_ready_matrix_f1_x bits
129 129
130 130 switch(status)
131 131 {
132 132 case 0:
133 133 break;
134 134 case 3:
135 135 // UNEXPECTED VALUE
136 136 spectral_matrix_regs->status = 0xc0; // [1100]
137 137 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
138 138 break;
139 139 case 1:
140 140 full_ring_node = current_ring_node_sm_f1->previous;
141 141 full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
142 142 full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
143 143 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
144 144 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
145 145 // if there are enough ring nodes ready, wake up an AVFx task
146 146 nb_sm_f1 = nb_sm_f1 + 1;
147 147 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
148 148 {
149 149 ring_node_for_averaging_sm_f1 = full_ring_node;
150 150 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
151 151 {
152 152 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
153 153 }
154 154 nb_sm_f1 = 0;
155 155 }
156 156 spectral_matrix_regs->status = 0x04; // [0000 0100]
157 157 break;
158 158 case 2:
159 159 full_ring_node = current_ring_node_sm_f1->previous;
160 160 full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
161 161 full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
162 162 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
163 163 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
164 164 // if there are enough ring nodes ready, wake up an AVFx task
165 165 nb_sm_f1 = nb_sm_f1 + 1;
166 166 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
167 167 {
168 168 ring_node_for_averaging_sm_f1 = full_ring_node;
169 169 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
170 170 {
171 171 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
172 172 }
173 173 nb_sm_f1 = 0;
174 174 }
175 175 spectral_matrix_regs->status = 0x08; // [1000 0000]
176 176 break;
177 177 }
178 178 }
179 179
180 180 void spectral_matrices_isr_f2( int statusReg )
181 181 {
182 182 unsigned char status;
183 183 rtems_status_code status_code;
184 184
185 185 status = (unsigned char) ((statusReg & 0x30) >> 4); // [0011 0000] get the status_ready_matrix_f2_x bits
186 186
187 187 switch(status)
188 188 {
189 189 case 0:
190 190 break;
191 191 case 3:
192 192 // UNEXPECTED VALUE
193 193 spectral_matrix_regs->status = 0x30; // [0011 0000]
194 194 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
195 195 break;
196 196 case 1:
197 197 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
198 198 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
199 199 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
200 200 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
201 201 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
202 202 spectral_matrix_regs->status = 0x10; // [0001 0000]
203 203 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
204 204 {
205 205 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
206 206 }
207 207 break;
208 208 case 2:
209 209 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
210 210 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
211 211 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
212 212 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
213 213 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
214 214 spectral_matrix_regs->status = 0x20; // [0010 0000]
215 215 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
216 216 {
217 217 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
218 218 }
219 219 break;
220 220 }
221 221 }
222 222
223 223 void spectral_matrix_isr_error_handler( int statusReg )
224 224 {
225 225 // STATUS REGISTER
226 226 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
227 227 // 10 9 8
228 228 // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
229 229 // 7 6 5 4 3 2 1 0
230 230 // [bad_component_err] not defined in the last version of the VHDL code
231 231
232 232 rtems_status_code status_code;
233 233
234 234 //***************************************************
235 235 // the ASM status register is copied in the HK packet
236 236 housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) (statusReg & 0x780 >> 7); // [0111 1000 0000]
237 237
238 238 if (statusReg & 0x7c0) // [0111 1100 0000]
239 239 {
240 240 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
241 241 }
242 242
243 243 spectral_matrix_regs->status = spectral_matrix_regs->status & 0x7c0;
244 244
245 245 }
246 246
247 247 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
248 248 {
249 249 // STATUS REGISTER
250 250 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
251 251 // 10 9 8
252 252 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
253 253 // 7 6 5 4 3 2 1 0
254 254
255 255 int statusReg;
256 256
257 257 static restartState state = WAIT_FOR_F2;
258 258
259 259 statusReg = spectral_matrix_regs->status;
260 260
261 261 if (thisIsAnASMRestart == 0)
262 262 { // this is not a restart sequence, process incoming matrices normally
263 263 spectral_matrices_isr_f0( statusReg );
264 264
265 265 spectral_matrices_isr_f1( statusReg );
266 266
267 267 spectral_matrices_isr_f2( statusReg );
268 268 }
269 269 else
270 270 { // a restart sequence has to be launched
271 271 switch (state) {
272 272 case WAIT_FOR_F2:
273 273 if ((statusReg & 0x30) != 0x00) // [0011 0000] check the status_ready_matrix_f2_x bits
274 274 {
275 275 state = WAIT_FOR_F1;
276 276 }
277 277 break;
278 278 case WAIT_FOR_F1:
279 279 if ((statusReg & 0x0c) != 0x00) // [0000 1100] check the status_ready_matrix_f1_x bits
280 280 {
281 281 state = WAIT_FOR_F0;
282 282 }
283 283 break;
284 284 case WAIT_FOR_F0:
285 285 if ((statusReg & 0x03) != 0x00) // [0000 0011] check the status_ready_matrix_f0_x bits
286 286 {
287 287 state = WAIT_FOR_F2;
288 288 thisIsAnASMRestart = 0;
289 289 }
290 290 break;
291 291 default:
292 292 break;
293 293 }
294 294 reset_sm_status();
295 295 }
296 296
297 297 spectral_matrix_isr_error_handler( statusReg );
298 298
299 299 }
300 300
301 301 //******************
302 302 // Spectral Matrices
303 303
304 304 void reset_nb_sm( void )
305 305 {
306 306 nb_sm_f0 = 0;
307 307 nb_sm_f0_aux_f1 = 0;
308 308 nb_sm_f0_aux_f2 = 0;
309 309
310 310 nb_sm_f1 = 0;
311 311 }
312 312
313 313 void SM_init_rings( void )
314 314 {
315 315 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
316 316 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
317 317 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
318 318
319 319 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
320 320 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
321 321 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
322 322 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
323 323 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
324 324 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
325 325 }
326 326
327 327 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
328 328 {
329 329 unsigned char i;
330 330
331 331 ring[ nbNodes - 1 ].next
332 332 = (ring_node_asm*) &ring[ 0 ];
333 333
334 334 for(i=0; i<nbNodes-1; i++)
335 335 {
336 336 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
337 337 }
338 338 }
339 339
340 340 void SM_reset_current_ring_nodes( void )
341 341 {
342 342 current_ring_node_sm_f0 = sm_ring_f0[0].next;
343 343 current_ring_node_sm_f1 = sm_ring_f1[0].next;
344 344 current_ring_node_sm_f2 = sm_ring_f2[0].next;
345 345
346 346 ring_node_for_averaging_sm_f0 = NULL;
347 347 ring_node_for_averaging_sm_f1 = NULL;
348 348 ring_node_for_averaging_sm_f2 = NULL;
349 349 }
350 350
351 351 //*****************
352 352 // Basic Parameters
353 353
354 354 void BP_init_header( bp_packet *packet,
355 355 unsigned int apid, unsigned char sid,
356 356 unsigned int packetLength, unsigned char blkNr )
357 357 {
358 358 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
359 359 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
360 360 packet->reserved = 0x00;
361 361 packet->userApplication = CCSDS_USER_APP;
362 362 packet->packetID[0] = (unsigned char) (apid >> 8);
363 363 packet->packetID[1] = (unsigned char) (apid);
364 364 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
365 365 packet->packetSequenceControl[1] = 0x00;
366 366 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
367 367 packet->packetLength[1] = (unsigned char) (packetLength);
368 368 // DATA FIELD HEADER
369 369 packet->spare1_pusVersion_spare2 = 0x10;
370 370 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
371 371 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
372 372 packet->destinationID = TM_DESTINATION_ID_GROUND;
373 373 packet->time[0] = 0x00;
374 374 packet->time[1] = 0x00;
375 375 packet->time[2] = 0x00;
376 376 packet->time[3] = 0x00;
377 377 packet->time[4] = 0x00;
378 378 packet->time[5] = 0x00;
379 379 // AUXILIARY DATA HEADER
380 380 packet->sid = sid;
381 381 packet->pa_bia_status_info = 0x00;
382 382 packet->sy_lfr_common_parameters_spare = 0x00;
383 383 packet->sy_lfr_common_parameters = 0x00;
384 384 packet->acquisitionTime[0] = 0x00;
385 385 packet->acquisitionTime[1] = 0x00;
386 386 packet->acquisitionTime[2] = 0x00;
387 387 packet->acquisitionTime[3] = 0x00;
388 388 packet->acquisitionTime[4] = 0x00;
389 389 packet->acquisitionTime[5] = 0x00;
390 390 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
391 391 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
392 392 }
393 393
394 394 void BP_init_header_with_spare( bp_packet_with_spare *packet,
395 395 unsigned int apid, unsigned char sid,
396 396 unsigned int packetLength , unsigned char blkNr)
397 397 {
398 398 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
399 399 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
400 400 packet->reserved = 0x00;
401 401 packet->userApplication = CCSDS_USER_APP;
402 402 packet->packetID[0] = (unsigned char) (apid >> 8);
403 403 packet->packetID[1] = (unsigned char) (apid);
404 404 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
405 405 packet->packetSequenceControl[1] = 0x00;
406 406 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
407 407 packet->packetLength[1] = (unsigned char) (packetLength);
408 408 // DATA FIELD HEADER
409 409 packet->spare1_pusVersion_spare2 = 0x10;
410 410 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
411 411 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
412 412 packet->destinationID = TM_DESTINATION_ID_GROUND;
413 413 // AUXILIARY DATA HEADER
414 414 packet->sid = sid;
415 415 packet->pa_bia_status_info = 0x00;
416 416 packet->sy_lfr_common_parameters_spare = 0x00;
417 417 packet->sy_lfr_common_parameters = 0x00;
418 418 packet->time[0] = 0x00;
419 419 packet->time[0] = 0x00;
420 420 packet->time[0] = 0x00;
421 421 packet->time[0] = 0x00;
422 422 packet->time[0] = 0x00;
423 423 packet->time[0] = 0x00;
424 424 packet->source_data_spare = 0x00;
425 425 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
426 426 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
427 427 }
428 428
429 429 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
430 430 {
431 431 rtems_status_code status;
432 432
433 433 // SEND PACKET
434 434 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
435 435 if (status != RTEMS_SUCCESSFUL)
436 436 {
437 437 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
438 438 }
439 439 }
440 440
441 441 void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
442 442 {
443 443 /** This function is used to send the BP paquets when needed.
444 444 *
445 445 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
446 446 *
447 447 * @return void
448 448 *
449 449 * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition.
450 450 * BURST paquets are sent everytime.
451 451 *
452 452 */
453 453
454 454 rtems_status_code status;
455 455
456 456 // SEND PACKET
457 457 // before lastValidTransitionDate, the data are drops even if they are ready
458 458 // this guarantees that no SBM packets will be received before the requested enter mode time
459 459 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
460 460 {
461 461 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
462 462 if (status != RTEMS_SUCCESSFUL)
463 463 {
464 464 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
465 465 }
466 466 }
467 467 }
468 468
469 469 //******************
470 470 // general functions
471 471
472 472 void reset_sm_status( void )
473 473 {
474 474 // error
475 475 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
476 476 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
477 477 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
478 478 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
479 479
480 480 spectral_matrix_regs->status = 0x7ff; // [0111 1111 1111]
481 481 }
482 482
483 483 void reset_spectral_matrix_regs( void )
484 484 {
485 485 /** This function resets the spectral matrices module registers.
486 486 *
487 487 * The registers affected by this function are located at the following offset addresses:
488 488 *
489 489 * - 0x00 config
490 490 * - 0x04 status
491 491 * - 0x08 matrixF0_Address0
492 492 * - 0x10 matrixFO_Address1
493 493 * - 0x14 matrixF1_Address
494 494 * - 0x18 matrixF2_Address
495 495 *
496 496 */
497 497
498 498 set_sm_irq_onError( 0 );
499 499
500 500 set_sm_irq_onNewMatrix( 0 );
501 501
502 502 reset_sm_status();
503 503
504 504 // F1
505 505 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
506 506 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
507 507 // F2
508 508 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
509 509 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
510 510 // F3
511 511 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
512 512 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
513 513
514 514 spectral_matrix_regs->matrix_length = 0xc8; // 25 * 128 / 16 = 200 = 0xc8
515 515 }
516 516
517 517 void set_time( unsigned char *time, unsigned char * timeInBuffer )
518 518 {
519 519 time[0] = timeInBuffer[0];
520 520 time[1] = timeInBuffer[1];
521 521 time[2] = timeInBuffer[2];
522 522 time[3] = timeInBuffer[3];
523 523 time[4] = timeInBuffer[6];
524 524 time[5] = timeInBuffer[7];
525 525 }
526 526
527 527 unsigned long long int get_acquisition_time( unsigned char *timePtr )
528 528 {
529 529 unsigned long long int acquisitionTimeAslong;
530 530 acquisitionTimeAslong = 0x00;
531 531 acquisitionTimeAslong = ( (unsigned long long int) (timePtr[0] & 0x7f) << 40 ) // [0111 1111] mask the synchronization bit
532 532 + ( (unsigned long long int) timePtr[1] << 32 )
533 533 + ( (unsigned long long int) timePtr[2] << 24 )
534 534 + ( (unsigned long long int) timePtr[3] << 16 )
535 535 + ( (unsigned long long int) timePtr[6] << 8 )
536 536 + ( (unsigned long long int) timePtr[7] );
537 537 return acquisitionTimeAslong;
538 538 }
539 539
540 540 unsigned char getSID( rtems_event_set event )
541 541 {
542 542 unsigned char sid;
543 543
544 544 rtems_event_set eventSetBURST;
545 545 rtems_event_set eventSetSBM;
546 546
547 547 //******
548 548 // BURST
549 549 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
550 550 | RTEMS_EVENT_BURST_BP1_F1
551 551 | RTEMS_EVENT_BURST_BP2_F0
552 552 | RTEMS_EVENT_BURST_BP2_F1;
553 553
554 554 //****
555 555 // SBM
556 556 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
557 557 | RTEMS_EVENT_SBM_BP1_F1
558 558 | RTEMS_EVENT_SBM_BP2_F0
559 559 | RTEMS_EVENT_SBM_BP2_F1;
560 560
561 561 if (event & eventSetBURST)
562 562 {
563 563 sid = SID_BURST_BP1_F0;
564 564 }
565 565 else if (event & eventSetSBM)
566 566 {
567 567 sid = SID_SBM1_BP1_F0;
568 568 }
569 569 else
570 570 {
571 571 sid = 0;
572 572 }
573 573
574 574 return sid;
575 575 }
576 576
577 577 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
578 578 {
579 579 unsigned int i;
580 580 float re;
581 581 float im;
582 582
583 583 for (i=0; i<NB_BINS_PER_SM; i++){
584 584 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 ];
585 585 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 + 1];
586 586 outputASM[ (asmComponent *NB_BINS_PER_SM) + i] = re;
587 587 outputASM[ (asmComponent+1)*NB_BINS_PER_SM + i] = im;
588 588 }
589 589 }
590 590
591 591 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
592 592 {
593 593 unsigned int i;
594 594 float re;
595 595
596 596 for (i=0; i<NB_BINS_PER_SM; i++){
597 597 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
598 598 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
599 599 }
600 600 }
601 601
602 602 void ASM_patch( float *inputASM, float *outputASM )
603 603 {
604 604 extractReImVectors( inputASM, outputASM, 1); // b1b2
605 605 extractReImVectors( inputASM, outputASM, 3 ); // b1b3
606 606 extractReImVectors( inputASM, outputASM, 5 ); // b1e1
607 607 extractReImVectors( inputASM, outputASM, 7 ); // b1e2
608 608 extractReImVectors( inputASM, outputASM, 10 ); // b2b3
609 609 extractReImVectors( inputASM, outputASM, 12 ); // b2e1
610 610 extractReImVectors( inputASM, outputASM, 14 ); // b2e2
611 611 extractReImVectors( inputASM, outputASM, 17 ); // b3e1
612 612 extractReImVectors( inputASM, outputASM, 19 ); // b3e2
613 613 extractReImVectors( inputASM, outputASM, 22 ); // e1e2
614 614
615 615 copyReVectors(inputASM, outputASM, 0 ); // b1b1
616 616 copyReVectors(inputASM, outputASM, 9 ); // b2b2
617 617 copyReVectors(inputASM, outputASM, 16); // b3b3
618 618 copyReVectors(inputASM, outputASM, 21); // e1e1
619 619 copyReVectors(inputASM, outputASM, 24); // e2e2
620 620 }
621 621
622 622 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
623 623 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage,
624 624 unsigned char ASMIndexStart,
625 625 unsigned char channel )
626 626 {
627 627 //*************
628 628 // input format
629 629 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
630 630 //**************
631 631 // output format
632 632 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
633 633 //************
634 634 // compression
635 635 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
636 636 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
637 637
638 638 int frequencyBin;
639 639 int asmComponent;
640 640 int offsetASM;
641 641 int offsetCompressed;
642 642 int offsetFBin;
643 643 int fBinMask;
644 644 int k;
645 645
646 646 // BUILD DATA
647 647 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
648 648 {
649 649 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
650 650 {
651 651 offsetCompressed = // NO TIME OFFSET
652 652 frequencyBin * NB_VALUES_PER_SM
653 653 + asmComponent;
654 654 offsetASM = // NO TIME OFFSET
655 655 asmComponent * NB_BINS_PER_SM
656 656 + ASMIndexStart
657 657 + frequencyBin * nbBinsToAverage;
658 658 offsetFBin = ASMIndexStart
659 659 + frequencyBin * nbBinsToAverage;
660 660 compressed_spec_mat[ offsetCompressed ] = 0;
661 661 for ( k = 0; k < nbBinsToAverage; k++ )
662 662 {
663 663 fBinMask = getFBinMask( offsetFBin + k, channel );
664 664 compressed_spec_mat[offsetCompressed ] =
665 665 ( compressed_spec_mat[ offsetCompressed ]
666 666 + averaged_spec_mat[ offsetASM + k ] * fBinMask );
667 667 }
668 668 compressed_spec_mat[ offsetCompressed ] =
669 669 (divider != 0.) ? compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage) : 0.0;
670 670 }
671 671 }
672 672
673 673 }
674 674
675 675 int getFBinMask( int index, unsigned char channel )
676 676 {
677 677 unsigned int indexInChar;
678 678 unsigned int indexInTheChar;
679 679 int fbin;
680 680 unsigned char *sy_lfr_fbins_fx_word1;
681 681
682 682 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
683 683
684 684 switch(channel)
685 685 {
686 686 case 0:
687 687 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0;
688 688 break;
689 689 case 1:
690 690 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1;
691 691 break;
692 692 case 2:
693 693 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2;
694 694 break;
695 695 default:
696 696 PRINTF("ERR *** in getFBinMask, wrong frequency channel")
697 697 }
698 698
699 699 indexInChar = index >> 3;
700 700 indexInTheChar = index - indexInChar * 8;
701 701
702 702 fbin = (int) ((sy_lfr_fbins_fx_word1[ NB_BYTES_PER_FREQ_MASK - 1 - indexInChar] >> indexInTheChar) & 0x1);
703 703
704 704 return fbin;
705 705 }
706 706
707 707 unsigned char acquisitionTimeIsValid( unsigned int coarseTime, unsigned int fineTime, unsigned char channel)
708 708 {
709 709 u_int64_t acquisitionTime;
710 710 u_int64_t timecodeReference;
711 711 u_int64_t offsetInFineTime;
712 712 u_int64_t shiftInFineTime;
713 713 u_int64_t tBadInFineTime;
714 714 u_int64_t acquisitionTimeRangeMin;
715 715 u_int64_t acquisitionTimeRangeMax;
716 716 unsigned char pasFilteringIsEnabled;
717 717 unsigned char ret;
718 718
719 pasFilteringIsEnabled = (parameter_dump_packet.spare_sy_lfr_pas_filter_enabled & 0x01); // [0000 0001]
719 pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 0x01); // [0000 0001]
720 720 ret = 1;
721 721
722 //***************************
723 // <FOR TESTING PURPOSE ONLY>
724 unsigned char sy_lfr_pas_filter_modulus = 4;
725 unsigned char sy_lfr_pas_filter_offset = 1;
726 float sy_lfr_pas_filter_shift = 0.5;
727 float sy_lfr_pas_filter_tbad = 1.0;
728 // </FOR TESTING PURPOSE ONLY>
729 //****************************
730
731 722 // compute acquisition time from caoarseTime and fineTime
732 723 acquisitionTime = ( ((u_int64_t)coarseTime) << 16 )
733 724 + (u_int64_t) fineTime;
734 725
735 726 // compute the timecode reference
736 timecodeReference = (u_int64_t) (floor( ((double) coarseTime) / ((double) sy_lfr_pas_filter_modulus) )
737 * ((double) sy_lfr_pas_filter_modulus)) * 65536;
727 timecodeReference = (u_int64_t) (floor( ((double) coarseTime) / ((double) filterPar.sy_lfr_pas_filter_modulus) )
728 * ((double) filterPar.sy_lfr_pas_filter_modulus)) * 65536;
738 729
739 730 // compute the acquitionTime range
740 offsetInFineTime = ((double) sy_lfr_pas_filter_offset) * 65536;
741 shiftInFineTime = ((double) sy_lfr_pas_filter_shift) * 65536;
742 tBadInFineTime = ((double) sy_lfr_pas_filter_tbad) * 65536;
731 offsetInFineTime = ((double) filterPar.sy_lfr_pas_filter_offset) * 65536;
732 shiftInFineTime = ((double) filterPar.sy_lfr_pas_filter_shift) * 65536;
733 tBadInFineTime = ((double) filterPar.sy_lfr_pas_filter_tbad) * 65536;
743 734
744 735 acquisitionTimeRangeMin =
745 736 timecodeReference
746 737 + offsetInFineTime
747 738 + shiftInFineTime
748 739 - acquisitionDurations[channel];
749 740 acquisitionTimeRangeMax =
750 741 timecodeReference
751 742 + offsetInFineTime
752 743 + shiftInFineTime
753 744 + tBadInFineTime;
754 745
755 746 if ( (acquisitionTime >= acquisitionTimeRangeMin)
756 747 && (acquisitionTime <= acquisitionTimeRangeMax)
757 748 && (pasFilteringIsEnabled == 1) )
758 749 {
759 750 ret = 0; // the acquisition time is INSIDE the range, the matrix shall be ignored
760 751 }
761 752 else
762 753 {
763 754 ret = 1; // the acquisition time is OUTSIDE the range, the matrix can be used for the averaging
764 755 }
765 756
766 757 // printf("coarseTime = %x, fineTime = %x\n",
767 758 // coarseTime,
768 759 // fineTime);
769 760
770 761 // printf("[ret = %d] *** acquisitionTime = %f, Reference = %f",
771 762 // ret,
772 763 // acquisitionTime / 65536.,
773 764 // timecodeReference / 65536.);
774 765
775 766 // printf(", Min = %f, Max = %f\n",
776 767 // acquisitionTimeRangeMin / 65536.,
777 768 // acquisitionTimeRangeMax / 65536.);
778 769
779 770 return ret;
780 771 }
781 772
782 773 void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
783 774 {
784 775 unsigned char bin;
785 776 unsigned char kcoeff;
786 777
787 778 for (bin=0; bin<nb_bins_norm; bin++)
788 779 {
789 780 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
790 781 {
791 782 output_kcoeff[ (bin*NB_K_COEFF_PER_BIN + kcoeff)*2 ] = input_kcoeff[ bin*NB_K_COEFF_PER_BIN + kcoeff ];
792 783 output_kcoeff[ (bin*NB_K_COEFF_PER_BIN + kcoeff)*2 + 1 ] = input_kcoeff[ bin*NB_K_COEFF_PER_BIN + kcoeff ];
793 784 }
794 785 }
795 786 }
@@ -1,1524 +1,1540
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;
18 18 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2;
19 19 ring_node kcoefficient_node_1;
20 20 ring_node kcoefficient_node_2;
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+10, 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+10, 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+10, 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+10, 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+10, 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*0.25) ) - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0*0.25));
193 193 if (aux > FLOAT_EQUAL_ZERO)
194 194 {
195 195 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0+10, sy_lfr_s1_bp_p0 );
196 196 flag = LFR_DEFAULT;
197 197 }
198 198 }
199 199
200 200 // SET THE PARAMETERS
201 201 if (flag == LFR_SUCCESSFUL)
202 202 {
203 203 flag = set_sy_lfr_s1_bp_p0( TC );
204 204 flag = set_sy_lfr_s1_bp_p1( TC );
205 205 }
206 206
207 207 return flag;
208 208 }
209 209
210 210 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
211 211 {
212 212 /** This function updates the LFR registers with the incoming sbm2 parameters.
213 213 *
214 214 * @param TC points to the TeleCommand packet that is being processed
215 215 * @param queue_id is the id of the queue which handles TM related to this execution step
216 216 *
217 217 */
218 218
219 219 int flag;
220 220 rtems_status_code status;
221 221 unsigned char sy_lfr_s2_bp_p0;
222 222 unsigned char sy_lfr_s2_bp_p1;
223 223 float aux;
224 224
225 225 flag = LFR_SUCCESSFUL;
226 226
227 227 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
228 228 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
229 229 flag = LFR_DEFAULT;
230 230 }
231 231
232 232 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
233 233 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
234 234
235 235 // sy_lfr_s2_bp_p0
236 236 if (flag == LFR_SUCCESSFUL)
237 237 {
238 238 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
239 239 {
240 240 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
241 241 flag = WRONG_APP_DATA;
242 242 }
243 243 }
244 244 // sy_lfr_s2_bp_p1
245 245 if (flag == LFR_SUCCESSFUL)
246 246 {
247 247 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
248 248 {
249 249 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P1+10, sy_lfr_s2_bp_p1 );
250 250 flag = WRONG_APP_DATA;
251 251 }
252 252 }
253 253 //******************************************************************
254 254 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
255 255 if (flag == LFR_SUCCESSFUL)
256 256 {
257 257 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
258 258 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
259 259 aux = ( (float ) sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0 ) - floor(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
260 260 if (aux > FLOAT_EQUAL_ZERO)
261 261 {
262 262 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
263 263 flag = LFR_DEFAULT;
264 264 }
265 265 }
266 266
267 267 // SET THE PARAMETERS
268 268 if (flag == LFR_SUCCESSFUL)
269 269 {
270 270 flag = set_sy_lfr_s2_bp_p0( TC );
271 271 flag = set_sy_lfr_s2_bp_p1( TC );
272 272 }
273 273
274 274 return flag;
275 275 }
276 276
277 277 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
278 278 {
279 279 /** This function updates the LFR registers with the incoming sbm2 parameters.
280 280 *
281 281 * @param TC points to the TeleCommand packet that is being processed
282 282 * @param queue_id is the id of the queue which handles TM related to this execution step
283 283 *
284 284 */
285 285
286 286 int flag;
287 287
288 288 flag = LFR_DEFAULT;
289 289
290 290 flag = set_sy_lfr_kcoeff( TC, queue_id );
291 291
292 292 return flag;
293 293 }
294 294
295 295 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
296 296 {
297 297 /** This function updates the LFR registers with the incoming sbm2 parameters.
298 298 *
299 299 * @param TC points to the TeleCommand packet that is being processed
300 300 * @param queue_id is the id of the queue which handles TM related to this execution step
301 301 *
302 302 */
303 303
304 304 int flag;
305 305
306 306 flag = LFR_DEFAULT;
307 307
308 308 flag = set_sy_lfr_fbins( TC );
309 309
310 310 return flag;
311 311 }
312 312
313 313 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
314 314 {
315 315 /** This function updates the LFR registers with the incoming sbm2 parameters.
316 316 *
317 317 * @param TC points to the TeleCommand packet that is being processed
318 318 * @param queue_id is the id of the queue which handles TM related to this execution step
319 319 *
320 320 */
321 321
322 322 int flag;
323 323
324 324 flag = LFR_DEFAULT;
325 325
326 326 flag = check_sy_lfr_filter_parameters( TC, queue_id );
327 327
328 328 if (flag == LFR_SUCCESSFUL)
329 329 {
330 330 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ];
331 331 parameter_dump_packet.sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
332 332 parameter_dump_packet.sy_lfr_pas_filter_tbad[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 0 ];
333 333 parameter_dump_packet.sy_lfr_pas_filter_tbad[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 1 ];
334 334 parameter_dump_packet.sy_lfr_pas_filter_tbad[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 2 ];
335 335 parameter_dump_packet.sy_lfr_pas_filter_tbad[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 3 ];
336 336 parameter_dump_packet.sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
337 337 parameter_dump_packet.sy_lfr_pas_filter_shift[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 0 ];
338 338 parameter_dump_packet.sy_lfr_pas_filter_shift[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 1 ];
339 339 parameter_dump_packet.sy_lfr_pas_filter_shift[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 2 ];
340 340 parameter_dump_packet.sy_lfr_pas_filter_shift[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 3 ];
341 341 parameter_dump_packet.sy_lfr_sc_rw_delta_f[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 0 ];
342 342 parameter_dump_packet.sy_lfr_sc_rw_delta_f[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 1 ];
343 343 parameter_dump_packet.sy_lfr_sc_rw_delta_f[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 2 ];
344 344 parameter_dump_packet.sy_lfr_sc_rw_delta_f[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 3 ];
345 345
346 //****************************
347 // store PAS filter parameters
348 // sy_lfr_pas_filter_enabled
349 filterPar.spare_sy_lfr_pas_filter_enabled = parameter_dump_packet.spare_sy_lfr_pas_filter_enabled;
350 // sy_lfr_pas_filter_modulus
351 filterPar.sy_lfr_pas_filter_modulus = parameter_dump_packet.sy_lfr_pas_filter_modulus;
352 // sy_lfr_pas_filter_tbad
353 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_tbad,
354 parameter_dump_packet.sy_lfr_pas_filter_tbad );
355 // sy_lfr_pas_filter_offset
356 filterPar.sy_lfr_pas_filter_offset = parameter_dump_packet.sy_lfr_pas_filter_offset;
357 // sy_lfr_pas_filter_shift
358 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_shift,
359 parameter_dump_packet.sy_lfr_pas_filter_shift );
360
361 //****************************************************
346 362 // store the parameter sy_lfr_sc_rw_delta_f as a float
347 copyFloatByChar( (unsigned char*) &sy_lfr_sc_rw_delta_f,
348 (unsigned char*) &parameter_dump_packet.sy_lfr_sc_rw_delta_f[0] );
363 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f,
364 parameter_dump_packet.sy_lfr_sc_rw_delta_f );
349 365 }
350 366
351 367 return flag;
352 368 }
353 369
354 370 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
355 371 {
356 372 /** This function updates the LFR registers with the incoming sbm2 parameters.
357 373 *
358 374 * @param TC points to the TeleCommand packet that is being processed
359 375 * @param queue_id is the id of the queue which handles TM related to this execution step
360 376 *
361 377 */
362 378
363 379 unsigned int address;
364 380 rtems_status_code status;
365 381 unsigned int freq;
366 382 unsigned int bin;
367 383 unsigned int coeff;
368 384 unsigned char *kCoeffPtr;
369 385 unsigned char *kCoeffDumpPtr;
370 386
371 387 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
372 388 // F0 => 11 bins
373 389 // F1 => 13 bins
374 390 // F2 => 12 bins
375 391 // 36 bins to dump in two packets (30 bins max per packet)
376 392
377 393 //*********
378 394 // PACKET 1
379 395 // 11 F0 bins, 13 F1 bins and 6 F2 bins
380 396 kcoefficients_dump_1.destinationID = TC->sourceID;
381 397 increment_seq_counter_destination_id_dump( kcoefficients_dump_1.packetSequenceControl, TC->sourceID );
382 398 for( freq=0;
383 399 freq<NB_BINS_COMPRESSED_SM_F0;
384 400 freq++ )
385 401 {
386 402 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1] = freq;
387 403 bin = freq;
388 404 // printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
389 405 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
390 406 {
391 407 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
392 408 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
393 409 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
394 410 }
395 411 }
396 412 for( freq=NB_BINS_COMPRESSED_SM_F0;
397 413 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
398 414 freq++ )
399 415 {
400 416 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
401 417 bin = freq - NB_BINS_COMPRESSED_SM_F0;
402 418 // printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
403 419 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
404 420 {
405 421 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
406 422 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
407 423 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
408 424 }
409 425 }
410 426 for( freq=(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
411 427 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1+6);
412 428 freq++ )
413 429 {
414 430 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
415 431 bin = freq - (NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
416 432 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
417 433 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
418 434 {
419 435 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
420 436 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
421 437 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
422 438 }
423 439 }
424 440 kcoefficients_dump_1.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
425 441 kcoefficients_dump_1.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
426 442 kcoefficients_dump_1.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
427 443 kcoefficients_dump_1.time[3] = (unsigned char) (time_management_regs->coarse_time);
428 444 kcoefficients_dump_1.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
429 445 kcoefficients_dump_1.time[5] = (unsigned char) (time_management_regs->fine_time);
430 446 // SEND DATA
431 447 kcoefficient_node_1.status = 1;
432 448 address = (unsigned int) &kcoefficient_node_1;
433 449 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
434 450 if (status != RTEMS_SUCCESSFUL) {
435 451 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
436 452 }
437 453
438 454 //********
439 455 // PACKET 2
440 456 // 6 F2 bins
441 457 kcoefficients_dump_2.destinationID = TC->sourceID;
442 458 increment_seq_counter_destination_id_dump( kcoefficients_dump_2.packetSequenceControl, TC->sourceID );
443 459 for( freq=0; freq<6; freq++ )
444 460 {
445 461 kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + 6 + freq;
446 462 bin = freq + 6;
447 463 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
448 464 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
449 465 {
450 466 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
451 467 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
452 468 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
453 469 }
454 470 }
455 471 kcoefficients_dump_2.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
456 472 kcoefficients_dump_2.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
457 473 kcoefficients_dump_2.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
458 474 kcoefficients_dump_2.time[3] = (unsigned char) (time_management_regs->coarse_time);
459 475 kcoefficients_dump_2.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
460 476 kcoefficients_dump_2.time[5] = (unsigned char) (time_management_regs->fine_time);
461 477 // SEND DATA
462 478 kcoefficient_node_2.status = 1;
463 479 address = (unsigned int) &kcoefficient_node_2;
464 480 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
465 481 if (status != RTEMS_SUCCESSFUL) {
466 482 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
467 483 }
468 484
469 485 return status;
470 486 }
471 487
472 488 int action_dump_par( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
473 489 {
474 490 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
475 491 *
476 492 * @param queue_id is the id of the queue which handles TM related to this execution step.
477 493 *
478 494 * @return RTEMS directive status codes:
479 495 * - RTEMS_SUCCESSFUL - message sent successfully
480 496 * - RTEMS_INVALID_ID - invalid queue id
481 497 * - RTEMS_INVALID_SIZE - invalid message size
482 498 * - RTEMS_INVALID_ADDRESS - buffer is NULL
483 499 * - RTEMS_UNSATISFIED - out of message buffers
484 500 * - RTEMS_TOO_MANY - queue s limit has been reached
485 501 *
486 502 */
487 503
488 504 int status;
489 505
490 506 increment_seq_counter_destination_id_dump( parameter_dump_packet.packetSequenceControl, TC->sourceID );
491 507 parameter_dump_packet.destinationID = TC->sourceID;
492 508
493 509 // UPDATE TIME
494 510 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
495 511 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
496 512 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
497 513 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
498 514 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
499 515 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
500 516 // SEND DATA
501 517 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
502 518 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
503 519 if (status != RTEMS_SUCCESSFUL) {
504 520 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
505 521 }
506 522
507 523 return status;
508 524 }
509 525
510 526 //***********************
511 527 // NORMAL MODE PARAMETERS
512 528
513 529 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
514 530 {
515 531 unsigned char msb;
516 532 unsigned char lsb;
517 533 int flag;
518 534 float aux;
519 535 rtems_status_code status;
520 536
521 537 unsigned int sy_lfr_n_swf_l;
522 538 unsigned int sy_lfr_n_swf_p;
523 539 unsigned int sy_lfr_n_asm_p;
524 540 unsigned char sy_lfr_n_bp_p0;
525 541 unsigned char sy_lfr_n_bp_p1;
526 542 unsigned char sy_lfr_n_cwf_long_f3;
527 543
528 544 flag = LFR_SUCCESSFUL;
529 545
530 546 //***************
531 547 // get parameters
532 548 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
533 549 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
534 550 sy_lfr_n_swf_l = msb * 256 + lsb;
535 551
536 552 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
537 553 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
538 554 sy_lfr_n_swf_p = msb * 256 + lsb;
539 555
540 556 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
541 557 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
542 558 sy_lfr_n_asm_p = msb * 256 + lsb;
543 559
544 560 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
545 561
546 562 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
547 563
548 564 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
549 565
550 566 //******************
551 567 // check consistency
552 568 // sy_lfr_n_swf_l
553 569 if (sy_lfr_n_swf_l != 2048)
554 570 {
555 571 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L+10, sy_lfr_n_swf_l );
556 572 flag = WRONG_APP_DATA;
557 573 }
558 574 // sy_lfr_n_swf_p
559 575 if (flag == LFR_SUCCESSFUL)
560 576 {
561 577 if ( sy_lfr_n_swf_p < 22 )
562 578 {
563 579 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P+10, sy_lfr_n_swf_p );
564 580 flag = WRONG_APP_DATA;
565 581 }
566 582 }
567 583 // sy_lfr_n_bp_p0
568 584 if (flag == LFR_SUCCESSFUL)
569 585 {
570 586 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
571 587 {
572 588 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0+10, sy_lfr_n_bp_p0 );
573 589 flag = WRONG_APP_DATA;
574 590 }
575 591 }
576 592 // sy_lfr_n_asm_p
577 593 if (flag == LFR_SUCCESSFUL)
578 594 {
579 595 if (sy_lfr_n_asm_p == 0)
580 596 {
581 597 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
582 598 flag = WRONG_APP_DATA;
583 599 }
584 600 }
585 601 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
586 602 if (flag == LFR_SUCCESSFUL)
587 603 {
588 604 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
589 605 if (aux > FLOAT_EQUAL_ZERO)
590 606 {
591 607 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
592 608 flag = WRONG_APP_DATA;
593 609 }
594 610 }
595 611 // sy_lfr_n_bp_p1
596 612 if (flag == LFR_SUCCESSFUL)
597 613 {
598 614 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
599 615 {
600 616 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
601 617 flag = WRONG_APP_DATA;
602 618 }
603 619 }
604 620 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
605 621 if (flag == LFR_SUCCESSFUL)
606 622 {
607 623 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
608 624 if (aux > FLOAT_EQUAL_ZERO)
609 625 {
610 626 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
611 627 flag = LFR_DEFAULT;
612 628 }
613 629 }
614 630 // sy_lfr_n_cwf_long_f3
615 631
616 632 return flag;
617 633 }
618 634
619 635 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
620 636 {
621 637 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
622 638 *
623 639 * @param TC points to the TeleCommand packet that is being processed
624 640 * @param queue_id is the id of the queue which handles TM related to this execution step
625 641 *
626 642 */
627 643
628 644 int result;
629 645
630 646 result = LFR_SUCCESSFUL;
631 647
632 648 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
633 649 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
634 650
635 651 return result;
636 652 }
637 653
638 654 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
639 655 {
640 656 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
641 657 *
642 658 * @param TC points to the TeleCommand packet that is being processed
643 659 * @param queue_id is the id of the queue which handles TM related to this execution step
644 660 *
645 661 */
646 662
647 663 int result;
648 664
649 665 result = LFR_SUCCESSFUL;
650 666
651 667 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
652 668 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
653 669
654 670 return result;
655 671 }
656 672
657 673 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
658 674 {
659 675 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
660 676 *
661 677 * @param TC points to the TeleCommand packet that is being processed
662 678 * @param queue_id is the id of the queue which handles TM related to this execution step
663 679 *
664 680 */
665 681
666 682 int result;
667 683
668 684 result = LFR_SUCCESSFUL;
669 685
670 686 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
671 687 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
672 688
673 689 return result;
674 690 }
675 691
676 692 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
677 693 {
678 694 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
679 695 *
680 696 * @param TC points to the TeleCommand packet that is being processed
681 697 * @param queue_id is the id of the queue which handles TM related to this execution step
682 698 *
683 699 */
684 700
685 701 int status;
686 702
687 703 status = LFR_SUCCESSFUL;
688 704
689 705 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
690 706
691 707 return status;
692 708 }
693 709
694 710 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
695 711 {
696 712 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
697 713 *
698 714 * @param TC points to the TeleCommand packet that is being processed
699 715 * @param queue_id is the id of the queue which handles TM related to this execution step
700 716 *
701 717 */
702 718
703 719 int status;
704 720
705 721 status = LFR_SUCCESSFUL;
706 722
707 723 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
708 724
709 725 return status;
710 726 }
711 727
712 728 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
713 729 {
714 730 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
715 731 *
716 732 * @param TC points to the TeleCommand packet that is being processed
717 733 * @param queue_id is the id of the queue which handles TM related to this execution step
718 734 *
719 735 */
720 736
721 737 int status;
722 738
723 739 status = LFR_SUCCESSFUL;
724 740
725 741 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
726 742
727 743 return status;
728 744 }
729 745
730 746 //**********************
731 747 // BURST MODE PARAMETERS
732 748 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
733 749 {
734 750 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
735 751 *
736 752 * @param TC points to the TeleCommand packet that is being processed
737 753 * @param queue_id is the id of the queue which handles TM related to this execution step
738 754 *
739 755 */
740 756
741 757 int status;
742 758
743 759 status = LFR_SUCCESSFUL;
744 760
745 761 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
746 762
747 763 return status;
748 764 }
749 765
750 766 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
751 767 {
752 768 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
753 769 *
754 770 * @param TC points to the TeleCommand packet that is being processed
755 771 * @param queue_id is the id of the queue which handles TM related to this execution step
756 772 *
757 773 */
758 774
759 775 int status;
760 776
761 777 status = LFR_SUCCESSFUL;
762 778
763 779 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
764 780
765 781 return status;
766 782 }
767 783
768 784 //*********************
769 785 // SBM1 MODE PARAMETERS
770 786 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
771 787 {
772 788 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
773 789 *
774 790 * @param TC points to the TeleCommand packet that is being processed
775 791 * @param queue_id is the id of the queue which handles TM related to this execution step
776 792 *
777 793 */
778 794
779 795 int status;
780 796
781 797 status = LFR_SUCCESSFUL;
782 798
783 799 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
784 800
785 801 return status;
786 802 }
787 803
788 804 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
789 805 {
790 806 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
791 807 *
792 808 * @param TC points to the TeleCommand packet that is being processed
793 809 * @param queue_id is the id of the queue which handles TM related to this execution step
794 810 *
795 811 */
796 812
797 813 int status;
798 814
799 815 status = LFR_SUCCESSFUL;
800 816
801 817 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
802 818
803 819 return status;
804 820 }
805 821
806 822 //*********************
807 823 // SBM2 MODE PARAMETERS
808 824 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC )
809 825 {
810 826 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
811 827 *
812 828 * @param TC points to the TeleCommand packet that is being processed
813 829 * @param queue_id is the id of the queue which handles TM related to this execution step
814 830 *
815 831 */
816 832
817 833 int status;
818 834
819 835 status = LFR_SUCCESSFUL;
820 836
821 837 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
822 838
823 839 return status;
824 840 }
825 841
826 842 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
827 843 {
828 844 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
829 845 *
830 846 * @param TC points to the TeleCommand packet that is being processed
831 847 * @param queue_id is the id of the queue which handles TM related to this execution step
832 848 *
833 849 */
834 850
835 851 int status;
836 852
837 853 status = LFR_SUCCESSFUL;
838 854
839 855 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
840 856
841 857 return status;
842 858 }
843 859
844 860 //*******************
845 861 // TC_LFR_UPDATE_INFO
846 862 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
847 863 {
848 864 unsigned int status;
849 865
850 866 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
851 867 || (mode == LFR_MODE_BURST)
852 868 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
853 869 {
854 870 status = LFR_SUCCESSFUL;
855 871 }
856 872 else
857 873 {
858 874 status = LFR_DEFAULT;
859 875 }
860 876
861 877 return status;
862 878 }
863 879
864 880 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
865 881 {
866 882 unsigned int status;
867 883
868 884 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
869 885 || (mode == TDS_MODE_BURST)
870 886 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
871 887 || (mode == TDS_MODE_LFM))
872 888 {
873 889 status = LFR_SUCCESSFUL;
874 890 }
875 891 else
876 892 {
877 893 status = LFR_DEFAULT;
878 894 }
879 895
880 896 return status;
881 897 }
882 898
883 899 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
884 900 {
885 901 unsigned int status;
886 902
887 903 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
888 904 || (mode == THR_MODE_BURST))
889 905 {
890 906 status = LFR_SUCCESSFUL;
891 907 }
892 908 else
893 909 {
894 910 status = LFR_DEFAULT;
895 911 }
896 912
897 913 return status;
898 914 }
899 915
900 916 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC )
901 917 {
902 918 /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally.
903 919 *
904 920 * @param TC points to the TeleCommand packet that is being processed
905 921 *
906 922 */
907 923
908 924 unsigned char * bytePosPtr; // pointer to the beginning of the incoming TC packet
909 925
910 926 bytePosPtr = (unsigned char *) &TC->packetID;
911 927
912 928 // cp_rpw_sc_rw1_f1
913 929 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f1,
914 930 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] );
915 931
916 932 // cp_rpw_sc_rw1_f2
917 933 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f2,
918 934 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] );
919 935
920 936 // cp_rpw_sc_rw2_f1
921 937 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f1,
922 938 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] );
923 939
924 940 // cp_rpw_sc_rw2_f2
925 941 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f2,
926 942 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] );
927 943
928 944 // cp_rpw_sc_rw3_f1
929 945 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f1,
930 946 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] );
931 947
932 948 // cp_rpw_sc_rw3_f2
933 949 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f2,
934 950 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] );
935 951
936 952 // cp_rpw_sc_rw4_f1
937 953 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f1,
938 954 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] );
939 955
940 956 // cp_rpw_sc_rw4_f2
941 957 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f2,
942 958 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] );
943 959 }
944 960
945 961 void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag )
946 962 {
947 963 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
948 964 *
949 965 * @param fbins_mask
950 966 * @param rw_f is the reaction wheel frequency to filter
951 967 * @param delta_f is the frequency step between the frequency bins, it depends on the frequency channel
952 968 * @param flag [true] filtering enabled [false] filtering disabled
953 969 *
954 970 * @return void
955 971 *
956 972 */
957 973
958 974 float fmin;
959 975 float fMAX;
960 976 int binBelow;
961 977 int binAbove;
962 978 unsigned int whichByte;
963 979 unsigned char selectedByte;
964 980 int bin;
965 981
966 982 whichByte = 0;
967 983 bin = 0;
968 984
969 985 // compute the frequency range to filter [ rw_f - delta_f/2; rw_f + delta_f/2 ]
970 fmin = rw_f - sy_lfr_sc_rw_delta_f / 2.;
971 fMAX = rw_f + sy_lfr_sc_rw_delta_f / 2.;
986 fmin = rw_f - filterPar.sy_lfr_sc_rw_delta_f / 2.;
987 fMAX = rw_f + filterPar.sy_lfr_sc_rw_delta_f / 2.;
972 988
973 989 // compute the index of the frequency bin immediately below fmin
974 990 binBelow = (int) ( floor( ((double) fmin) / ((double) deltaFreq)) );
975 991
976 992 // compute the index of the frequency bin immediately above fMAX
977 993 binAbove = (int) ( floor( ((double) fMAX) / ((double) deltaFreq)) );
978 994
979 995 for (bin = binBelow; bin <= binAbove; bin++)
980 996 {
981 997 if ( (bin >= 0) && (bin<=127) )
982 998 {
983 999 if (flag == 1)
984 1000 {
985 1001 whichByte = bin >> 3; // division by 8
986 1002 selectedByte = (unsigned char) ( 1 << (bin - (whichByte * 8)) );
987 1003 fbins_mask[whichByte] = fbins_mask[whichByte] & (~selectedByte);
988 1004 }
989 1005 }
990 1006 }
991 1007 }
992 1008
993 1009 void build_sy_lfr_rw_mask( unsigned int channel )
994 1010 {
995 1011 unsigned char local_rw_fbins_mask[16];
996 1012 unsigned char *maskPtr;
997 1013 double deltaF;
998 1014 unsigned k;
999 1015
1000 1016 k = 0;
1001 1017
1002 1018 maskPtr = NULL;
1003 1019 deltaF = 1.;
1004 1020
1005 1021 switch (channel)
1006 1022 {
1007 1023 case 0:
1008 1024 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1009 1025 deltaF = 96.;
1010 1026 break;
1011 1027 case 1:
1012 1028 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1013 1029 deltaF = 16.;
1014 1030 break;
1015 1031 case 2:
1016 1032 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1017 1033 deltaF = 1.;
1018 1034 break;
1019 1035 default:
1020 1036 break;
1021 1037 }
1022 1038
1023 1039 for (k = 0; k < 16; k++)
1024 1040 {
1025 1041 local_rw_fbins_mask[k] = 0xff;
1026 1042 }
1027 1043
1028 1044 // RW1 F1
1029 1045 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x80) >> 7 ); // [1000 0000]
1030 1046
1031 1047 // RW1 F2
1032 1048 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x40) >> 6 ); // [0100 0000]
1033 1049
1034 1050 // RW2 F1
1035 1051 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x20) >> 5 ); // [0010 0000]
1036 1052
1037 1053 // RW2 F2
1038 1054 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x10) >> 4 ); // [0001 0000]
1039 1055
1040 1056 // RW3 F1
1041 1057 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x08) >> 3 ); // [0000 1000]
1042 1058
1043 1059 // RW3 F2
1044 1060 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x04) >> 2 ); // [0000 0100]
1045 1061
1046 1062 // RW4 F1
1047 1063 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x02) >> 1 ); // [0000 0010]
1048 1064
1049 1065 // RW4 F2
1050 1066 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x01) ); // [0000 0001]
1051 1067
1052 1068 // update the value of the fbins related to reaction wheels frequency filtering
1053 1069 if (maskPtr != NULL)
1054 1070 {
1055 1071 for (k = 0; k < 16; k++)
1056 1072 {
1057 1073 maskPtr[k] = local_rw_fbins_mask[k];
1058 1074 }
1059 1075 }
1060 1076 }
1061 1077
1062 1078 void build_sy_lfr_rw_masks( void )
1063 1079 {
1064 1080 build_sy_lfr_rw_mask( 0 );
1065 1081 build_sy_lfr_rw_mask( 1 );
1066 1082 build_sy_lfr_rw_mask( 2 );
1067 1083
1068 1084 merge_fbins_masks();
1069 1085 }
1070 1086
1071 1087 void merge_fbins_masks( void )
1072 1088 {
1073 1089 unsigned char k;
1074 1090
1075 1091 unsigned char *fbins_f0;
1076 1092 unsigned char *fbins_f1;
1077 1093 unsigned char *fbins_f2;
1078 1094 unsigned char *rw_mask_f0;
1079 1095 unsigned char *rw_mask_f1;
1080 1096 unsigned char *rw_mask_f2;
1081 1097
1082 1098 fbins_f0 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1083 1099 fbins_f1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
1084 1100 fbins_f2 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
1085 1101 rw_mask_f0 = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1086 1102 rw_mask_f1 = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1087 1103 rw_mask_f2 = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1088 1104
1089 1105 for( k=0; k < 16; k++ )
1090 1106 {
1091 1107 fbins_masks.merged_fbins_mask_f0[k] = fbins_f0[k] & rw_mask_f0[k];
1092 1108 fbins_masks.merged_fbins_mask_f1[k] = fbins_f1[k] & rw_mask_f1[k];
1093 1109 fbins_masks.merged_fbins_mask_f2[k] = fbins_f2[k] & rw_mask_f2[k];
1094 1110 }
1095 1111 }
1096 1112
1097 1113 //***********
1098 1114 // FBINS MASK
1099 1115
1100 1116 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
1101 1117 {
1102 1118 int status;
1103 1119 unsigned int k;
1104 1120 unsigned char *fbins_mask_dump;
1105 1121 unsigned char *fbins_mask_TC;
1106 1122
1107 1123 status = LFR_SUCCESSFUL;
1108 1124
1109 1125 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1110 1126 fbins_mask_TC = TC->dataAndCRC;
1111 1127
1112 1128 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
1113 1129 {
1114 1130 fbins_mask_dump[k] = fbins_mask_TC[k];
1115 1131 }
1116 1132
1117 1133 return status;
1118 1134 }
1119 1135
1120 1136 //***************************
1121 1137 // TC_LFR_LOAD_PAS_FILTER_PAR
1122 1138
1123 1139 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
1124 1140 {
1125 1141 int flag;
1126 1142 rtems_status_code status;
1127 1143
1128 1144 unsigned char sy_lfr_pas_filter_enabled;
1129 1145 unsigned char sy_lfr_pas_filter_modulus;
1130 1146 float sy_lfr_pas_filter_tbad;
1131 1147 unsigned char sy_lfr_pas_filter_offset;
1132 1148 float sy_lfr_pas_filter_shift;
1133 1149 float sy_lfr_sc_rw_delta_f;
1134 1150 char *parPtr;
1135 1151
1136 1152 flag = LFR_SUCCESSFUL;
1137 1153 sy_lfr_pas_filter_tbad = 0.0;
1138 1154 sy_lfr_pas_filter_shift = 0.0;
1139 1155 sy_lfr_sc_rw_delta_f = 0.0;
1140 1156 parPtr = NULL;
1141 1157
1142 1158 //***************
1143 1159 // get parameters
1144 1160 sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & 0x01; // [0000 0001]
1145 1161 sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
1146 1162 copyFloatByChar(
1147 1163 (unsigned char*) &sy_lfr_pas_filter_tbad,
1148 1164 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD ]
1149 1165 );
1150 1166 sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
1151 1167 copyFloatByChar(
1152 1168 (unsigned char*) &sy_lfr_pas_filter_shift,
1153 1169 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT ]
1154 1170 );
1155 1171 copyFloatByChar(
1156 1172 (unsigned char*) &sy_lfr_sc_rw_delta_f,
1157 1173 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F ]
1158 1174 );
1159 1175
1160 1176 //******************
1161 1177 // CHECK CONSISTENCY
1162 1178
1163 1179 //**************************
1164 1180 // sy_lfr_pas_filter_enabled
1165 1181 // nothing to check, value is 0 or 1
1166 1182
1167 1183 //**************************
1168 1184 // sy_lfr_pas_filter_modulus
1169 1185 if ( (sy_lfr_pas_filter_modulus < 4) || (sy_lfr_pas_filter_modulus > 8) )
1170 1186 {
1171 1187 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS+10, sy_lfr_pas_filter_modulus );
1172 1188 flag = WRONG_APP_DATA;
1173 1189 }
1174 1190
1175 1191 //***********************
1176 1192 // sy_lfr_pas_filter_tbad
1177 1193 if ( (sy_lfr_pas_filter_tbad < 0.0) || (sy_lfr_pas_filter_tbad > 4.0) )
1178 1194 {
1179 1195 parPtr = (char*) &sy_lfr_pas_filter_tbad;
1180 1196 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD+10, parPtr[3] );
1181 1197 flag = WRONG_APP_DATA;
1182 1198 }
1183 1199
1184 1200 //*************************
1185 1201 // sy_lfr_pas_filter_offset
1186 1202 if (flag == LFR_SUCCESSFUL)
1187 1203 {
1188 1204 if ( (sy_lfr_pas_filter_offset < 0) || (sy_lfr_pas_filter_offset > 7) )
1189 1205 {
1190 1206 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET+10, sy_lfr_pas_filter_offset );
1191 1207 flag = WRONG_APP_DATA;
1192 1208 }
1193 1209 }
1194 1210
1195 1211 //************************
1196 1212 // sy_lfr_pas_filter_shift
1197 1213 if ( (sy_lfr_pas_filter_shift < 0.0) || (sy_lfr_pas_filter_shift > 1.0) )
1198 1214 {
1199 1215 parPtr = (char*) &sy_lfr_pas_filter_shift;
1200 1216 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT+10, parPtr[3] );
1201 1217 flag = WRONG_APP_DATA;
1202 1218 }
1203 1219
1204 1220 //*********************
1205 1221 // sy_lfr_sc_rw_delta_f
1206 1222 // nothing to check, no default value in the ICD
1207 1223
1208 1224 return flag;
1209 1225 }
1210 1226
1211 1227 //**************
1212 1228 // KCOEFFICIENTS
1213 1229 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC,rtems_id queue_id )
1214 1230 {
1215 1231 unsigned int kcoeff;
1216 1232 unsigned short sy_lfr_kcoeff_frequency;
1217 1233 unsigned short bin;
1218 1234 unsigned short *freqPtr;
1219 1235 float *kcoeffPtr_norm;
1220 1236 float *kcoeffPtr_sbm;
1221 1237 int status;
1222 1238 unsigned char *kcoeffLoadPtr;
1223 1239 unsigned char *kcoeffNormPtr;
1224 1240 unsigned char *kcoeffSbmPtr_a;
1225 1241 unsigned char *kcoeffSbmPtr_b;
1226 1242
1227 1243 status = LFR_SUCCESSFUL;
1228 1244
1229 1245 kcoeffPtr_norm = NULL;
1230 1246 kcoeffPtr_sbm = NULL;
1231 1247 bin = 0;
1232 1248
1233 1249 freqPtr = (unsigned short *) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY];
1234 1250 sy_lfr_kcoeff_frequency = *freqPtr;
1235 1251
1236 1252 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
1237 1253 {
1238 1254 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
1239 1255 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 10 + 1,
1240 1256 TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 1] ); // +1 to get the LSB instead of the MSB
1241 1257 status = LFR_DEFAULT;
1242 1258 }
1243 1259 else
1244 1260 {
1245 1261 if ( ( sy_lfr_kcoeff_frequency >= 0 )
1246 1262 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
1247 1263 {
1248 1264 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
1249 1265 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
1250 1266 bin = sy_lfr_kcoeff_frequency;
1251 1267 }
1252 1268 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
1253 1269 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
1254 1270 {
1255 1271 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
1256 1272 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
1257 1273 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
1258 1274 }
1259 1275 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
1260 1276 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
1261 1277 {
1262 1278 kcoeffPtr_norm = k_coeff_intercalib_f2;
1263 1279 kcoeffPtr_sbm = NULL;
1264 1280 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
1265 1281 }
1266 1282 }
1267 1283
1268 1284 if (kcoeffPtr_norm != NULL ) // update K coefficient for NORMAL data products
1269 1285 {
1270 1286 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1271 1287 {
1272 1288 // destination
1273 1289 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + kcoeff ];
1274 1290 // source
1275 1291 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * kcoeff];
1276 1292 // copy source to destination
1277 1293 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
1278 1294 }
1279 1295 }
1280 1296
1281 1297 if (kcoeffPtr_sbm != NULL ) // update K coefficient for SBM data products
1282 1298 {
1283 1299 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1284 1300 {
1285 1301 // destination
1286 1302 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * 2 ];
1287 1303 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * 2 + 1 ];
1288 1304 // source
1289 1305 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * kcoeff];
1290 1306 // copy source to destination
1291 1307 copyFloatByChar( kcoeffSbmPtr_a, kcoeffLoadPtr );
1292 1308 copyFloatByChar( kcoeffSbmPtr_b, kcoeffLoadPtr );
1293 1309 }
1294 1310 }
1295 1311
1296 1312 // print_k_coeff();
1297 1313
1298 1314 return status;
1299 1315 }
1300 1316
1301 1317 void copyFloatByChar( unsigned char *destination, unsigned char *source )
1302 1318 {
1303 1319 destination[0] = source[0];
1304 1320 destination[1] = source[1];
1305 1321 destination[2] = source[2];
1306 1322 destination[3] = source[3];
1307 1323 }
1308 1324
1309 1325 //**********
1310 1326 // init dump
1311 1327
1312 1328 void init_parameter_dump( void )
1313 1329 {
1314 1330 /** This function initialize the parameter_dump_packet global variable with default values.
1315 1331 *
1316 1332 */
1317 1333
1318 1334 unsigned int k;
1319 1335
1320 1336 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
1321 1337 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
1322 1338 parameter_dump_packet.reserved = CCSDS_RESERVED;
1323 1339 parameter_dump_packet.userApplication = CCSDS_USER_APP;
1324 1340 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);
1325 1341 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1326 1342 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1327 1343 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1328 1344 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> 8);
1329 1345 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
1330 1346 // DATA FIELD HEADER
1331 1347 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1332 1348 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
1333 1349 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
1334 1350 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
1335 1351 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
1336 1352 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
1337 1353 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
1338 1354 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
1339 1355 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
1340 1356 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
1341 1357 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
1342 1358
1343 1359 //******************
1344 1360 // COMMON PARAMETERS
1345 1361 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
1346 1362 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
1347 1363
1348 1364 //******************
1349 1365 // NORMAL PARAMETERS
1350 1366 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> 8);
1351 1367 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1352 1368 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> 8);
1353 1369 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1354 1370 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> 8);
1355 1371 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1356 1372 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1357 1373 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1358 1374 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1359 1375
1360 1376 //*****************
1361 1377 // BURST PARAMETERS
1362 1378 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1363 1379 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1364 1380
1365 1381 //****************
1366 1382 // SBM1 PARAMETERS
1367 1383 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
1368 1384 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1369 1385
1370 1386 //****************
1371 1387 // SBM2 PARAMETERS
1372 1388 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1373 1389 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1374 1390
1375 1391 //************
1376 1392 // FBINS MASKS
1377 1393 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
1378 1394 {
1379 1395 parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = 0xff;
1380 1396 }
1381 1397 }
1382 1398
1383 1399 void init_kcoefficients_dump( void )
1384 1400 {
1385 1401 init_kcoefficients_dump_packet( &kcoefficients_dump_1, 1, 30 );
1386 1402 init_kcoefficients_dump_packet( &kcoefficients_dump_2, 2, 6 );
1387 1403
1388 1404 kcoefficient_node_1.previous = NULL;
1389 1405 kcoefficient_node_1.next = NULL;
1390 1406 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1391 1407 kcoefficient_node_1.coarseTime = 0x00;
1392 1408 kcoefficient_node_1.fineTime = 0x00;
1393 1409 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1394 1410 kcoefficient_node_1.status = 0x00;
1395 1411
1396 1412 kcoefficient_node_2.previous = NULL;
1397 1413 kcoefficient_node_2.next = NULL;
1398 1414 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1399 1415 kcoefficient_node_2.coarseTime = 0x00;
1400 1416 kcoefficient_node_2.fineTime = 0x00;
1401 1417 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1402 1418 kcoefficient_node_2.status = 0x00;
1403 1419 }
1404 1420
1405 1421 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1406 1422 {
1407 1423 unsigned int k;
1408 1424 unsigned int packetLength;
1409 1425
1410 1426 packetLength = blk_nr * 130 + 20 - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1411 1427
1412 1428 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1413 1429 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1414 1430 kcoefficients_dump->reserved = CCSDS_RESERVED;
1415 1431 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1416 1432 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);;
1417 1433 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;;
1418 1434 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1419 1435 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1420 1436 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> 8);
1421 1437 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1422 1438 // DATA FIELD HEADER
1423 1439 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1424 1440 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1425 1441 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1426 1442 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1427 1443 kcoefficients_dump->time[0] = 0x00;
1428 1444 kcoefficients_dump->time[1] = 0x00;
1429 1445 kcoefficients_dump->time[2] = 0x00;
1430 1446 kcoefficients_dump->time[3] = 0x00;
1431 1447 kcoefficients_dump->time[4] = 0x00;
1432 1448 kcoefficients_dump->time[5] = 0x00;
1433 1449 kcoefficients_dump->sid = SID_K_DUMP;
1434 1450
1435 1451 kcoefficients_dump->pkt_cnt = 2;
1436 1452 kcoefficients_dump->pkt_nr = pkt_nr;
1437 1453 kcoefficients_dump->blk_nr = blk_nr;
1438 1454
1439 1455 //******************
1440 1456 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1441 1457 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1442 1458 for (k=0; k<3900; k++)
1443 1459 {
1444 1460 kcoefficients_dump->kcoeff_blks[k] = 0x00;
1445 1461 }
1446 1462 }
1447 1463
1448 1464 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id )
1449 1465 {
1450 1466 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
1451 1467 *
1452 1468 * @param packet_sequence_control points to the packet sequence control which will be incremented
1453 1469 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
1454 1470 *
1455 1471 * If the destination ID is not known, a dedicated counter is incremented.
1456 1472 *
1457 1473 */
1458 1474
1459 1475 unsigned short sequence_cnt;
1460 1476 unsigned short segmentation_grouping_flag;
1461 1477 unsigned short new_packet_sequence_control;
1462 1478 unsigned char i;
1463 1479
1464 1480 switch (destination_id)
1465 1481 {
1466 1482 case SID_TC_GROUND:
1467 1483 i = GROUND;
1468 1484 break;
1469 1485 case SID_TC_MISSION_TIMELINE:
1470 1486 i = MISSION_TIMELINE;
1471 1487 break;
1472 1488 case SID_TC_TC_SEQUENCES:
1473 1489 i = TC_SEQUENCES;
1474 1490 break;
1475 1491 case SID_TC_RECOVERY_ACTION_CMD:
1476 1492 i = RECOVERY_ACTION_CMD;
1477 1493 break;
1478 1494 case SID_TC_BACKUP_MISSION_TIMELINE:
1479 1495 i = BACKUP_MISSION_TIMELINE;
1480 1496 break;
1481 1497 case SID_TC_DIRECT_CMD:
1482 1498 i = DIRECT_CMD;
1483 1499 break;
1484 1500 case SID_TC_SPARE_GRD_SRC1:
1485 1501 i = SPARE_GRD_SRC1;
1486 1502 break;
1487 1503 case SID_TC_SPARE_GRD_SRC2:
1488 1504 i = SPARE_GRD_SRC2;
1489 1505 break;
1490 1506 case SID_TC_OBCP:
1491 1507 i = OBCP;
1492 1508 break;
1493 1509 case SID_TC_SYSTEM_CONTROL:
1494 1510 i = SYSTEM_CONTROL;
1495 1511 break;
1496 1512 case SID_TC_AOCS:
1497 1513 i = AOCS;
1498 1514 break;
1499 1515 case SID_TC_RPW_INTERNAL:
1500 1516 i = RPW_INTERNAL;
1501 1517 break;
1502 1518 default:
1503 1519 i = GROUND;
1504 1520 break;
1505 1521 }
1506 1522
1507 1523 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
1508 1524 sequence_cnt = sequenceCounters_TM_DUMP[ i ] & 0x3fff;
1509 1525
1510 1526 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
1511 1527
1512 1528 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
1513 1529 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1514 1530
1515 1531 // increment the sequence counter
1516 1532 if ( sequenceCounters_TM_DUMP[ i ] < SEQ_CNT_MAX )
1517 1533 {
1518 1534 sequenceCounters_TM_DUMP[ i ] = sequenceCounters_TM_DUMP[ i ] + 1;
1519 1535 }
1520 1536 else
1521 1537 {
1522 1538 sequenceCounters_TM_DUMP[ i ] = 0;
1523 1539 }
1524 1540 }
General Comments 0
You need to be logged in to leave comments. Login now