##// END OF EJS Templates
AVGV modified...
paul -
r352:c07c16776bd4 R3++ draft
parent child
Show More
@@ -1,2 +1,2
1 1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
2 e01ac8bd125a79a7af38b0e3ba0330f5be1a3c92 header/lfr_common_headers
2 7ee7da2ed42fbc9cd673ae7f3a865345cea0f83f header/lfr_common_headers
@@ -1,238 +1,235
1 1 #ifndef GRLIB_REGS_H_INCLUDED
2 2 #define GRLIB_REGS_H_INCLUDED
3 3
4 4 #define NB_GPTIMER 3
5 5
6 6 #include <stdint.h>
7 7
8 8 struct apbuart_regs_str{
9 9 volatile unsigned int data;
10 10 volatile unsigned int status;
11 11 volatile unsigned int ctrl;
12 12 volatile unsigned int scaler;
13 13 volatile unsigned int fifoDebug;
14 14 };
15 15
16 16 struct grgpio_regs_str{
17 17 volatile int io_port_data_register;
18 18 int io_port_output_register;
19 19 int io_port_direction_register;
20 20 int interrupt_mak_register;
21 21 int interrupt_polarity_register;
22 22 int interrupt_edge_register;
23 23 int bypass_register;
24 24 int reserved;
25 25 // 0x20-0x3c interrupt map register(s)
26 26 };
27 27
28 28 typedef struct {
29 29 volatile unsigned int counter;
30 30 volatile unsigned int reload;
31 31 volatile unsigned int ctrl;
32 32 volatile unsigned int unused;
33 33 } timer_regs_t;
34 34
35 35 //*************
36 36 //*************
37 37 // GPTIMER_REGS
38 38
39 39 #define GPTIMER_CLEAR_IRQ 0x00000010 // clear pending IRQ if any
40 40 #define GPTIMER_LD 0x00000004 // LD load value from the reload register
41 41 #define GPTIMER_EN 0x00000001 // EN enable the timer
42 42 #define GPTIMER_EN_MASK 0xfffffffe // EN enable the timer
43 43 #define GPTIMER_RS 0x00000002 // RS restart
44 44 #define GPTIMER_IE 0x00000008 // IE interrupt enable
45 45 #define GPTIMER_IE_MASK 0xffffffef // IE interrupt enable
46 46
47 47 typedef struct {
48 48 volatile unsigned int scaler_value;
49 49 volatile unsigned int scaler_reload;
50 50 volatile unsigned int conf;
51 51 volatile unsigned int unused0;
52 52 timer_regs_t timer[NB_GPTIMER];
53 53 } gptimer_regs_t;
54 54
55 55 //*********************
56 56 //*********************
57 57 // TIME_MANAGEMENT_REGS
58 58
59 59 #define VAL_SOFTWARE_RESET 0x02 // [0010] software reset
60 60 #define VAL_LFR_SYNCHRONIZED 0x80000000
61 61 #define BIT_SYNCHRONIZATION 31
62 62 #define COARSE_TIME_MASK 0x7fffffff
63 63 #define SYNC_BIT_MASK 0x7f
64 64 #define SYNC_BIT 0x80
65 65 #define BIT_CAL_RELOAD 0x00000010
66 66 #define MASK_CAL_RELOAD 0xffffffef // [1110 1111]
67 67 #define BIT_CAL_ENABLE 0x00000040
68 68 #define MASK_CAL_ENABLE 0xffffffbf // [1011 1111]
69 69 #define BIT_SET_INTERLEAVED 0x00000020 // [0010 0000]
70 70 #define MASK_SET_INTERLEAVED 0xffffffdf // [1101 1111]
71 71 #define BIT_SOFT_RESET 0x00000004 // [0100]
72 72 #define MASK_SOFT_RESET 0xfffffffb // [1011]
73 73
74 74 typedef struct {
75 75 volatile int ctrl; // bit 0 forces the load of the coarse_time_load value and resets the fine_time
76 76 // bit 1 is the soft reset for the time management module
77 77 // bit 2 is the soft reset for the waveform picker and the spectral matrix modules, set to 1 after HW reset
78 78 volatile int coarse_time_load;
79 79 volatile int coarse_time;
80 80 volatile int fine_time;
81 81 // TEMPERATURES
82 82 volatile int temp_pcb; // SEL1 = 0 SEL0 = 0
83 83 volatile int temp_fpga; // SEL1 = 0 SEL0 = 1
84 84 volatile int temp_scm; // SEL1 = 1 SEL0 = 0
85 85 // CALIBRATION
86 86 volatile unsigned int calDACCtrl;
87 87 volatile unsigned int calPrescaler;
88 88 volatile unsigned int calDivisor;
89 89 volatile unsigned int calDataPtr;
90 90 volatile unsigned int calData;
91 91 } time_management_regs_t;
92 92
93 93 //*********************
94 94 //*********************
95 95 // WAVEFORM_PICKER_REGS
96 96
97 97 #define BITS_WFP_STATUS_F3 0xc0 // [1100 0000] check the f3 full bits
98 98 #define BIT_WFP_BUF_F3_0 0x40 // [0100 0000] f3 buffer 0 is full
99 99 #define BIT_WFP_BUF_F3_1 0x80 // [1000 0000] f3 buffer 1 is full
100 100 #define RST_WFP_F3_0 0x00008840 // [1000 1000 0100 0000]
101 101 #define RST_WFP_F3_1 0x00008880 // [1000 1000 1000 0000]
102 102
103 103 #define BITS_WFP_STATUS_F2 0x30 // [0011 0000] get the status bits for f2
104 104 #define SHIFT_WFP_STATUS_F2 4
105 105 #define BIT_WFP_BUF_F2_0 0x10 // [0001 0000] f2 buffer 0 is full
106 106 #define BIT_WFP_BUF_F2_1 0x20 // [0010 0000] f2 buffer 1 is full
107 107 #define RST_WFP_F2_0 0x00004410 // [0100 0100 0001 0000]
108 108 #define RST_WFP_F2_1 0x00004420 // [0100 0100 0010 0000]
109 109
110 110 #define BITS_WFP_STATUS_F1 0x0c // [0000 1100] check the f1 full bits
111 111 #define BIT_WFP_BUF_F1_0 0x04 // [0000 0100] f1 buffer 0 is full
112 112 #define BIT_WFP_BUF_F1_1 0x08 // [0000 1000] f1 buffer 1 is full
113 113 #define RST_WFP_F1_0 0x00002204 // [0010 0010 0000 0100] f1 bits = 0
114 114 #define RST_WFP_F1_1 0x00002208 // [0010 0010 0000 1000] f1 bits = 0
115 115
116 116 #define BITS_WFP_STATUS_F0 0x03 // [0000 0011] check the f0 full bits
117 117 #define RST_WFP_F0_0 0x00001101 // [0001 0001 0000 0001]
118 118 #define RST_WFP_F0_1 0x00001102 // [0001 0001 0000 0010]
119 119
120 120 #define BIT_WFP_BUFFER_0 0x01
121 121 #define BIT_WFP_BUFFER_1 0x02
122 122
123 123 #define RST_BITS_RUN_BURST_EN 0x80 // [1000 0000] burst f2, f1, f0 enable f3, f2, f1, f0
124 124 #define BITS_WFP_ENABLE_ALL 0x0f // [0000 1111] enable f3, f2, f1, f0
125 125 #define BITS_WFP_ENABLE_BURST 0x0c // [0000 1100] enable f3, f2
126 126 #define RUN_BURST_ENABLE_SBM2 0x60 // [0110 0000] enable f2 and f1 burst
127 127 #define RUN_BURST_ENABLE_BURST 0x40 // [0100 0000] f2 burst enabled
128 128
129 129 #define DFLT_WFP_NB_DATA_BY_BUFFER 0xa7f // 0x30 *** 2688 - 1 => nb samples -1
130 130 #define DFLT_WFP_SNAPSHOT_PARAM 0xa80 // 0x34 *** 2688 => nb samples
131 131 #define DFLT_WFP_BUFFER_LENGTH 0x1f8 // buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
132 132 #define DFLT_WFP_DELTA_F0_2 0x30 // 48 = 11 0000, max 7 bits
133 133
134 134 // PDB >= 0.1.28, 0x80000f54
135 135 typedef struct{
136 136 int data_shaping; // 0x00 00 *** R2 R1 R0 SP1 SP0 BW
137 137 int run_burst_enable; // 0x04 01 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
138 138 int addr_data_f0_0; // 0x08
139 139 int addr_data_f0_1; // 0x0c
140 140 int addr_data_f1_0; // 0x10
141 141 int addr_data_f1_1; // 0x14
142 142 int addr_data_f2_0; // 0x18
143 143 int addr_data_f2_1; // 0x1c
144 144 int addr_data_f3_0; // 0x20
145 145 int addr_data_f3_1; // 0x24
146 146 volatile int status; // 0x28
147 147 volatile int delta_snapshot; // 0x2c
148 148 int delta_f0; // 0x30
149 149 int delta_f0_2; // 0x34
150 150 int delta_f1; // 0x38
151 151 int delta_f2; // 0x3c
152 152 int nb_data_by_buffer; // 0x40 number of samples in a buffer = 2688
153 153 int snapshot_param; // 0x44
154 154 int start_date; // 0x48
155 155 //
156 156 volatile unsigned int f0_0_coarse_time; // 0x4c
157 157 volatile unsigned int f0_0_fine_time; // 0x50
158 158 volatile unsigned int f0_1_coarse_time; // 0x54
159 159 volatile unsigned int f0_1_fine_time; // 0x58
160 160 //
161 161 volatile unsigned int f1_0_coarse_time; // 0x5c
162 162 volatile unsigned int f1_0_fine_time; // 0x60
163 163 volatile unsigned int f1_1_coarse_time; // 0x64
164 164 volatile unsigned int f1_1_fine_time; // 0x68
165 165 //
166 166 volatile unsigned int f2_0_coarse_time; // 0x6c
167 167 volatile unsigned int f2_0_fine_time; // 0x70
168 168 volatile unsigned int f2_1_coarse_time; // 0x74
169 169 volatile unsigned int f2_1_fine_time; // 0x78
170 170 //
171 171 volatile unsigned int f3_0_coarse_time; // 0x7c => 0x7c + 0xf54 = 0xd0
172 172 volatile unsigned int f3_0_fine_time; // 0x80
173 173 volatile unsigned int f3_1_coarse_time; // 0x84
174 174 volatile unsigned int f3_1_fine_time; // 0x88
175 175 //
176 176 unsigned int buffer_length; // 0x8c = buffer length in burst 2688 / 16 = 168
177 177 //
178 volatile int16_t v_dummy; // 0x90
179 volatile int16_t v; // 0x90
180 volatile int16_t e1_dummy; // 0x94
181 volatile int16_t e1; // 0x94
182 volatile int16_t e2_dummy; // 0x98
183 volatile int16_t e2; // 0x98
178 volatile int v; // 0x90
179 volatile int e1; // 0x94
180 volatile int e2; // 0x98
184 181 } waveform_picker_regs_0_1_18_t;
185 182
186 183 //*********************
187 184 //*********************
188 185 // SPECTRAL_MATRIX_REGS
189 186
190 187 #define BITS_STATUS_F0 0x03 // [0011]
191 188 #define BITS_STATUS_F1 0x0c // [1100]
192 189 #define BITS_STATUS_F2 0x30 // [0011 0000]
193 190 #define BITS_HK_AA_SM 0x780 // [0111 1000 0000]
194 191 #define BITS_SM_ERR 0x7c0 // [0111 1100 0000]
195 192 #define BITS_STATUS_REG 0x7ff // [0111 1111 1111]
196 193 #define BIT_READY_0 0x1 // [01]
197 194 #define BIT_READY_1 0x2 // [10]
198 195 #define BIT_READY_0_1 0x3 // [11]
199 196 #define BIT_STATUS_F1_0 0x04 // [0100]
200 197 #define BIT_STATUS_F1_1 0x08 // [1000]
201 198 #define BIT_STATUS_F2_0 0x10 // [0001 0000]
202 199 #define BIT_STATUS_F2_1 0x20 // [0010 0000]
203 200 #define DEFAULT_MATRIX_LENGTH 0xc8 // 25 * 128 / 16 = 200 = 0xc8
204 201 #define BIT_IRQ_ON_NEW_MATRIX 0x01
205 202 #define MASK_IRQ_ON_NEW_MATRIX 0xfffffffe
206 203 #define BIT_IRQ_ON_ERROR 0x02
207 204 #define MASK_IRQ_ON_ERROR 0xfffffffd
208 205
209 206 typedef struct {
210 207 volatile int config; // 0x00
211 208 volatile int status; // 0x04
212 209 volatile int f0_0_address; // 0x08
213 210 volatile int f0_1_address; // 0x0C
214 211 //
215 212 volatile int f1_0_address; // 0x10
216 213 volatile int f1_1_address; // 0x14
217 214 volatile int f2_0_address; // 0x18
218 215 volatile int f2_1_address; // 0x1C
219 216 //
220 217 volatile unsigned int f0_0_coarse_time; // 0x20
221 218 volatile unsigned int f0_0_fine_time; // 0x24
222 219 volatile unsigned int f0_1_coarse_time; // 0x28
223 220 volatile unsigned int f0_1_fine_time; // 0x2C
224 221 //
225 222 volatile unsigned int f1_0_coarse_time; // 0x30
226 223 volatile unsigned int f1_0_fine_time; // 0x34
227 224 volatile unsigned int f1_1_coarse_time; // 0x38
228 225 volatile unsigned int f1_1_fine_time; // 0x3C
229 226 //
230 227 volatile unsigned int f2_0_coarse_time; // 0x40
231 228 volatile unsigned int f2_0_fine_time; // 0x44
232 229 volatile unsigned int f2_1_coarse_time; // 0x48
233 230 volatile unsigned int f2_1_fine_time; // 0x4C
234 231 //
235 232 unsigned int matrix_length; // 0x50, length of a spectral matrix in burst 3200 / 16 = 200 = 0xc8
236 233 } spectral_matrix_regs_t;
237 234
238 235 #endif // GRLIB_REGS_H_INCLUDED
@@ -1,122 +1,124
1 1 #ifndef TC_LOAD_DUMP_PARAMETERS_H
2 2 #define TC_LOAD_DUMP_PARAMETERS_H
3 3
4 4 #include <rtems.h>
5 5 #include <stdio.h>
6 6
7 7 #include "fsw_params.h"
8 8 #include "wf_handler.h"
9 9 #include "tm_lfr_tc_exe.h"
10 10 #include "fsw_misc.h"
11 11 #include "basic_parameters_params.h"
12 12 #include "avf0_prc0.h"
13 13
14 14 #define FLOAT_EQUAL_ZERO 0.001
15 15 #define NB_BINS_TO_REMOVE 3
16 16 #define FI_INTERVAL_COEFF 0.285
17 17 #define BIN_MIN 0
18 18 #define BIN_MAX 127
19 19 #define DELTAF_F0 96.
20 20 #define DELTAF_F1 16.
21 21 #define DELTAF_F2 1.
22 22 #define DELTAF_DIV 2.
23 23
24 24 #define BIT_RW1_F1 0x80
25 25 #define BIT_RW1_F2 0x40
26 26 #define BIT_RW2_F1 0x20
27 27 #define BIT_RW2_F2 0x10
28 28 #define BIT_RW3_F1 0x08
29 29 #define BIT_RW3_F2 0x04
30 30 #define BIT_RW4_F1 0x02
31 31 #define BIT_RW4_F2 0x01
32 32
33 33 #define WHEEL_1 1
34 34 #define WHEEL_2 2
35 35 #define WHEEL_3 3
36 36 #define WHEEL_4 4
37 37 #define FREQ_1 1
38 38 #define FREQ_2 2
39 39 #define FREQ_3 3
40 40 #define FREQ_4 4
41 41 #define FLAG_OFFSET_WHEELS_1_3 8
42 42 #define FLAG_OFFSET_WHEELS_2_4 4
43 43
44 44 #define FLAG_NAN 0 // Not A NUMBER
45 45 #define FLAG_IAN 1 // Is A Number
46 46
47 47 #define SBM_KCOEFF_PER_NORM_KCOEFF 2
48 48
49 49 extern unsigned short sequenceCounterParameterDump;
50 50 extern unsigned short sequenceCounters_TM_DUMP[];
51 51 extern float k_coeff_intercalib_f0_norm[ ];
52 52 extern float k_coeff_intercalib_f0_sbm[ ];
53 53 extern float k_coeff_intercalib_f1_norm[ ];
54 54 extern float k_coeff_intercalib_f1_sbm[ ];
55 55 extern float k_coeff_intercalib_f2[ ];
56 56 extern fbins_masks_t fbins_masks;
57 57
58 58 int action_load_common_par( ccsdsTelecommandPacket_t *TC );
59 59 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
60 60 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
61 61 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
62 62 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
63 63 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
64 64 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
65 65 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
66 66 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
67 67 int action_dump_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
68 68
69 69 // NORMAL
70 70 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
71 71 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC );
72 72 int set_sy_lfr_n_swf_p( ccsdsTelecommandPacket_t *TC );
73 73 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC );
74 74 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC );
75 75 int set_sy_lfr_n_bp_p1( ccsdsTelecommandPacket_t *TC );
76 76 int set_sy_lfr_n_cwf_long_f3( ccsdsTelecommandPacket_t *TC );
77 77
78 78 // BURST
79 79 int set_sy_lfr_b_bp_p0( ccsdsTelecommandPacket_t *TC );
80 80 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC );
81 81
82 82 // SBM1
83 83 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC );
84 84 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC );
85 85
86 86 // SBM2
87 87 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC );
88 88 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC );
89 89
90 90 // TC_LFR_UPDATE_INFO
91 91 unsigned int check_update_info_hk_lfr_mode( unsigned char mode );
92 92 unsigned int check_update_info_hk_tds_mode( unsigned char mode );
93 93 unsigned int check_update_info_hk_thr_mode( unsigned char mode );
94 94 void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value );
95 95 void set_hk_lfr_sc_rw_f_flags( void );
96 int check_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value );
97 int check_all_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int *pos, float*value );
96 98 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC );
97 99 void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, float sy_lfr_rw_k );
98 100 void build_sy_lfr_rw_mask( unsigned int channel );
99 101 void build_sy_lfr_rw_masks();
100 102 void merge_fbins_masks( void );
101 103
102 104 // FBINS_MASK
103 105 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC );
104 106
105 107 // TC_LFR_LOAD_PARS_FILTER_PAR
106 108 int check_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value );
107 109 int check_all_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int *pos, float*value );
108 110 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
109 111
110 112 // KCOEFFICIENTS
111 113 int set_sy_lfr_kcoeff(ccsdsTelecommandPacket_t *TC , rtems_id queue_id);
112 114 void copyFloatByChar( unsigned char *destination, unsigned char *source );
113 115 void copyInt32ByChar( unsigned char *destination, unsigned char *source );
114 116 void copyInt16ByChar( unsigned char *destination, unsigned char *source );
115 117 void floatToChar( float value, unsigned char* ptr);
116 118
117 119 void init_parameter_dump( void );
118 120 void init_kcoefficients_dump( void );
119 121 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr );
120 122 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id );
121 123
122 124 #endif // TC_LOAD_DUMP_PARAMETERS_H
@@ -1,98 +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 #define NB_OF_TASKS 20
26 26 #define NB_OF_MISC_NAMES 5
27 27
28 28 // RTEMS GLOBAL VARIABLES
29 29 rtems_name misc_name[NB_OF_MISC_NAMES] = {0};
30 30 rtems_name Task_name[NB_OF_TASKS] = {0}; /* array of task names */
31 31 rtems_id Task_id[NB_OF_TASKS] = {0}; /* array of task ids */
32 32 rtems_name timecode_timer_name = 0;
33 33 rtems_id timecode_timer_id = RTEMS_ID_NONE;
34 34 rtems_name name_hk_rate_monotonic = 0; // name of the HK rate monotonic
35 35 rtems_id HK_id = RTEMS_ID_NONE;// id of the HK rate monotonic period
36 36 rtems_name name_avgv_rate_monotonic = 0; // name of the AVGV rate monotonic
37 37 rtems_id AVGV_id = RTEMS_ID_NONE;// id of the AVGV rate monotonic period
38 38 int fdSPW = 0;
39 39 int fdUART = 0;
40 40 unsigned char lfrCurrentMode = 0;
41 41 unsigned char pa_bia_status_info = 0;
42 42 unsigned char thisIsAnASMRestart = 0;
43 43 unsigned char oneTcLfrUpdateTimeReceived = 0;
44 44
45 45 // WAVEFORMS GLOBAL VARIABLES // 2048 * 3 * 4 + 2 * 4 = 24576 + 8 bytes = 24584
46 46 // 97 * 256 = 24832 => delta = 248 bytes = 62 words
47 47 // WAVEFORMS GLOBAL VARIABLES // 2688 * 3 * 4 + 2 * 4 = 32256 + 8 bytes = 32264
48 48 // 127 * 256 = 32512 => delta = 248 bytes = 62 words
49 49 // F0 F1 F2 F3
50 50 volatile int wf_buffer_f0[ NB_RING_NODES_F0 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
51 51 volatile int wf_buffer_f1[ NB_RING_NODES_F1 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
52 52 volatile int wf_buffer_f2[ NB_RING_NODES_F2 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
53 53 volatile int wf_buffer_f3[ NB_RING_NODES_F3 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
54 54
55 55 //***********************************
56 56 // SPECTRAL MATRICES GLOBAL VARIABLES
57 57
58 58 // alignment constraints for the spectral matrices buffers => the first data after the time (8 bytes) shall be aligned on 0x00
59 59 volatile int sm_f0[ NB_RING_NODES_SM_F0 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0};
60 60 volatile int sm_f1[ NB_RING_NODES_SM_F1 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0};
61 61 volatile int sm_f2[ NB_RING_NODES_SM_F2 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0};
62 62
63 63 // APB CONFIGURATION REGISTERS
64 64 time_management_regs_t *time_management_regs = (time_management_regs_t*) REGS_ADDR_TIME_MANAGEMENT;
65 65 gptimer_regs_t *gptimer_regs = (gptimer_regs_t *) REGS_ADDR_GPTIMER;
66 66 waveform_picker_regs_0_1_18_t *waveform_picker_regs = (waveform_picker_regs_0_1_18_t*) REGS_ADDR_WAVEFORM_PICKER;
67 67 spectral_matrix_regs_t *spectral_matrix_regs = (spectral_matrix_regs_t*) REGS_ADDR_SPECTRAL_MATRIX;
68 68
69 69 // MODE PARAMETERS
70 70 Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet = {0};
71 71 struct param_local_str param_local = {0};
72 72 unsigned int lastValidEnterModeTime = {0};
73 73
74 74 // HK PACKETS
75 75 Packet_TM_LFR_HK_t housekeeping_packet = {0};
76 76 // message queues occupancy
77 77 unsigned char hk_lfr_q_sd_fifo_size_max = 0;
78 78 unsigned char hk_lfr_q_rv_fifo_size_max = 0;
79 79 unsigned char hk_lfr_q_p0_fifo_size_max = 0;
80 80 unsigned char hk_lfr_q_p1_fifo_size_max = 0;
81 81 unsigned char hk_lfr_q_p2_fifo_size_max = 0;
82 82 // sequence counters are incremented by APID (PID + CAT) and destination ID
83 unsigned short sequenceCounters_SCIENCE_NORMAL_BURST = 0;
84 unsigned short sequenceCounters_SCIENCE_SBM1_SBM2 = 0;
85 unsigned short sequenceCounters_TC_EXE[SEQ_CNT_NB_DEST_ID] = {0};
86 unsigned short sequenceCounters_TM_DUMP[SEQ_CNT_NB_DEST_ID] = {0};
87 unsigned short sequenceCounterHK = {0};
88 spw_stats grspw_stats = {0};
83 unsigned short sequenceCounters_SCIENCE_NORMAL_BURST __attribute__((aligned(0x4))) = 0;
84 unsigned short sequenceCounters_SCIENCE_SBM1_SBM2 __attribute__((aligned(0x4))) = 0;
85 unsigned short sequenceCounters_TC_EXE[SEQ_CNT_NB_DEST_ID] __attribute__((aligned(0x4))) = {0};
86 unsigned short sequenceCounters_TM_DUMP[SEQ_CNT_NB_DEST_ID] __attribute__((aligned(0x4))) = {0};
87 unsigned short sequenceCounterHK __attribute__((aligned(0x4))) = {0};
88 spw_stats grspw_stats __attribute__((aligned(0x4))) = {0};
89 89
90 90 // TC_LFR_UPDATE_INFO
91 91 rw_f_t rw_f;
92 92
93 93 // TC_LFR_LOAD_FILTER_PAR
94 94 filterPar_t filterPar = {0};
95 95
96 96 fbins_masks_t fbins_masks = {0};
97 97 unsigned int acquisitionDurations[NB_ACQUISITION_DURATION]
98 98 = {ACQUISITION_DURATION_F0, ACQUISITION_DURATION_F1, ACQUISITION_DURATION_F2};
@@ -1,972 +1,972
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 21 // number of tasks concurrently active including INIT
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 #define CONFIGURE_MAXIMUM_PERIODS 5 // [hous] [load] [avgv]
38 #define CONFIGURE_MAXIMUM_TIMERS 5 // [spiq] [link] [spacewire_reset_link]
37 #define CONFIGURE_MAXIMUM_PERIODS 6 // [hous] [load] [avgv]
38 #define CONFIGURE_MAXIMUM_TIMERS 6 // [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
51 51 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
52 52 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
53 53 #endif
54 54
55 55 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
56 56 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
57 57 #endif
58 58
59 59 #endif
60 60 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
61 61
62 62 #include <drvmgr/drvmgr_confdefs.h>
63 63 #endif
64 64
65 65 #include "fsw_init.h"
66 66 #include "fsw_config.c"
67 67 #include "GscMemoryLPP.hpp"
68 68
69 69 void initCache()
70 70 {
71 71 // ASI 2 contains a few control registers that have not been assigned as ancillary state registers.
72 72 // These should only be read and written using 32-bit LDA/STA instructions.
73 73 // All cache registers are accessed through load/store operations to the alternate address space (LDA/STA), using ASI = 2.
74 74 // The table below shows the register addresses:
75 75 // 0x00 Cache control register
76 76 // 0x04 Reserved
77 77 // 0x08 Instruction cache configuration register
78 78 // 0x0C Data cache configuration register
79 79
80 80 // Cache Control Register Leon3 / Leon3FT
81 81 // 31..30 29 28 27..24 23 22 21 20..19 18 17 16
82 82 // RFT PS TB DS FD FI FT ST IB
83 83 // 15 14 13..12 11..10 9..8 7..6 5 4 3..2 1..0
84 84 // IP DP ITE IDE DTE DDE DF IF DCS ICS
85 85
86 86 unsigned int cacheControlRegister;
87 87
88 88 CCR_resetCacheControlRegister();
89 89 ASR16_resetRegisterProtectionControlRegister();
90 90
91 91 cacheControlRegister = CCR_getValue();
92 92 PRINTF1("(0) CCR - Cache Control Register = %x\n", cacheControlRegister);
93 93 PRINTF1("(0) ASR16 = %x\n", *asr16Ptr);
94 94
95 95 CCR_enableInstructionCache(); // ICS bits
96 96 CCR_enableDataCache(); // DCS bits
97 97 CCR_enableInstructionBurstFetch(); // IB bit
98 98
99 99 faultTolerantScheme();
100 100
101 101 cacheControlRegister = CCR_getValue();
102 102 PRINTF1("(1) CCR - Cache Control Register = %x\n", cacheControlRegister);
103 103 PRINTF1("(1) ASR16 Register protection control register = %x\n", *asr16Ptr);
104 104
105 105 PRINTF("\n");
106 106 }
107 107
108 108 rtems_task Init( rtems_task_argument ignored )
109 109 {
110 110 /** This is the RTEMS INIT taks, it is the first task launched by the system.
111 111 *
112 112 * @param unused is the starting argument of the RTEMS task
113 113 *
114 114 * The INIT task create and run all other RTEMS tasks.
115 115 *
116 116 */
117 117
118 118 //***********
119 119 // INIT CACHE
120 120
121 121 unsigned char *vhdlVersion;
122 122
123 123 reset_lfr();
124 124
125 125 reset_local_time();
126 126
127 127 rtems_cpu_usage_reset();
128 128
129 129 rtems_status_code status;
130 130 rtems_status_code status_spw;
131 131 rtems_isr_entry old_isr_handler;
132 132
133 133 old_isr_handler = NULL;
134 134
135 135 // UART settings
136 136 enable_apbuart_transmitter();
137 137 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
138 138
139 139 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
140 140
141 141
142 142 PRINTF("\n\n\n\n\n")
143 143
144 144 initCache();
145 145
146 146 PRINTF("*************************\n")
147 147 PRINTF("** LFR Flight Software **\n")
148 148
149 149 PRINTF1("** %d-", SW_VERSION_N1)
150 150 PRINTF1("%d-" , SW_VERSION_N2)
151 151 PRINTF1("%d-" , SW_VERSION_N3)
152 152 PRINTF1("%d **\n", SW_VERSION_N4)
153 153
154 154 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
155 155 PRINTF("** VHDL **\n")
156 156 PRINTF1("** %d-", vhdlVersion[1])
157 157 PRINTF1("%d-" , vhdlVersion[2])
158 158 PRINTF1("%d **\n", vhdlVersion[3])
159 159 PRINTF("*************************\n")
160 160 PRINTF("\n\n")
161 161
162 162 init_parameter_dump();
163 163 init_kcoefficients_dump();
164 164 init_local_mode_parameters();
165 165 init_housekeeping_parameters();
166 166 init_k_coefficients_prc0();
167 167 init_k_coefficients_prc1();
168 168 init_k_coefficients_prc2();
169 169 pa_bia_status_info = INIT_CHAR;
170 170
171 171 // initialize all reaction wheels frequencies to NaN
172 172 rw_f.cp_rpw_sc_rw1_f1 = NAN;
173 173 rw_f.cp_rpw_sc_rw1_f2 = NAN;
174 174 rw_f.cp_rpw_sc_rw1_f3 = NAN;
175 175 rw_f.cp_rpw_sc_rw1_f4 = NAN;
176 176 rw_f.cp_rpw_sc_rw2_f1 = NAN;
177 177 rw_f.cp_rpw_sc_rw2_f2 = NAN;
178 178 rw_f.cp_rpw_sc_rw2_f3 = NAN;
179 179 rw_f.cp_rpw_sc_rw2_f4 = NAN;
180 180 rw_f.cp_rpw_sc_rw3_f1 = NAN;
181 181 rw_f.cp_rpw_sc_rw3_f2 = NAN;
182 182 rw_f.cp_rpw_sc_rw3_f3 = NAN;
183 183 rw_f.cp_rpw_sc_rw3_f4 = NAN;
184 184 rw_f.cp_rpw_sc_rw4_f1 = NAN;
185 185 rw_f.cp_rpw_sc_rw4_f2 = NAN;
186 186 rw_f.cp_rpw_sc_rw4_f3 = NAN;
187 187 rw_f.cp_rpw_sc_rw4_f4 = NAN;
188 188
189 189 // initialize filtering parameters
190 190 filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED;
191 191 filterPar.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
192 192 filterPar.sy_lfr_pas_filter_tbad = DEFAULT_SY_LFR_PAS_FILTER_TBAD;
193 193 filterPar.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
194 194 filterPar.sy_lfr_pas_filter_shift = DEFAULT_SY_LFR_PAS_FILTER_SHIFT;
195 195 filterPar.sy_lfr_sc_rw_delta_f = DEFAULT_SY_LFR_SC_RW_DELTA_F;
196 196 update_last_valid_transition_date( DEFAULT_LAST_VALID_TRANSITION_DATE );
197 197
198 198 // waveform picker initialization
199 199 WFP_init_rings();
200 200 LEON_Clear_interrupt( IRQ_SPARC_GPTIMER_WATCHDOG ); // initialize the waveform rings
201 201 WFP_reset_current_ring_nodes();
202 202 reset_waveform_picker_regs();
203 203
204 204 // spectral matrices initialization
205 205 SM_init_rings(); // initialize spectral matrices rings
206 206 SM_reset_current_ring_nodes();
207 207 reset_spectral_matrix_regs();
208 208
209 209 // configure calibration
210 210 configureCalibration( false ); // true means interleaved mode, false is for normal mode
211 211
212 212 updateLFRCurrentMode( LFR_MODE_STANDBY );
213 213
214 214 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
215 215
216 216 create_names(); // create all names
217 217
218 218 status = create_timecode_timer(); // create the timer used by timecode_irq_handler
219 219 if (status != RTEMS_SUCCESSFUL)
220 220 {
221 221 PRINTF1("in INIT *** ERR in create_timer_timecode, code %d", status)
222 222 }
223 223
224 224 status = create_message_queues(); // create message queues
225 225 if (status != RTEMS_SUCCESSFUL)
226 226 {
227 227 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
228 228 }
229 229
230 230 status = create_all_tasks(); // create all tasks
231 231 if (status != RTEMS_SUCCESSFUL)
232 232 {
233 233 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
234 234 }
235 235
236 236 // **************************
237 237 // <SPACEWIRE INITIALIZATION>
238 238 status_spw = spacewire_open_link(); // (1) open the link
239 239 if ( status_spw != RTEMS_SUCCESSFUL )
240 240 {
241 241 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
242 242 }
243 243
244 244 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
245 245 {
246 246 status_spw = spacewire_configure_link( fdSPW );
247 247 if ( status_spw != RTEMS_SUCCESSFUL )
248 248 {
249 249 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
250 250 }
251 251 }
252 252
253 253 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
254 254 {
255 255 status_spw = spacewire_start_link( fdSPW );
256 256 if ( status_spw != RTEMS_SUCCESSFUL )
257 257 {
258 258 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
259 259 }
260 260 }
261 261 // </SPACEWIRE INITIALIZATION>
262 262 // ***************************
263 263
264 264 status = start_all_tasks(); // start all tasks
265 265 if (status != RTEMS_SUCCESSFUL)
266 266 {
267 267 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
268 268 }
269 269
270 270 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
271 271 status = start_recv_send_tasks();
272 272 if ( status != RTEMS_SUCCESSFUL )
273 273 {
274 274 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
275 275 }
276 276
277 277 // suspend science tasks, they will be restarted later depending on the mode
278 278 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
279 279 if (status != RTEMS_SUCCESSFUL)
280 280 {
281 281 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
282 282 }
283 283
284 284 // configure IRQ handling for the waveform picker unit
285 285 status = rtems_interrupt_catch( waveforms_isr,
286 286 IRQ_SPARC_WAVEFORM_PICKER,
287 287 &old_isr_handler) ;
288 288 // configure IRQ handling for the spectral matrices unit
289 289 status = rtems_interrupt_catch( spectral_matrices_isr,
290 290 IRQ_SPARC_SPECTRAL_MATRIX,
291 291 &old_isr_handler) ;
292 292
293 293 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
294 294 if ( status_spw != RTEMS_SUCCESSFUL )
295 295 {
296 296 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
297 297 if ( status != RTEMS_SUCCESSFUL ) {
298 298 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
299 299 }
300 300 }
301 301
302 302 BOOT_PRINTF("delete INIT\n")
303 303
304 304 set_hk_lfr_sc_potential_flag( true );
305 305
306 306 // start the timer to detect a missing spacewire timecode
307 307 // the timeout is larger because the spw IP needs to receive several valid timecodes before generating a tickout
308 308 // if a tickout is generated, the timer is restarted
309 309 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT_INIT, timecode_timer_routine, NULL );
310 310
311 311 grspw_timecode_callback = &timecode_irq_handler;
312 312
313 313 status = rtems_task_delete(RTEMS_SELF);
314 314
315 315 }
316 316
317 317 void init_local_mode_parameters( void )
318 318 {
319 319 /** This function initialize the param_local global variable with default values.
320 320 *
321 321 */
322 322
323 323 unsigned int i;
324 324
325 325 // LOCAL PARAMETERS
326 326
327 327 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
328 328 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
329 329
330 330 // init sequence counters
331 331
332 332 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
333 333 {
334 334 sequenceCounters_TC_EXE[i] = INIT_CHAR;
335 335 sequenceCounters_TM_DUMP[i] = INIT_CHAR;
336 336 }
337 337 sequenceCounters_SCIENCE_NORMAL_BURST = INIT_CHAR;
338 338 sequenceCounters_SCIENCE_SBM1_SBM2 = INIT_CHAR;
339 339 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << TM_PACKET_SEQ_SHIFT;
340 340 }
341 341
342 342 void reset_local_time( void )
343 343 {
344 344 time_management_regs->ctrl = time_management_regs->ctrl | VAL_SOFTWARE_RESET; // [0010] software reset, coarse time = 0x80000000
345 345 }
346 346
347 347 void create_names( void ) // create all names for tasks and queues
348 348 {
349 349 /** This function creates all RTEMS names used in the software for tasks and queues.
350 350 *
351 351 * @return RTEMS directive status codes:
352 352 * - RTEMS_SUCCESSFUL - successful completion
353 353 *
354 354 */
355 355
356 356 // task names
357 357 Task_name[TASKID_AVGV] = rtems_build_name( 'A', 'V', 'G', 'V' );
358 358 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
359 359 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
360 360 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
361 361 Task_name[TASKID_LOAD] = rtems_build_name( 'L', 'O', 'A', 'D' );
362 362 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
363 363 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
364 364 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
365 365 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
366 366 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
367 367 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
368 368 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
369 369 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
370 370 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
371 371 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
372 372 Task_name[TASKID_LINK] = rtems_build_name( 'L', 'I', 'N', 'K' );
373 373 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
374 374 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
375 375 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
376 376 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
377 377
378 378 // rate monotonic period names
379 379 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
380 380 name_avgv_rate_monotonic = rtems_build_name( 'A', 'V', 'G', 'V' );
381 381
382 382 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
383 383 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
384 384 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
385 385 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
386 386 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
387 387
388 388 timecode_timer_name = rtems_build_name( 'S', 'P', 'T', 'C' );
389 389 }
390 390
391 391 int create_all_tasks( void ) // create all tasks which run in the software
392 392 {
393 393 /** This function creates all RTEMS tasks used in the software.
394 394 *
395 395 * @return RTEMS directive status codes:
396 396 * - RTEMS_SUCCESSFUL - task created successfully
397 397 * - RTEMS_INVALID_ADDRESS - id is NULL
398 398 * - RTEMS_INVALID_NAME - invalid task name
399 399 * - RTEMS_INVALID_PRIORITY - invalid task priority
400 400 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
401 401 * - RTEMS_TOO_MANY - too many tasks created
402 402 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
403 403 * - RTEMS_TOO_MANY - too many global objects
404 404 *
405 405 */
406 406
407 407 rtems_status_code status;
408 408
409 409 //**********
410 410 // SPACEWIRE
411 411 // RECV
412 412 status = rtems_task_create(
413 413 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
414 414 RTEMS_DEFAULT_MODES,
415 415 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
416 416 );
417 417 if (status == RTEMS_SUCCESSFUL) // SEND
418 418 {
419 419 status = rtems_task_create(
420 420 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
421 421 RTEMS_DEFAULT_MODES,
422 422 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
423 423 );
424 424 }
425 425 if (status == RTEMS_SUCCESSFUL) // LINK
426 426 {
427 427 status = rtems_task_create(
428 428 Task_name[TASKID_LINK], TASK_PRIORITY_LINK, RTEMS_MINIMUM_STACK_SIZE,
429 429 RTEMS_DEFAULT_MODES,
430 430 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LINK]
431 431 );
432 432 }
433 433 if (status == RTEMS_SUCCESSFUL) // ACTN
434 434 {
435 435 status = rtems_task_create(
436 436 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
437 437 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
438 438 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
439 439 );
440 440 }
441 441 if (status == RTEMS_SUCCESSFUL) // SPIQ
442 442 {
443 443 status = rtems_task_create(
444 444 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
445 445 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
446 446 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
447 447 );
448 448 }
449 449
450 450 //******************
451 451 // SPECTRAL MATRICES
452 452 if (status == RTEMS_SUCCESSFUL) // AVF0
453 453 {
454 454 status = rtems_task_create(
455 455 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
456 456 RTEMS_DEFAULT_MODES,
457 457 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
458 458 );
459 459 }
460 460 if (status == RTEMS_SUCCESSFUL) // PRC0
461 461 {
462 462 status = rtems_task_create(
463 463 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
464 464 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
465 465 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
466 466 );
467 467 }
468 468 if (status == RTEMS_SUCCESSFUL) // AVF1
469 469 {
470 470 status = rtems_task_create(
471 471 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
472 472 RTEMS_DEFAULT_MODES,
473 473 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
474 474 );
475 475 }
476 476 if (status == RTEMS_SUCCESSFUL) // PRC1
477 477 {
478 478 status = rtems_task_create(
479 479 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
480 480 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
481 481 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
482 482 );
483 483 }
484 484 if (status == RTEMS_SUCCESSFUL) // AVF2
485 485 {
486 486 status = rtems_task_create(
487 487 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
488 488 RTEMS_DEFAULT_MODES,
489 489 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
490 490 );
491 491 }
492 492 if (status == RTEMS_SUCCESSFUL) // PRC2
493 493 {
494 494 status = rtems_task_create(
495 495 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
496 496 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
497 497 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
498 498 );
499 499 }
500 500
501 501 //****************
502 502 // WAVEFORM PICKER
503 503 if (status == RTEMS_SUCCESSFUL) // WFRM
504 504 {
505 505 status = rtems_task_create(
506 506 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
507 507 RTEMS_DEFAULT_MODES,
508 508 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
509 509 );
510 510 }
511 511 if (status == RTEMS_SUCCESSFUL) // CWF3
512 512 {
513 513 status = rtems_task_create(
514 514 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
515 515 RTEMS_DEFAULT_MODES,
516 516 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
517 517 );
518 518 }
519 519 if (status == RTEMS_SUCCESSFUL) // CWF2
520 520 {
521 521 status = rtems_task_create(
522 522 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
523 523 RTEMS_DEFAULT_MODES,
524 524 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
525 525 );
526 526 }
527 527 if (status == RTEMS_SUCCESSFUL) // CWF1
528 528 {
529 529 status = rtems_task_create(
530 530 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
531 531 RTEMS_DEFAULT_MODES,
532 532 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
533 533 );
534 534 }
535 535 if (status == RTEMS_SUCCESSFUL) // SWBD
536 536 {
537 537 status = rtems_task_create(
538 538 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
539 539 RTEMS_DEFAULT_MODES,
540 540 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
541 541 );
542 542 }
543 543
544 544 //*****
545 545 // MISC
546 546 if (status == RTEMS_SUCCESSFUL) // LOAD
547 547 {
548 548 status = rtems_task_create(
549 549 Task_name[TASKID_LOAD], TASK_PRIORITY_LOAD, RTEMS_MINIMUM_STACK_SIZE,
550 550 RTEMS_DEFAULT_MODES,
551 551 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LOAD]
552 552 );
553 553 }
554 554 if (status == RTEMS_SUCCESSFUL) // DUMB
555 555 {
556 556 status = rtems_task_create(
557 557 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
558 558 RTEMS_DEFAULT_MODES,
559 559 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
560 560 );
561 561 }
562 562 if (status == RTEMS_SUCCESSFUL) // HOUS
563 563 {
564 564 status = rtems_task_create(
565 565 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
566 566 RTEMS_DEFAULT_MODES,
567 567 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
568 568 );
569 569 }
570 570 if (status == RTEMS_SUCCESSFUL) // AVGV
571 571 {
572 572 status = rtems_task_create(
573 573 Task_name[TASKID_AVGV], TASK_PRIORITY_AVGV, RTEMS_MINIMUM_STACK_SIZE,
574 574 RTEMS_DEFAULT_MODES,
575 575 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVGV]
576 576 );
577 577 }
578 578
579 579 return status;
580 580 }
581 581
582 582 int start_recv_send_tasks( void )
583 583 {
584 584 rtems_status_code status;
585 585
586 586 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
587 587 if (status!=RTEMS_SUCCESSFUL) {
588 588 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
589 589 }
590 590
591 591 if (status == RTEMS_SUCCESSFUL) // SEND
592 592 {
593 593 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
594 594 if (status!=RTEMS_SUCCESSFUL) {
595 595 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
596 596 }
597 597 }
598 598
599 599 return status;
600 600 }
601 601
602 602 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
603 603 {
604 604 /** This function starts all RTEMS tasks used in the software.
605 605 *
606 606 * @return RTEMS directive status codes:
607 607 * - RTEMS_SUCCESSFUL - ask started successfully
608 608 * - RTEMS_INVALID_ADDRESS - invalid task entry point
609 609 * - RTEMS_INVALID_ID - invalid task id
610 610 * - RTEMS_INCORRECT_STATE - task not in the dormant state
611 611 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
612 612 *
613 613 */
614 614 // starts all the tasks fot eh flight software
615 615
616 616 rtems_status_code status;
617 617
618 618 //**********
619 619 // SPACEWIRE
620 620 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
621 621 if (status!=RTEMS_SUCCESSFUL) {
622 622 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
623 623 }
624 624
625 625 if (status == RTEMS_SUCCESSFUL) // LINK
626 626 {
627 627 status = rtems_task_start( Task_id[TASKID_LINK], link_task, 1 );
628 628 if (status!=RTEMS_SUCCESSFUL) {
629 629 BOOT_PRINTF("in INIT *** Error starting TASK_LINK\n")
630 630 }
631 631 }
632 632
633 633 if (status == RTEMS_SUCCESSFUL) // ACTN
634 634 {
635 635 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
636 636 if (status!=RTEMS_SUCCESSFUL) {
637 637 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
638 638 }
639 639 }
640 640
641 641 //******************
642 642 // SPECTRAL MATRICES
643 643 if (status == RTEMS_SUCCESSFUL) // AVF0
644 644 {
645 645 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
646 646 if (status!=RTEMS_SUCCESSFUL) {
647 647 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
648 648 }
649 649 }
650 650 if (status == RTEMS_SUCCESSFUL) // PRC0
651 651 {
652 652 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
653 653 if (status!=RTEMS_SUCCESSFUL) {
654 654 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
655 655 }
656 656 }
657 657 if (status == RTEMS_SUCCESSFUL) // AVF1
658 658 {
659 659 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
660 660 if (status!=RTEMS_SUCCESSFUL) {
661 661 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
662 662 }
663 663 }
664 664 if (status == RTEMS_SUCCESSFUL) // PRC1
665 665 {
666 666 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
667 667 if (status!=RTEMS_SUCCESSFUL) {
668 668 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
669 669 }
670 670 }
671 671 if (status == RTEMS_SUCCESSFUL) // AVF2
672 672 {
673 673 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
674 674 if (status!=RTEMS_SUCCESSFUL) {
675 675 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
676 676 }
677 677 }
678 678 if (status == RTEMS_SUCCESSFUL) // PRC2
679 679 {
680 680 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
681 681 if (status!=RTEMS_SUCCESSFUL) {
682 682 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
683 683 }
684 684 }
685 685
686 686 //****************
687 687 // WAVEFORM PICKER
688 688 if (status == RTEMS_SUCCESSFUL) // WFRM
689 689 {
690 690 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
691 691 if (status!=RTEMS_SUCCESSFUL) {
692 692 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
693 693 }
694 694 }
695 695 if (status == RTEMS_SUCCESSFUL) // CWF3
696 696 {
697 697 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
698 698 if (status!=RTEMS_SUCCESSFUL) {
699 699 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
700 700 }
701 701 }
702 702 if (status == RTEMS_SUCCESSFUL) // CWF2
703 703 {
704 704 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
705 705 if (status!=RTEMS_SUCCESSFUL) {
706 706 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
707 707 }
708 708 }
709 709 if (status == RTEMS_SUCCESSFUL) // CWF1
710 710 {
711 711 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
712 712 if (status!=RTEMS_SUCCESSFUL) {
713 713 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
714 714 }
715 715 }
716 716 if (status == RTEMS_SUCCESSFUL) // SWBD
717 717 {
718 718 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
719 719 if (status!=RTEMS_SUCCESSFUL) {
720 720 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
721 721 }
722 722 }
723 723
724 724 //*****
725 725 // MISC
726 726 if (status == RTEMS_SUCCESSFUL) // HOUS
727 727 {
728 728 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
729 729 if (status!=RTEMS_SUCCESSFUL) {
730 730 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
731 731 }
732 732 }
733 733 if (status == RTEMS_SUCCESSFUL) // AVGV
734 734 {
735 735 status = rtems_task_start( Task_id[TASKID_AVGV], avgv_task, 1 );
736 736 if (status!=RTEMS_SUCCESSFUL) {
737 737 BOOT_PRINTF("in INIT *** Error starting TASK_AVGV\n")
738 738 }
739 739 }
740 740 if (status == RTEMS_SUCCESSFUL) // DUMB
741 741 {
742 742 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
743 743 if (status!=RTEMS_SUCCESSFUL) {
744 744 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
745 745 }
746 746 }
747 747 if (status == RTEMS_SUCCESSFUL) // LOAD
748 748 {
749 749 status = rtems_task_start( Task_id[TASKID_LOAD], load_task, 1 );
750 750 if (status!=RTEMS_SUCCESSFUL) {
751 751 BOOT_PRINTF("in INIT *** Error starting TASK_LOAD\n")
752 752 }
753 753 }
754 754
755 755 return status;
756 756 }
757 757
758 758 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
759 759 {
760 760 rtems_status_code status_recv;
761 761 rtems_status_code status_send;
762 762 rtems_status_code status_q_p0;
763 763 rtems_status_code status_q_p1;
764 764 rtems_status_code status_q_p2;
765 765 rtems_status_code ret;
766 766 rtems_id queue_id;
767 767
768 768 ret = RTEMS_SUCCESSFUL;
769 769 queue_id = RTEMS_ID_NONE;
770 770
771 771 //****************************************
772 772 // create the queue for handling valid TCs
773 773 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
774 774 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
775 775 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
776 776 if ( status_recv != RTEMS_SUCCESSFUL ) {
777 777 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
778 778 }
779 779
780 780 //************************************************
781 781 // create the queue for handling TM packet sending
782 782 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
783 783 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
784 784 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
785 785 if ( status_send != RTEMS_SUCCESSFUL ) {
786 786 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
787 787 }
788 788
789 789 //*****************************************************************************
790 790 // create the queue for handling averaged spectral matrices for processing @ f0
791 791 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
792 792 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
793 793 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
794 794 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
795 795 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
796 796 }
797 797
798 798 //*****************************************************************************
799 799 // create the queue for handling averaged spectral matrices for processing @ f1
800 800 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
801 801 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
802 802 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
803 803 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
804 804 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
805 805 }
806 806
807 807 //*****************************************************************************
808 808 // create the queue for handling averaged spectral matrices for processing @ f2
809 809 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
810 810 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
811 811 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
812 812 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
813 813 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
814 814 }
815 815
816 816 if ( status_recv != RTEMS_SUCCESSFUL )
817 817 {
818 818 ret = status_recv;
819 819 }
820 820 else if( status_send != RTEMS_SUCCESSFUL )
821 821 {
822 822 ret = status_send;
823 823 }
824 824 else if( status_q_p0 != RTEMS_SUCCESSFUL )
825 825 {
826 826 ret = status_q_p0;
827 827 }
828 828 else if( status_q_p1 != RTEMS_SUCCESSFUL )
829 829 {
830 830 ret = status_q_p1;
831 831 }
832 832 else
833 833 {
834 834 ret = status_q_p2;
835 835 }
836 836
837 837 return ret;
838 838 }
839 839
840 840 rtems_status_code create_timecode_timer( void )
841 841 {
842 842 rtems_status_code status;
843 843
844 844 status = rtems_timer_create( timecode_timer_name, &timecode_timer_id );
845 845
846 846 if ( status != RTEMS_SUCCESSFUL )
847 847 {
848 848 PRINTF1("in create_timer_timecode *** ERR creating SPTC timer, %d\n", status)
849 849 }
850 850 else
851 851 {
852 852 PRINTF("in create_timer_timecode *** OK creating SPTC timer\n")
853 853 }
854 854
855 855 return status;
856 856 }
857 857
858 858 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
859 859 {
860 860 rtems_status_code status;
861 861 rtems_name queue_name;
862 862
863 863 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
864 864
865 865 status = rtems_message_queue_ident( queue_name, 0, queue_id );
866 866
867 867 return status;
868 868 }
869 869
870 870 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
871 871 {
872 872 rtems_status_code status;
873 873 rtems_name queue_name;
874 874
875 875 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
876 876
877 877 status = rtems_message_queue_ident( queue_name, 0, queue_id );
878 878
879 879 return status;
880 880 }
881 881
882 882 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
883 883 {
884 884 rtems_status_code status;
885 885 rtems_name queue_name;
886 886
887 887 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
888 888
889 889 status = rtems_message_queue_ident( queue_name, 0, queue_id );
890 890
891 891 return status;
892 892 }
893 893
894 894 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
895 895 {
896 896 rtems_status_code status;
897 897 rtems_name queue_name;
898 898
899 899 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
900 900
901 901 status = rtems_message_queue_ident( queue_name, 0, queue_id );
902 902
903 903 return status;
904 904 }
905 905
906 906 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
907 907 {
908 908 rtems_status_code status;
909 909 rtems_name queue_name;
910 910
911 911 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
912 912
913 913 status = rtems_message_queue_ident( queue_name, 0, queue_id );
914 914
915 915 return status;
916 916 }
917 917
918 918 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max )
919 919 {
920 920 u_int32_t count;
921 921 rtems_status_code status;
922 922
923 923 count = 0;
924 924
925 925 status = rtems_message_queue_get_number_pending( queue_id, &count );
926 926
927 927 count = count + 1;
928 928
929 929 if (status != RTEMS_SUCCESSFUL)
930 930 {
931 931 PRINTF1("in update_queue_max_count *** ERR = %d\n", status)
932 932 }
933 933 else
934 934 {
935 935 if (count > *fifo_size_max)
936 936 {
937 937 *fifo_size_max = count;
938 938 }
939 939 }
940 940 }
941 941
942 942 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
943 943 {
944 944 unsigned char i;
945 945
946 946 //***************
947 947 // BUFFER ADDRESS
948 948 for(i=0; i<nbNodes; i++)
949 949 {
950 950 ring[i].coarseTime = INT32_ALL_F;
951 951 ring[i].fineTime = INT32_ALL_F;
952 952 ring[i].sid = INIT_CHAR;
953 953 ring[i].status = INIT_CHAR;
954 954 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
955 955 }
956 956
957 957 //*****
958 958 // NEXT
959 959 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
960 960 for(i=0; i<nbNodes-1; i++)
961 961 {
962 962 ring[i].next = (ring_node*) &ring[ i + 1 ];
963 963 }
964 964
965 965 //*********
966 966 // PREVIOUS
967 967 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
968 968 for(i=1; i<nbNodes; i++)
969 969 {
970 970 ring[i].previous = (ring_node*) &ring[ i - 1 ];
971 971 }
972 972 }
@@ -1,1001 +1,1019
1 1 /** General usage functions and RTEMS tasks.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 */
7 7
8 8 #include "fsw_misc.h"
9 9
10 10 int16_t hk_lfr_sc_v_f3_as_int16 = 0;
11 11 int16_t hk_lfr_sc_e1_f3_as_int16 = 0;
12 12 int16_t hk_lfr_sc_e2_f3_as_int16 = 0;
13 13
14 14 void timer_configure(unsigned char timer, unsigned int clock_divider,
15 15 unsigned char interrupt_level, rtems_isr (*timer_isr)() )
16 16 {
17 17 /** This function configures a GPTIMER timer instantiated in the VHDL design.
18 18 *
19 19 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
20 20 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
21 21 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
22 22 * @param interrupt_level is the interrupt level that the timer drives.
23 23 * @param timer_isr is the interrupt subroutine that will be attached to the IRQ driven by the timer.
24 24 *
25 25 * Interrupt levels are described in the SPARC documentation sparcv8.pdf p.76
26 26 *
27 27 */
28 28
29 29 rtems_status_code status;
30 30 rtems_isr_entry old_isr_handler;
31 31
32 32 old_isr_handler = NULL;
33 33
34 34 gptimer_regs->timer[timer].ctrl = INIT_CHAR; // reset the control register
35 35
36 36 status = rtems_interrupt_catch( timer_isr, interrupt_level, &old_isr_handler) ; // see sparcv8.pdf p.76 for interrupt levels
37 37 if (status!=RTEMS_SUCCESSFUL)
38 38 {
39 39 PRINTF("in configure_timer *** ERR rtems_interrupt_catch\n")
40 40 }
41 41
42 42 timer_set_clock_divider( timer, clock_divider);
43 43 }
44 44
45 45 void timer_start(unsigned char timer)
46 46 {
47 47 /** This function starts a GPTIMER timer.
48 48 *
49 49 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
50 50 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
51 51 *
52 52 */
53 53
54 54 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_CLEAR_IRQ;
55 55 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_LD;
56 56 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_EN;
57 57 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_RS;
58 58 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_IE;
59 59 }
60 60
61 61 void timer_stop(unsigned char timer)
62 62 {
63 63 /** This function stops a GPTIMER timer.
64 64 *
65 65 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
66 66 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
67 67 *
68 68 */
69 69
70 70 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & GPTIMER_EN_MASK;
71 71 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & GPTIMER_IE_MASK;
72 72 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_CLEAR_IRQ;
73 73 }
74 74
75 75 void timer_set_clock_divider(unsigned char timer, unsigned int clock_divider)
76 76 {
77 77 /** This function sets the clock divider of a GPTIMER timer.
78 78 *
79 79 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
80 80 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
81 81 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
82 82 *
83 83 */
84 84
85 85 gptimer_regs->timer[timer].reload = clock_divider; // base clock frequency is 1 MHz
86 86 }
87 87
88 88 // WATCHDOG
89 89
90 90 rtems_isr watchdog_isr( rtems_vector_number vector )
91 91 {
92 92 rtems_status_code status_code;
93 93
94 94 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_12 );
95 95
96 96 PRINTF("watchdog_isr *** this is the end, exit(0)\n");
97 97
98 98 exit(0);
99 99 }
100 100
101 101 void watchdog_configure(void)
102 102 {
103 103 /** This function configure the watchdog.
104 104 *
105 105 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
106 106 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
107 107 *
108 108 * The watchdog is a timer provided by the GPTIMER IP core of the GRLIB.
109 109 *
110 110 */
111 111
112 112 LEON_Mask_interrupt( IRQ_GPTIMER_WATCHDOG ); // mask gptimer/watchdog interrupt during configuration
113 113
114 114 timer_configure( TIMER_WATCHDOG, CLKDIV_WATCHDOG, IRQ_SPARC_GPTIMER_WATCHDOG, watchdog_isr );
115 115
116 116 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG ); // clear gptimer/watchdog interrupt
117 117 }
118 118
119 119 void watchdog_stop(void)
120 120 {
121 121 LEON_Mask_interrupt( IRQ_GPTIMER_WATCHDOG ); // mask gptimer/watchdog interrupt line
122 122 timer_stop( TIMER_WATCHDOG );
123 123 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG ); // clear gptimer/watchdog interrupt
124 124 }
125 125
126 126 void watchdog_reload(void)
127 127 {
128 128 /** This function reloads the watchdog timer counter with the timer reload value.
129 129 *
130 130 * @param void
131 131 *
132 132 * @return void
133 133 *
134 134 */
135 135
136 136 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_LD;
137 137 }
138 138
139 139 void watchdog_start(void)
140 140 {
141 141 /** This function starts the watchdog timer.
142 142 *
143 143 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
144 144 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
145 145 *
146 146 */
147 147
148 148 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG );
149 149
150 150 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_CLEAR_IRQ;
151 151 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_LD;
152 152 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_EN;
153 153 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_IE;
154 154
155 155 LEON_Unmask_interrupt( IRQ_GPTIMER_WATCHDOG );
156 156
157 157 }
158 158
159 159 int enable_apbuart_transmitter( void ) // set the bit 1, TE Transmitter Enable to 1 in the APBUART control register
160 160 {
161 161 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
162 162
163 163 apbuart_regs->ctrl = APBUART_CTRL_REG_MASK_TE;
164 164
165 165 return 0;
166 166 }
167 167
168 168 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value)
169 169 {
170 170 /** This function sets the scaler reload register of the apbuart module
171 171 *
172 172 * @param regs is the address of the apbuart registers in memory
173 173 * @param value is the value that will be stored in the scaler register
174 174 *
175 175 * The value shall be set by the software to get data on the serial interface.
176 176 *
177 177 */
178 178
179 179 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) regs;
180 180
181 181 apbuart_regs->scaler = value;
182 182
183 183 BOOT_PRINTF1("OK *** apbuart port scaler reload register set to 0x%x\n", value)
184 184 }
185 185
186 186 //************
187 187 // RTEMS TASKS
188 188
189 189 rtems_task load_task(rtems_task_argument argument)
190 190 {
191 191 BOOT_PRINTF("in LOAD *** \n")
192 192
193 193 rtems_status_code status;
194 194 unsigned int i;
195 195 unsigned int j;
196 196 rtems_name name_watchdog_rate_monotonic; // name of the watchdog rate monotonic
197 197 rtems_id watchdog_period_id; // id of the watchdog rate monotonic period
198 198
199 199 watchdog_period_id = RTEMS_ID_NONE;
200 200
201 201 name_watchdog_rate_monotonic = rtems_build_name( 'L', 'O', 'A', 'D' );
202 202
203 203 status = rtems_rate_monotonic_create( name_watchdog_rate_monotonic, &watchdog_period_id );
204 204 if( status != RTEMS_SUCCESSFUL ) {
205 205 PRINTF1( "in LOAD *** rtems_rate_monotonic_create failed with status of %d\n", status )
206 206 }
207 207
208 208 i = 0;
209 209 j = 0;
210 210
211 211 watchdog_configure();
212 212
213 213 watchdog_start();
214 214
215 215 set_sy_lfr_watchdog_enabled( true );
216 216
217 217 while(1){
218 218 status = rtems_rate_monotonic_period( watchdog_period_id, WATCHDOG_PERIOD );
219 219 watchdog_reload();
220 220 i = i + 1;
221 221 if ( i == WATCHDOG_LOOP_PRINTF )
222 222 {
223 223 i = 0;
224 224 j = j + 1;
225 225 PRINTF1("%d\n", j)
226 226 }
227 227 #ifdef DEBUG_WATCHDOG
228 228 if (j == WATCHDOG_LOOP_DEBUG )
229 229 {
230 230 status = rtems_task_delete(RTEMS_SELF);
231 231 }
232 232 #endif
233 233 }
234 234 }
235 235
236 236 rtems_task hous_task(rtems_task_argument argument)
237 237 {
238 238 rtems_status_code status;
239 239 rtems_status_code spare_status;
240 240 rtems_id queue_id;
241 241 rtems_rate_monotonic_period_status period_status;
242 242 bool isSynchronized;
243 243
244 244 queue_id = RTEMS_ID_NONE;
245 245 memset(&period_status, 0, sizeof(rtems_rate_monotonic_period_status));
246 246 isSynchronized = false;
247 247
248 248 status = get_message_queue_id_send( &queue_id );
249 249 if (status != RTEMS_SUCCESSFUL)
250 250 {
251 251 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
252 252 }
253 253
254 254 BOOT_PRINTF("in HOUS ***\n");
255 255
256 256 if (rtems_rate_monotonic_ident( name_hk_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
257 257 status = rtems_rate_monotonic_create( name_hk_rate_monotonic, &HK_id );
258 258 if( status != RTEMS_SUCCESSFUL ) {
259 259 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status );
260 260 }
261 261 }
262 262
263 263 status = rtems_rate_monotonic_cancel(HK_id);
264 264 if( status != RTEMS_SUCCESSFUL ) {
265 265 PRINTF1( "ERR *** in HOUS *** rtems_rate_monotonic_cancel(HK_id) ***code: %d\n", status );
266 266 }
267 267 else {
268 268 DEBUG_PRINTF("OK *** in HOUS *** rtems_rate_monotonic_cancel(HK_id)\n");
269 269 }
270 270
271 271 // startup phase
272 272 status = rtems_rate_monotonic_period( HK_id, SY_LFR_TIME_SYN_TIMEOUT_in_ticks );
273 273 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
274 274 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
275 275 while( (period_status.state != RATE_MONOTONIC_EXPIRED)
276 276 && (isSynchronized == false) ) // after SY_LFR_TIME_SYN_TIMEOUT ms, starts HK anyway
277 277 {
278 278 if ((time_management_regs->coarse_time & VAL_LFR_SYNCHRONIZED) == INT32_ALL_0) // check time synchronization
279 279 {
280 280 isSynchronized = true;
281 281 }
282 282 else
283 283 {
284 284 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
285 285
286 286 status = rtems_task_wake_after( HK_SYNC_WAIT ); // wait HK_SYNCH_WAIT 100 ms = 10 * 10 ms
287 287 }
288 288 }
289 289 status = rtems_rate_monotonic_cancel(HK_id);
290 290 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
291 291
292 292 set_hk_lfr_reset_cause( POWER_ON );
293 293
294 294 while(1){ // launch the rate monotonic task
295 295 status = rtems_rate_monotonic_period( HK_id, HK_PERIOD );
296 296 if ( status != RTEMS_SUCCESSFUL ) {
297 297 PRINTF1( "in HOUS *** ERR period: %d\n", status);
298 298 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_6 );
299 299 }
300 300 else {
301 301 housekeeping_packet.packetSequenceControl[BYTE_0] = (unsigned char) (sequenceCounterHK >> SHIFT_1_BYTE);
302 302 housekeeping_packet.packetSequenceControl[BYTE_1] = (unsigned char) (sequenceCounterHK );
303 303 increment_seq_counter( &sequenceCounterHK );
304 304
305 305 housekeeping_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
306 306 housekeeping_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
307 307 housekeeping_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
308 308 housekeeping_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
309 309 housekeeping_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
310 310 housekeeping_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
311 311
312 312 spacewire_update_hk_lfr_link_state( &housekeeping_packet.lfr_status_word[0] );
313 313
314 314 spacewire_read_statistics();
315 315
316 316 update_hk_with_grspw_stats();
317 317
318 318 set_hk_lfr_time_not_synchro();
319 319
320 320 housekeeping_packet.hk_lfr_q_sd_fifo_size_max = hk_lfr_q_sd_fifo_size_max;
321 321 housekeeping_packet.hk_lfr_q_rv_fifo_size_max = hk_lfr_q_rv_fifo_size_max;
322 322 housekeeping_packet.hk_lfr_q_p0_fifo_size_max = hk_lfr_q_p0_fifo_size_max;
323 323 housekeeping_packet.hk_lfr_q_p1_fifo_size_max = hk_lfr_q_p1_fifo_size_max;
324 324 housekeeping_packet.hk_lfr_q_p2_fifo_size_max = hk_lfr_q_p2_fifo_size_max;
325 325
326 326 housekeeping_packet.sy_lfr_common_parameters_spare = parameter_dump_packet.sy_lfr_common_parameters_spare;
327 327 housekeeping_packet.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
328 328 get_temperatures( housekeeping_packet.hk_lfr_temp_scm );
329 329 get_v_e1_e2_f3( housekeeping_packet.hk_lfr_sc_v_f3 );
330 330 get_cpu_load( (unsigned char *) &housekeeping_packet.hk_lfr_cpu_load );
331 331
332 332 hk_lfr_le_me_he_update();
333 333
334 334 // SEND PACKET
335 335 status = rtems_message_queue_send( queue_id, &housekeeping_packet,
336 336 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
337 337 if (status != RTEMS_SUCCESSFUL) {
338 338 PRINTF1("in HOUS *** ERR send: %d\n", status)
339 339 }
340 340 }
341 341 }
342 342
343 343 PRINTF("in HOUS *** deleting task\n")
344 344
345 345 status = rtems_task_delete( RTEMS_SELF ); // should not return
346 346
347 347 return;
348 348 }
349 349
350 int32_t getIntFromShort( int reg )
351 {
352 int16_t ret_as_int16;
353 int32_t ret_as_int32;
354 char *regPtr;
355 char *ret_as_int16_ptr;
356
357 regPtr = (char*) &reg;
358 ret_as_int16_ptr = (char*) &ret_as_int16;
359
360 ret_as_int16_ptr[BYTE_0] = regPtr[BYTE_3];
361 ret_as_int16_ptr[BYTE_1] = regPtr[BYTE_4];
362
363 ret_as_int32 = (int32_t) ret_as_int16;
364
365 return ret_as_int32;
366 }
367
350 368 rtems_task avgv_task(rtems_task_argument argument)
351 369 {
352 370 #define MOVING_AVERAGE 16
353 371 rtems_status_code status;
354 372 static int32_t v[MOVING_AVERAGE] = {0};
355 373 static int32_t e1[MOVING_AVERAGE] = {0};
356 374 static int32_t e2[MOVING_AVERAGE] = {0};
357 375 int32_t average_v;
358 376 int32_t average_e1;
359 377 int32_t average_e2;
360 378 int32_t newValue_v;
361 379 int32_t newValue_e1;
362 380 int32_t newValue_e2;
363 381 unsigned char k;
364 382 unsigned char indexOfOldValue;
365 383
366 384 BOOT_PRINTF("in AVGV ***\n");
367 385
368 386 if (rtems_rate_monotonic_ident( name_avgv_rate_monotonic, &AVGV_id) != RTEMS_SUCCESSFUL) {
369 387 status = rtems_rate_monotonic_create( name_avgv_rate_monotonic, &AVGV_id );
370 388 if( status != RTEMS_SUCCESSFUL ) {
371 389 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status );
372 390 }
373 391 }
374 392
375 393 status = rtems_rate_monotonic_cancel(AVGV_id);
376 394 if( status != RTEMS_SUCCESSFUL ) {
377 395 PRINTF1( "ERR *** in AVGV *** rtems_rate_monotonic_cancel(AVGV_id) ***code: %d\n", status );
378 396 }
379 397 else {
380 398 DEBUG_PRINTF("OK *** in AVGV *** rtems_rate_monotonic_cancel(AVGV_id)\n");
381 399 }
382 400
383 401 // initialize values
384 402 indexOfOldValue = MOVING_AVERAGE - 1;
385 403 average_v = 0;
386 404 average_e1 = 0;
387 405 average_e2 = 0;
388 406 newValue_v = 0;
389 407 newValue_e1 = 0;
390 408 newValue_e2 = 0;
391 409
392 410 k = INIT_CHAR;
393 411
394 412 while(1)
395 413 { // launch the rate monotonic task
396 414 status = rtems_rate_monotonic_period( AVGV_id, AVGV_PERIOD );
397 415 if ( status != RTEMS_SUCCESSFUL )
398 416 {
399 417 PRINTF1( "in AVGV *** ERR period: %d\n", status);
400 418 }
401 419 else
402 420 {
403 421 // get new values
404 newValue_v = (int32_t) waveform_picker_regs->v;
405 newValue_e1 = (int32_t) waveform_picker_regs->e1;
406 newValue_e2 = (int32_t) waveform_picker_regs->e2;
422 newValue_v = getIntFromShort( waveform_picker_regs->v );
423 newValue_e1 = getIntFromShort( waveform_picker_regs->e1 );
424 newValue_e2 = getIntFromShort( waveform_picker_regs->e2 );
407 425
408 426 // compute the moving average
409 427 average_v = average_v + newValue_v - v[k];
410 428 average_e1 = average_e1 + newValue_e1 - e1[k];
411 429 average_e2 = average_e2 + newValue_e2 - e2[k];
412 430
413 431 // store new values in buffers
414 432 v[k] = newValue_v;
415 433 e1[k] = newValue_e1;
416 434 e2[k] = newValue_e2;
417 435 }
418 436 if (k == (MOVING_AVERAGE-1))
419 437 {
420 438 k = 0;
421 439 }
422 440 else
423 441 {
424 442 k++;
425 443 }
426 444 //update int16 values
427 445 hk_lfr_sc_v_f3_as_int16 = (int16_t) (average_v / MOVING_AVERAGE );
428 446 hk_lfr_sc_e1_f3_as_int16 = (int16_t) (average_e1 / MOVING_AVERAGE );
429 447 hk_lfr_sc_e2_f3_as_int16 = (int16_t) (average_e2 / MOVING_AVERAGE );
430 448 }
431 449
432 450 PRINTF("in AVGV *** deleting task\n");
433 451
434 452 status = rtems_task_delete( RTEMS_SELF ); // should not return
435 453
436 454 return;
437 455 }
438 456
439 457 rtems_task dumb_task( rtems_task_argument unused )
440 458 {
441 459 /** This RTEMS taks is used to print messages without affecting the general behaviour of the software.
442 460 *
443 461 * @param unused is the starting argument of the RTEMS task
444 462 *
445 463 * The DUMB taks waits for RTEMS events and print messages depending on the incoming events.
446 464 *
447 465 */
448 466
449 467 unsigned int i;
450 468 unsigned int intEventOut;
451 469 unsigned int coarse_time = 0;
452 470 unsigned int fine_time = 0;
453 471 rtems_event_set event_out;
454 472
455 473 event_out = EVENT_SETS_NONE_PENDING;
456 474
457 475 BOOT_PRINTF("in DUMB *** \n")
458 476
459 477 while(1){
460 478 rtems_event_receive(RTEMS_EVENT_0 | RTEMS_EVENT_1 | RTEMS_EVENT_2 | RTEMS_EVENT_3
461 479 | RTEMS_EVENT_4 | RTEMS_EVENT_5 | RTEMS_EVENT_6 | RTEMS_EVENT_7
462 480 | RTEMS_EVENT_8 | RTEMS_EVENT_9 | RTEMS_EVENT_12 | RTEMS_EVENT_13
463 481 | RTEMS_EVENT_14,
464 482 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT
465 483 intEventOut = (unsigned int) event_out;
466 484 for ( i=0; i<NB_RTEMS_EVENTS; i++)
467 485 {
468 486 if ( ((intEventOut >> i) & 1) != 0)
469 487 {
470 488 coarse_time = time_management_regs->coarse_time;
471 489 fine_time = time_management_regs->fine_time;
472 490 if (i==EVENT_12)
473 491 {
474 492 PRINTF1("%s\n", DUMB_MESSAGE_12)
475 493 }
476 494 if (i==EVENT_13)
477 495 {
478 496 PRINTF1("%s\n", DUMB_MESSAGE_13)
479 497 }
480 498 if (i==EVENT_14)
481 499 {
482 500 PRINTF1("%s\n", DUMB_MESSAGE_1)
483 501 }
484 502 }
485 503 }
486 504 }
487 505 }
488 506
489 507 //*****************************
490 508 // init housekeeping parameters
491 509
492 510 void init_housekeeping_parameters( void )
493 511 {
494 512 /** This function initialize the housekeeping_packet global variable with default values.
495 513 *
496 514 */
497 515
498 516 unsigned int i = 0;
499 517 unsigned char *parameters;
500 518 unsigned char sizeOfHK;
501 519
502 520 sizeOfHK = sizeof( Packet_TM_LFR_HK_t );
503 521
504 522 parameters = (unsigned char*) &housekeeping_packet;
505 523
506 524 for(i = 0; i< sizeOfHK; i++)
507 525 {
508 526 parameters[i] = INIT_CHAR;
509 527 }
510 528
511 529 housekeeping_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
512 530 housekeeping_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
513 531 housekeeping_packet.reserved = DEFAULT_RESERVED;
514 532 housekeeping_packet.userApplication = CCSDS_USER_APP;
515 533 housekeeping_packet.packetID[0] = (unsigned char) (APID_TM_HK >> SHIFT_1_BYTE);
516 534 housekeeping_packet.packetID[1] = (unsigned char) (APID_TM_HK);
517 535 housekeeping_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
518 536 housekeeping_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
519 537 housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> SHIFT_1_BYTE);
520 538 housekeeping_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
521 539 housekeeping_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
522 540 housekeeping_packet.serviceType = TM_TYPE_HK;
523 541 housekeeping_packet.serviceSubType = TM_SUBTYPE_HK;
524 542 housekeeping_packet.destinationID = TM_DESTINATION_ID_GROUND;
525 543 housekeeping_packet.sid = SID_HK;
526 544
527 545 // init status word
528 546 housekeeping_packet.lfr_status_word[0] = DEFAULT_STATUS_WORD_BYTE0;
529 547 housekeeping_packet.lfr_status_word[1] = DEFAULT_STATUS_WORD_BYTE1;
530 548 // init software version
531 549 housekeeping_packet.lfr_sw_version[0] = SW_VERSION_N1;
532 550 housekeeping_packet.lfr_sw_version[1] = SW_VERSION_N2;
533 551 housekeeping_packet.lfr_sw_version[BYTE_2] = SW_VERSION_N3;
534 552 housekeeping_packet.lfr_sw_version[BYTE_3] = SW_VERSION_N4;
535 553 // init fpga version
536 554 parameters = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
537 555 housekeeping_packet.lfr_fpga_version[BYTE_0] = parameters[BYTE_1]; // n1
538 556 housekeeping_packet.lfr_fpga_version[BYTE_1] = parameters[BYTE_2]; // n2
539 557 housekeeping_packet.lfr_fpga_version[BYTE_2] = parameters[BYTE_3]; // n3
540 558
541 559 housekeeping_packet.hk_lfr_q_sd_fifo_size = MSG_QUEUE_COUNT_SEND;
542 560 housekeeping_packet.hk_lfr_q_rv_fifo_size = MSG_QUEUE_COUNT_RECV;
543 561 housekeeping_packet.hk_lfr_q_p0_fifo_size = MSG_QUEUE_COUNT_PRC0;
544 562 housekeeping_packet.hk_lfr_q_p1_fifo_size = MSG_QUEUE_COUNT_PRC1;
545 563 housekeeping_packet.hk_lfr_q_p2_fifo_size = MSG_QUEUE_COUNT_PRC2;
546 564 }
547 565
548 566 void increment_seq_counter( unsigned short *packetSequenceControl )
549 567 {
550 568 /** This function increment the sequence counter passes in argument.
551 569 *
552 570 * The increment does not affect the grouping flag. In case of an overflow, the counter is reset to 0.
553 571 *
554 572 */
555 573
556 574 unsigned short segmentation_grouping_flag;
557 575 unsigned short sequence_cnt;
558 576
559 577 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE; // keep bits 7 downto 6
560 578 sequence_cnt = (*packetSequenceControl) & SEQ_CNT_MASK; // [0011 1111 1111 1111]
561 579
562 580 if ( sequence_cnt < SEQ_CNT_MAX)
563 581 {
564 582 sequence_cnt = sequence_cnt + 1;
565 583 }
566 584 else
567 585 {
568 586 sequence_cnt = 0;
569 587 }
570 588
571 589 *packetSequenceControl = segmentation_grouping_flag | sequence_cnt ;
572 590 }
573 591
574 592 void getTime( unsigned char *time)
575 593 {
576 594 /** This function write the current local time in the time buffer passed in argument.
577 595 *
578 596 */
579 597
580 598 time[0] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_3_BYTES);
581 599 time[1] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_2_BYTES);
582 600 time[2] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_1_BYTE);
583 601 time[3] = (unsigned char) (time_management_regs->coarse_time);
584 602 time[4] = (unsigned char) (time_management_regs->fine_time>>SHIFT_1_BYTE);
585 603 time[5] = (unsigned char) (time_management_regs->fine_time);
586 604 }
587 605
588 606 unsigned long long int getTimeAsUnsignedLongLongInt( )
589 607 {
590 608 /** This function write the current local time in the time buffer passed in argument.
591 609 *
592 610 */
593 611 unsigned long long int time;
594 612
595 613 time = ( (unsigned long long int) (time_management_regs->coarse_time & COARSE_TIME_MASK) << SHIFT_2_BYTES )
596 614 + time_management_regs->fine_time;
597 615
598 616 return time;
599 617 }
600 618
601 619 void send_dumb_hk( void )
602 620 {
603 621 Packet_TM_LFR_HK_t dummy_hk_packet;
604 622 unsigned char *parameters;
605 623 unsigned int i;
606 624 rtems_id queue_id;
607 625
608 626 queue_id = RTEMS_ID_NONE;
609 627
610 628 dummy_hk_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
611 629 dummy_hk_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
612 630 dummy_hk_packet.reserved = DEFAULT_RESERVED;
613 631 dummy_hk_packet.userApplication = CCSDS_USER_APP;
614 632 dummy_hk_packet.packetID[0] = (unsigned char) (APID_TM_HK >> SHIFT_1_BYTE);
615 633 dummy_hk_packet.packetID[1] = (unsigned char) (APID_TM_HK);
616 634 dummy_hk_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
617 635 dummy_hk_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
618 636 dummy_hk_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> SHIFT_1_BYTE);
619 637 dummy_hk_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
620 638 dummy_hk_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
621 639 dummy_hk_packet.serviceType = TM_TYPE_HK;
622 640 dummy_hk_packet.serviceSubType = TM_SUBTYPE_HK;
623 641 dummy_hk_packet.destinationID = TM_DESTINATION_ID_GROUND;
624 642 dummy_hk_packet.time[0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
625 643 dummy_hk_packet.time[1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
626 644 dummy_hk_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
627 645 dummy_hk_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
628 646 dummy_hk_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
629 647 dummy_hk_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
630 648 dummy_hk_packet.sid = SID_HK;
631 649
632 650 // init status word
633 651 dummy_hk_packet.lfr_status_word[0] = INT8_ALL_F;
634 652 dummy_hk_packet.lfr_status_word[1] = INT8_ALL_F;
635 653 // init software version
636 654 dummy_hk_packet.lfr_sw_version[0] = SW_VERSION_N1;
637 655 dummy_hk_packet.lfr_sw_version[1] = SW_VERSION_N2;
638 656 dummy_hk_packet.lfr_sw_version[BYTE_2] = SW_VERSION_N3;
639 657 dummy_hk_packet.lfr_sw_version[BYTE_3] = SW_VERSION_N4;
640 658 // init fpga version
641 659 parameters = (unsigned char *) (REGS_ADDR_WAVEFORM_PICKER + APB_OFFSET_VHDL_REV);
642 660 dummy_hk_packet.lfr_fpga_version[BYTE_0] = parameters[BYTE_1]; // n1
643 661 dummy_hk_packet.lfr_fpga_version[BYTE_1] = parameters[BYTE_2]; // n2
644 662 dummy_hk_packet.lfr_fpga_version[BYTE_2] = parameters[BYTE_3]; // n3
645 663
646 664 parameters = (unsigned char *) &dummy_hk_packet.hk_lfr_cpu_load;
647 665
648 666 for (i=0; i<(BYTE_POS_HK_REACTION_WHEELS_FREQUENCY - BYTE_POS_HK_LFR_CPU_LOAD); i++)
649 667 {
650 668 parameters[i] = INT8_ALL_F;
651 669 }
652 670
653 671 get_message_queue_id_send( &queue_id );
654 672
655 673 rtems_message_queue_send( queue_id, &dummy_hk_packet,
656 674 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
657 675 }
658 676
659 677 void get_temperatures( unsigned char *temperatures )
660 678 {
661 679 unsigned char* temp_scm_ptr;
662 680 unsigned char* temp_pcb_ptr;
663 681 unsigned char* temp_fpga_ptr;
664 682
665 683 // SEL1 SEL0
666 684 // 0 0 => PCB
667 685 // 0 1 => FPGA
668 686 // 1 0 => SCM
669 687
670 688 temp_scm_ptr = (unsigned char *) &time_management_regs->temp_scm;
671 689 temp_pcb_ptr = (unsigned char *) &time_management_regs->temp_pcb;
672 690 temp_fpga_ptr = (unsigned char *) &time_management_regs->temp_fpga;
673 691
674 692 temperatures[ BYTE_0 ] = temp_scm_ptr[ BYTE_2 ];
675 693 temperatures[ BYTE_1 ] = temp_scm_ptr[ BYTE_3 ];
676 694 temperatures[ BYTE_2 ] = temp_pcb_ptr[ BYTE_2 ];
677 695 temperatures[ BYTE_3 ] = temp_pcb_ptr[ BYTE_3 ];
678 696 temperatures[ BYTE_4 ] = temp_fpga_ptr[ BYTE_2 ];
679 697 temperatures[ BYTE_5 ] = temp_fpga_ptr[ BYTE_3 ];
680 698 }
681 699
682 700 void get_v_e1_e2_f3( unsigned char *spacecraft_potential )
683 701 {
684 702 unsigned char* v_ptr;
685 703 unsigned char* e1_ptr;
686 704 unsigned char* e2_ptr;
687 705
688 706 v_ptr = (unsigned char *) &hk_lfr_sc_v_f3_as_int16;
689 707 e1_ptr = (unsigned char *) &hk_lfr_sc_e1_f3_as_int16;
690 708 e2_ptr = (unsigned char *) &hk_lfr_sc_e2_f3_as_int16;
691 709
692 710 spacecraft_potential[BYTE_0] = v_ptr[0];
693 711 spacecraft_potential[BYTE_1] = v_ptr[1];
694 712 spacecraft_potential[BYTE_2] = e1_ptr[0];
695 713 spacecraft_potential[BYTE_3] = e1_ptr[1];
696 714 spacecraft_potential[BYTE_4] = e2_ptr[0];
697 715 spacecraft_potential[BYTE_5] = e2_ptr[1];
698 716 }
699 717
700 718 void get_cpu_load( unsigned char *resource_statistics )
701 719 {
702 720 unsigned char cpu_load;
703 721
704 722 cpu_load = lfr_rtems_cpu_usage_report();
705 723
706 724 // HK_LFR_CPU_LOAD
707 725 resource_statistics[0] = cpu_load;
708 726
709 727 // HK_LFR_CPU_LOAD_MAX
710 728 if (cpu_load > resource_statistics[1])
711 729 {
712 730 resource_statistics[1] = cpu_load;
713 731 }
714 732
715 733 // CPU_LOAD_AVE
716 734 resource_statistics[BYTE_2] = 0;
717 735
718 736 #ifndef PRINT_TASK_STATISTICS
719 737 rtems_cpu_usage_reset();
720 738 #endif
721 739
722 740 }
723 741
724 742 void set_hk_lfr_sc_potential_flag( bool state )
725 743 {
726 744 if (state == true)
727 745 {
728 746 housekeeping_packet.lfr_status_word[1] =
729 747 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_SC_POTENTIAL_FLAG_BIT; // [0100 0000]
730 748 }
731 749 else
732 750 {
733 751 housekeeping_packet.lfr_status_word[1] =
734 752 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_SC_POTENTIAL_FLAG_MASK; // [1011 1111]
735 753 }
736 754 }
737 755
738 756 void set_sy_lfr_pas_filter_enabled( bool state )
739 757 {
740 758 if (state == true)
741 759 {
742 760 housekeeping_packet.lfr_status_word[1] =
743 761 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_PAS_FILTER_ENABLED_BIT; // [0010 0000]
744 762 }
745 763 else
746 764 {
747 765 housekeeping_packet.lfr_status_word[1] =
748 766 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_PAS_FILTER_ENABLED_MASK; // [1101 1111]
749 767 }
750 768 }
751 769
752 770 void set_sy_lfr_watchdog_enabled( bool state )
753 771 {
754 772 if (state == true)
755 773 {
756 774 housekeeping_packet.lfr_status_word[1] =
757 775 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_WATCHDOG_BIT; // [0001 0000]
758 776 }
759 777 else
760 778 {
761 779 housekeeping_packet.lfr_status_word[1] =
762 780 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_WATCHDOG_MASK; // [1110 1111]
763 781 }
764 782 }
765 783
766 784 void set_hk_lfr_calib_enable( bool state )
767 785 {
768 786 if (state == true)
769 787 {
770 788 housekeeping_packet.lfr_status_word[1] =
771 789 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_CALIB_BIT; // [0000 1000]
772 790 }
773 791 else
774 792 {
775 793 housekeeping_packet.lfr_status_word[1] =
776 794 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_CALIB_MASK; // [1111 0111]
777 795 }
778 796 }
779 797
780 798 void set_hk_lfr_reset_cause( enum lfr_reset_cause_t lfr_reset_cause )
781 799 {
782 800 housekeeping_packet.lfr_status_word[1] =
783 801 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_RESET_CAUSE_MASK; // [1111 1000]
784 802
785 803 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1]
786 804 | (lfr_reset_cause & STATUS_WORD_RESET_CAUSE_BITS ); // [0000 0111]
787 805
788 806 }
789 807
790 808 void increment_hk_counter( unsigned char newValue, unsigned char oldValue, unsigned int *counter )
791 809 {
792 810 int delta;
793 811
794 812 delta = 0;
795 813
796 814 if (newValue >= oldValue)
797 815 {
798 816 delta = newValue - oldValue;
799 817 }
800 818 else
801 819 {
802 820 delta = (CONST_256 - oldValue) + newValue;
803 821 }
804 822
805 823 *counter = *counter + delta;
806 824 }
807 825
808 826 void hk_lfr_le_update( void )
809 827 {
810 828 static hk_lfr_le_t old_hk_lfr_le = {0};
811 829 hk_lfr_le_t new_hk_lfr_le;
812 830 unsigned int counter;
813 831
814 832 counter = (((unsigned int) housekeeping_packet.hk_lfr_le_cnt[0]) * CONST_256) + housekeeping_packet.hk_lfr_le_cnt[1];
815 833
816 834 // DPU
817 835 new_hk_lfr_le.dpu_spw_parity = housekeeping_packet.hk_lfr_dpu_spw_parity;
818 836 new_hk_lfr_le.dpu_spw_disconnect= housekeeping_packet.hk_lfr_dpu_spw_disconnect;
819 837 new_hk_lfr_le.dpu_spw_escape = housekeeping_packet.hk_lfr_dpu_spw_escape;
820 838 new_hk_lfr_le.dpu_spw_credit = housekeeping_packet.hk_lfr_dpu_spw_credit;
821 839 new_hk_lfr_le.dpu_spw_write_sync= housekeeping_packet.hk_lfr_dpu_spw_write_sync;
822 840 // TIMECODE
823 841 new_hk_lfr_le.timecode_erroneous= housekeeping_packet.hk_lfr_timecode_erroneous;
824 842 new_hk_lfr_le.timecode_missing = housekeeping_packet.hk_lfr_timecode_missing;
825 843 new_hk_lfr_le.timecode_invalid = housekeeping_packet.hk_lfr_timecode_invalid;
826 844 // TIME
827 845 new_hk_lfr_le.time_timecode_it = housekeeping_packet.hk_lfr_time_timecode_it;
828 846 new_hk_lfr_le.time_not_synchro = housekeeping_packet.hk_lfr_time_not_synchro;
829 847 new_hk_lfr_le.time_timecode_ctr = housekeeping_packet.hk_lfr_time_timecode_ctr;
830 848 //AHB
831 849 new_hk_lfr_le.ahb_correctable = housekeeping_packet.hk_lfr_ahb_correctable;
832 850 // housekeeping_packet.hk_lfr_dpu_spw_rx_ahb => not handled by the grspw driver
833 851 // housekeeping_packet.hk_lfr_dpu_spw_tx_ahb => not handled by the grspw driver
834 852
835 853 // update the le counter
836 854 // DPU
837 855 increment_hk_counter( new_hk_lfr_le.dpu_spw_parity, old_hk_lfr_le.dpu_spw_parity, &counter );
838 856 increment_hk_counter( new_hk_lfr_le.dpu_spw_disconnect,old_hk_lfr_le.dpu_spw_disconnect, &counter );
839 857 increment_hk_counter( new_hk_lfr_le.dpu_spw_escape, old_hk_lfr_le.dpu_spw_escape, &counter );
840 858 increment_hk_counter( new_hk_lfr_le.dpu_spw_credit, old_hk_lfr_le.dpu_spw_credit, &counter );
841 859 increment_hk_counter( new_hk_lfr_le.dpu_spw_write_sync,old_hk_lfr_le.dpu_spw_write_sync, &counter );
842 860 // TIMECODE
843 861 increment_hk_counter( new_hk_lfr_le.timecode_erroneous,old_hk_lfr_le.timecode_erroneous, &counter );
844 862 increment_hk_counter( new_hk_lfr_le.timecode_missing, old_hk_lfr_le.timecode_missing, &counter );
845 863 increment_hk_counter( new_hk_lfr_le.timecode_invalid, old_hk_lfr_le.timecode_invalid, &counter );
846 864 // TIME
847 865 increment_hk_counter( new_hk_lfr_le.time_timecode_it, old_hk_lfr_le.time_timecode_it, &counter );
848 866 increment_hk_counter( new_hk_lfr_le.time_not_synchro, old_hk_lfr_le.time_not_synchro, &counter );
849 867 increment_hk_counter( new_hk_lfr_le.time_timecode_ctr, old_hk_lfr_le.time_timecode_ctr, &counter );
850 868 // AHB
851 869 increment_hk_counter( new_hk_lfr_le.ahb_correctable, old_hk_lfr_le.ahb_correctable, &counter );
852 870
853 871 // DPU
854 872 old_hk_lfr_le.dpu_spw_parity = new_hk_lfr_le.dpu_spw_parity;
855 873 old_hk_lfr_le.dpu_spw_disconnect= new_hk_lfr_le.dpu_spw_disconnect;
856 874 old_hk_lfr_le.dpu_spw_escape = new_hk_lfr_le.dpu_spw_escape;
857 875 old_hk_lfr_le.dpu_spw_credit = new_hk_lfr_le.dpu_spw_credit;
858 876 old_hk_lfr_le.dpu_spw_write_sync= new_hk_lfr_le.dpu_spw_write_sync;
859 877 // TIMECODE
860 878 old_hk_lfr_le.timecode_erroneous= new_hk_lfr_le.timecode_erroneous;
861 879 old_hk_lfr_le.timecode_missing = new_hk_lfr_le.timecode_missing;
862 880 old_hk_lfr_le.timecode_invalid = new_hk_lfr_le.timecode_invalid;
863 881 // TIME
864 882 old_hk_lfr_le.time_timecode_it = new_hk_lfr_le.time_timecode_it;
865 883 old_hk_lfr_le.time_not_synchro = new_hk_lfr_le.time_not_synchro;
866 884 old_hk_lfr_le.time_timecode_ctr = new_hk_lfr_le.time_timecode_ctr;
867 885 //AHB
868 886 old_hk_lfr_le.ahb_correctable = new_hk_lfr_le.ahb_correctable;
869 887 // housekeeping_packet.hk_lfr_dpu_spw_rx_ahb => not handled by the grspw driver
870 888 // housekeeping_packet.hk_lfr_dpu_spw_tx_ahb => not handled by the grspw driver
871 889
872 890 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
873 891 // LE
874 892 housekeeping_packet.hk_lfr_le_cnt[0] = (unsigned char) ((counter & BYTE0_MASK) >> SHIFT_1_BYTE);
875 893 housekeeping_packet.hk_lfr_le_cnt[1] = (unsigned char) (counter & BYTE1_MASK);
876 894 }
877 895
878 896 void hk_lfr_me_update( void )
879 897 {
880 898 static hk_lfr_me_t old_hk_lfr_me = {0};
881 899 hk_lfr_me_t new_hk_lfr_me;
882 900 unsigned int counter;
883 901
884 902 counter = (((unsigned int) housekeeping_packet.hk_lfr_me_cnt[0]) * CONST_256) + housekeeping_packet.hk_lfr_me_cnt[1];
885 903
886 904 // get the current values
887 905 new_hk_lfr_me.dpu_spw_early_eop = housekeeping_packet.hk_lfr_dpu_spw_early_eop;
888 906 new_hk_lfr_me.dpu_spw_invalid_addr = housekeeping_packet.hk_lfr_dpu_spw_invalid_addr;
889 907 new_hk_lfr_me.dpu_spw_eep = housekeeping_packet.hk_lfr_dpu_spw_eep;
890 908 new_hk_lfr_me.dpu_spw_rx_too_big = housekeeping_packet.hk_lfr_dpu_spw_rx_too_big;
891 909
892 910 // update the me counter
893 911 increment_hk_counter( new_hk_lfr_me.dpu_spw_early_eop, old_hk_lfr_me.dpu_spw_early_eop, &counter );
894 912 increment_hk_counter( new_hk_lfr_me.dpu_spw_invalid_addr, old_hk_lfr_me.dpu_spw_invalid_addr, &counter );
895 913 increment_hk_counter( new_hk_lfr_me.dpu_spw_eep, old_hk_lfr_me.dpu_spw_eep, &counter );
896 914 increment_hk_counter( new_hk_lfr_me.dpu_spw_rx_too_big, old_hk_lfr_me.dpu_spw_rx_too_big, &counter );
897 915
898 916 // store the counters for the next time
899 917 old_hk_lfr_me.dpu_spw_early_eop = new_hk_lfr_me.dpu_spw_early_eop;
900 918 old_hk_lfr_me.dpu_spw_invalid_addr = new_hk_lfr_me.dpu_spw_invalid_addr;
901 919 old_hk_lfr_me.dpu_spw_eep = new_hk_lfr_me.dpu_spw_eep;
902 920 old_hk_lfr_me.dpu_spw_rx_too_big = new_hk_lfr_me.dpu_spw_rx_too_big;
903 921
904 922 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
905 923 // ME
906 924 housekeeping_packet.hk_lfr_me_cnt[0] = (unsigned char) ((counter & BYTE0_MASK) >> SHIFT_1_BYTE);
907 925 housekeeping_packet.hk_lfr_me_cnt[1] = (unsigned char) (counter & BYTE1_MASK);
908 926 }
909 927
910 928 void hk_lfr_le_me_he_update()
911 929 {
912 930
913 931 unsigned int hk_lfr_he_cnt;
914 932
915 933 hk_lfr_he_cnt = (((unsigned int) housekeeping_packet.hk_lfr_he_cnt[0]) * 256) + housekeeping_packet.hk_lfr_he_cnt[1];
916 934
917 935 //update the low severity error counter
918 936 hk_lfr_le_update( );
919 937
920 938 //update the medium severity error counter
921 939 hk_lfr_me_update();
922 940
923 941 //update the high severity error counter
924 942 hk_lfr_he_cnt = 0;
925 943
926 944 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
927 945 // HE
928 946 housekeeping_packet.hk_lfr_he_cnt[0] = (unsigned char) ((hk_lfr_he_cnt & BYTE0_MASK) >> SHIFT_1_BYTE);
929 947 housekeeping_packet.hk_lfr_he_cnt[1] = (unsigned char) (hk_lfr_he_cnt & BYTE1_MASK);
930 948
931 949 }
932 950
933 951 void set_hk_lfr_time_not_synchro()
934 952 {
935 953 static unsigned char synchroLost = 1;
936 954 int synchronizationBit;
937 955
938 956 // get the synchronization bit
939 957 synchronizationBit =
940 958 (time_management_regs->coarse_time & VAL_LFR_SYNCHRONIZED) >> BIT_SYNCHRONIZATION; // 1000 0000 0000 0000
941 959
942 960 switch (synchronizationBit)
943 961 {
944 962 case 0:
945 963 if (synchroLost == 1)
946 964 {
947 965 synchroLost = 0;
948 966 }
949 967 break;
950 968 case 1:
951 969 if (synchroLost == 0 )
952 970 {
953 971 synchroLost = 1;
954 972 increase_unsigned_char_counter(&housekeeping_packet.hk_lfr_time_not_synchro);
955 973 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_NOT_SYNCHRO );
956 974 }
957 975 break;
958 976 default:
959 977 PRINTF1("in hk_lfr_time_not_synchro *** unexpected value for synchronizationBit = %d\n", synchronizationBit);
960 978 break;
961 979 }
962 980
963 981 }
964 982
965 983 void set_hk_lfr_ahb_correctable() // CRITICITY L
966 984 {
967 985 /** This function builds the error counter hk_lfr_ahb_correctable using the statistics provided
968 986 * by the Cache Control Register (ASI 2, offset 0) and in the Register Protection Control Register (ASR16) on the
969 987 * detected errors in the cache, in the integer unit and in the floating point unit.
970 988 *
971 989 * @param void
972 990 *
973 991 * @return void
974 992 *
975 993 * All errors are summed to set the value of the hk_lfr_ahb_correctable counter.
976 994 *
977 995 */
978 996
979 997 unsigned int ahb_correctable;
980 998 unsigned int instructionErrorCounter;
981 999 unsigned int dataErrorCounter;
982 1000 unsigned int fprfErrorCounter;
983 1001 unsigned int iurfErrorCounter;
984 1002
985 1003 instructionErrorCounter = 0;
986 1004 dataErrorCounter = 0;
987 1005 fprfErrorCounter = 0;
988 1006 iurfErrorCounter = 0;
989 1007
990 1008 CCR_getInstructionAndDataErrorCounters( &instructionErrorCounter, &dataErrorCounter);
991 1009 ASR16_get_FPRF_IURF_ErrorCounters( &fprfErrorCounter, &iurfErrorCounter);
992 1010
993 1011 ahb_correctable = instructionErrorCounter
994 1012 + dataErrorCounter
995 1013 + fprfErrorCounter
996 1014 + iurfErrorCounter
997 1015 + housekeeping_packet.hk_lfr_ahb_correctable;
998 1016
999 1017 housekeeping_packet.hk_lfr_ahb_correctable = (unsigned char) (ahb_correctable & INT8_ALL_F); // [1111 1111]
1000 1018
1001 1019 }
@@ -1,817 +1,818
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 = 0;
15 15 unsigned int nb_sm_f0_aux_f1= 0;
16 16 unsigned int nb_sm_f1 = 0;
17 17 unsigned int nb_sm_f0_aux_f2= 0;
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 ] = {0};
29 29 ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ] = {0};
30 30 ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ] = {0};
31 31 ring_node *current_ring_node_sm_f0 = NULL;
32 32 ring_node *current_ring_node_sm_f1 = NULL;
33 33 ring_node *current_ring_node_sm_f2 = NULL;
34 34 ring_node *ring_node_for_averaging_sm_f0= NULL;
35 35 ring_node *ring_node_for_averaging_sm_f1= NULL;
36 36 ring_node *ring_node_for_averaging_sm_f2= NULL;
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 CHANNELF0:
46 46 node = ring_node_for_averaging_sm_f0;
47 47 break;
48 48 case CHANNELF1:
49 49 node = ring_node_for_averaging_sm_f1;
50 50 break;
51 51 case CHANNELF2:
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 & BITS_STATUS_F0); // [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 BIT_READY_0_1:
77 77 // UNEXPECTED VALUE
78 78 spectral_matrix_regs->status = BIT_READY_0_1; // [0011]
79 79 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
80 80 break;
81 81 case BIT_READY_0:
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_F1)
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 = BIT_READY_0; // [0000 0001]
99 99 break;
100 100 case BIT_READY_1:
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_F1)
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 = BIT_READY_1; // [0000 0010]
118 118 break;
119 119 default:
120 120 break;
121 121 }
122 122 }
123 123
124 124 void spectral_matrices_isr_f1( int statusReg )
125 125 {
126 126 rtems_status_code status_code;
127 127 unsigned char status;
128 128 ring_node *full_ring_node;
129 129
130 130 status = (unsigned char) ((statusReg & BITS_STATUS_F1) >> SHIFT_2_BITS); // [1100] get the status_ready_matrix_f1_x bits
131 131
132 132 switch(status)
133 133 {
134 134 case 0:
135 135 break;
136 136 case BIT_READY_0_1:
137 137 // UNEXPECTED VALUE
138 138 spectral_matrix_regs->status = BITS_STATUS_F1; // [1100]
139 139 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
140 140 break;
141 141 case BIT_READY_0:
142 142 full_ring_node = current_ring_node_sm_f1->previous;
143 143 full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
144 144 full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
145 145 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
146 146 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
147 147 // if there are enough ring nodes ready, wake up an AVFx task
148 148 nb_sm_f1 = nb_sm_f1 + 1;
149 149 if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
150 150 {
151 151 ring_node_for_averaging_sm_f1 = full_ring_node;
152 152 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
153 153 {
154 154 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
155 155 }
156 156 nb_sm_f1 = 0;
157 157 }
158 158 spectral_matrix_regs->status = BIT_STATUS_F1_0; // [0000 0100]
159 159 break;
160 160 case BIT_READY_1:
161 161 full_ring_node = current_ring_node_sm_f1->previous;
162 162 full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
163 163 full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
164 164 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
165 165 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
166 166 // if there are enough ring nodes ready, wake up an AVFx task
167 167 nb_sm_f1 = nb_sm_f1 + 1;
168 168 if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
169 169 {
170 170 ring_node_for_averaging_sm_f1 = full_ring_node;
171 171 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
172 172 {
173 173 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
174 174 }
175 175 nb_sm_f1 = 0;
176 176 }
177 177 spectral_matrix_regs->status = BIT_STATUS_F1_1; // [1000 0000]
178 178 break;
179 179 default:
180 180 break;
181 181 }
182 182 }
183 183
184 184 void spectral_matrices_isr_f2( int statusReg )
185 185 {
186 186 unsigned char status;
187 187 rtems_status_code status_code;
188 188
189 189 status = (unsigned char) ((statusReg & BITS_STATUS_F2) >> SHIFT_4_BITS); // [0011 0000] get the status_ready_matrix_f2_x bits
190 190
191 191 switch(status)
192 192 {
193 193 case 0:
194 194 break;
195 195 case BIT_READY_0_1:
196 196 // UNEXPECTED VALUE
197 197 spectral_matrix_regs->status = BITS_STATUS_F2; // [0011 0000]
198 198 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
199 199 break;
200 200 case BIT_READY_0:
201 201 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
202 202 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
203 203 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
204 204 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
205 205 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
206 206 spectral_matrix_regs->status = BIT_STATUS_F2_0; // [0001 0000]
207 207 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
208 208 {
209 209 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
210 210 }
211 211 break;
212 212 case BIT_READY_1:
213 213 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
214 214 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
215 215 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
216 216 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
217 217 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
218 218 spectral_matrix_regs->status = BIT_STATUS_F2_1; // [0010 0000]
219 219 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
220 220 {
221 221 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
222 222 }
223 223 break;
224 224 default:
225 225 break;
226 226 }
227 227 }
228 228
229 229 void spectral_matrix_isr_error_handler( int statusReg )
230 230 {
231 231 // STATUS REGISTER
232 232 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
233 233 // 10 9 8
234 234 // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
235 235 // 7 6 5 4 3 2 1 0
236 236 // [bad_component_err] not defined in the last version of the VHDL code
237 237
238 238 rtems_status_code status_code;
239 239
240 240 //***************************************************
241 241 // the ASM status register is copied in the HK packet
242 242 housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) ((statusReg & BITS_HK_AA_SM) >> SHIFT_7_BITS); // [0111 1000 0000]
243 243
244 244 if (statusReg & BITS_SM_ERR) // [0111 1100 0000]
245 245 {
246 246 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
247 247 }
248 248
249 249 spectral_matrix_regs->status = spectral_matrix_regs->status & BITS_SM_ERR;
250 250
251 251 }
252 252
253 253 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
254 254 {
255 255 // STATUS REGISTER
256 256 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
257 257 // 10 9 8
258 258 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
259 259 // 7 6 5 4 3 2 1 0
260 260
261 261 int statusReg;
262 262
263 263 static restartState state = WAIT_FOR_F2;
264 264
265 265 statusReg = spectral_matrix_regs->status;
266 266
267 267 if (thisIsAnASMRestart == 0)
268 268 { // this is not a restart sequence, process incoming matrices normally
269 269 spectral_matrices_isr_f0( statusReg );
270 270
271 271 spectral_matrices_isr_f1( statusReg );
272 272
273 273 spectral_matrices_isr_f2( statusReg );
274 274 }
275 275 else
276 276 { // a restart sequence has to be launched
277 277 switch (state) {
278 278 case WAIT_FOR_F2:
279 279 if ((statusReg & BITS_STATUS_F2) != INIT_CHAR) // [0011 0000] check the status_ready_matrix_f2_x bits
280 280 {
281 281 state = WAIT_FOR_F1;
282 282 }
283 283 break;
284 284 case WAIT_FOR_F1:
285 285 if ((statusReg & BITS_STATUS_F1) != INIT_CHAR) // [0000 1100] check the status_ready_matrix_f1_x bits
286 286 {
287 287 state = WAIT_FOR_F0;
288 288 }
289 289 break;
290 290 case WAIT_FOR_F0:
291 291 if ((statusReg & BITS_STATUS_F0) != INIT_CHAR) // [0000 0011] check the status_ready_matrix_f0_x bits
292 292 {
293 293 state = WAIT_FOR_F2;
294 294 thisIsAnASMRestart = 0;
295 295 }
296 296 break;
297 297 default:
298 298 break;
299 299 }
300 300 reset_sm_status();
301 301 }
302 302
303 303 spectral_matrix_isr_error_handler( statusReg );
304 304
305 305 }
306 306
307 307 //******************
308 308 // Spectral Matrices
309 309
310 310 void reset_nb_sm( void )
311 311 {
312 312 nb_sm_f0 = 0;
313 313 nb_sm_f0_aux_f1 = 0;
314 314 nb_sm_f0_aux_f2 = 0;
315 315
316 316 nb_sm_f1 = 0;
317 317 }
318 318
319 319 void SM_init_rings( void )
320 320 {
321 321 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
322 322 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
323 323 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
324 324
325 325 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
326 326 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
327 327 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
328 328 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
329 329 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
330 330 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
331 331 }
332 332
333 333 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
334 334 {
335 335 unsigned char i;
336 336
337 337 ring[ nbNodes - 1 ].next
338 338 = (ring_node_asm*) &ring[ 0 ];
339 339
340 340 for(i=0; i<nbNodes-1; i++)
341 341 {
342 342 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
343 343 }
344 344 }
345 345
346 346 void SM_reset_current_ring_nodes( void )
347 347 {
348 348 current_ring_node_sm_f0 = sm_ring_f0[0].next;
349 349 current_ring_node_sm_f1 = sm_ring_f1[0].next;
350 350 current_ring_node_sm_f2 = sm_ring_f2[0].next;
351 351
352 352 ring_node_for_averaging_sm_f0 = NULL;
353 353 ring_node_for_averaging_sm_f1 = NULL;
354 354 ring_node_for_averaging_sm_f2 = NULL;
355 355 }
356 356
357 357 //*****************
358 358 // Basic Parameters
359 359
360 360 void BP_init_header( bp_packet *packet,
361 361 unsigned int apid, unsigned char sid,
362 362 unsigned int packetLength, unsigned char blkNr )
363 363 {
364 364 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
365 365 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
366 366 packet->reserved = INIT_CHAR;
367 367 packet->userApplication = CCSDS_USER_APP;
368 368 packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
369 369 packet->packetID[1] = (unsigned char) (apid);
370 370 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
371 371 packet->packetSequenceControl[1] = INIT_CHAR;
372 372 packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
373 373 packet->packetLength[1] = (unsigned char) (packetLength);
374 374 // DATA FIELD HEADER
375 375 packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
376 376 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
377 377 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
378 378 packet->destinationID = TM_DESTINATION_ID_GROUND;
379 379 packet->time[BYTE_0] = INIT_CHAR;
380 380 packet->time[BYTE_1] = INIT_CHAR;
381 381 packet->time[BYTE_2] = INIT_CHAR;
382 382 packet->time[BYTE_3] = INIT_CHAR;
383 383 packet->time[BYTE_4] = INIT_CHAR;
384 384 packet->time[BYTE_5] = INIT_CHAR;
385 385 // AUXILIARY DATA HEADER
386 386 packet->sid = sid;
387 387 packet->pa_bia_status_info = INIT_CHAR;
388 388 packet->sy_lfr_common_parameters_spare = INIT_CHAR;
389 389 packet->sy_lfr_common_parameters = INIT_CHAR;
390 390 packet->acquisitionTime[BYTE_0] = INIT_CHAR;
391 391 packet->acquisitionTime[BYTE_1] = INIT_CHAR;
392 392 packet->acquisitionTime[BYTE_2] = INIT_CHAR;
393 393 packet->acquisitionTime[BYTE_3] = INIT_CHAR;
394 394 packet->acquisitionTime[BYTE_4] = INIT_CHAR;
395 395 packet->acquisitionTime[BYTE_5] = INIT_CHAR;
396 396 packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB
397 397 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
398 398 }
399 399
400 400 void BP_init_header_with_spare( bp_packet_with_spare *packet,
401 401 unsigned int apid, unsigned char sid,
402 402 unsigned int packetLength , unsigned char blkNr)
403 403 {
404 404 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
405 405 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
406 406 packet->reserved = INIT_CHAR;
407 407 packet->userApplication = CCSDS_USER_APP;
408 408 packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
409 409 packet->packetID[1] = (unsigned char) (apid);
410 410 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
411 411 packet->packetSequenceControl[1] = INIT_CHAR;
412 412 packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
413 413 packet->packetLength[1] = (unsigned char) (packetLength);
414 414 // DATA FIELD HEADER
415 415 packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
416 416 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
417 417 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
418 418 packet->destinationID = TM_DESTINATION_ID_GROUND;
419 419 // AUXILIARY DATA HEADER
420 420 packet->sid = sid;
421 421 packet->pa_bia_status_info = INIT_CHAR;
422 422 packet->sy_lfr_common_parameters_spare = INIT_CHAR;
423 423 packet->sy_lfr_common_parameters = INIT_CHAR;
424 424 packet->time[BYTE_0] = INIT_CHAR;
425 425 packet->time[BYTE_1] = INIT_CHAR;
426 426 packet->time[BYTE_2] = INIT_CHAR;
427 427 packet->time[BYTE_3] = INIT_CHAR;
428 428 packet->time[BYTE_4] = INIT_CHAR;
429 429 packet->time[BYTE_5] = INIT_CHAR;
430 430 packet->source_data_spare = INIT_CHAR;
431 431 packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB
432 432 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
433 433 }
434 434
435 435 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
436 436 {
437 437 rtems_status_code status;
438 438
439 439 // SEND PACKET
440 440 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
441 441 if (status != RTEMS_SUCCESSFUL)
442 442 {
443 443 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
444 444 }
445 445 }
446 446
447 447 void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
448 448 {
449 449 /** This function is used to send the BP paquets when needed.
450 450 *
451 451 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
452 452 *
453 453 * @return void
454 454 *
455 455 * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition.
456 456 * BURST paquets are sent everytime.
457 457 *
458 458 */
459 459
460 460 rtems_status_code status;
461 461
462 462 // SEND PACKET
463 463 // before lastValidTransitionDate, the data are drops even if they are ready
464 464 // this guarantees that no SBM packets will be received before the requested enter mode time
465 465 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
466 466 {
467 467 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
468 468 if (status != RTEMS_SUCCESSFUL)
469 469 {
470 470 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
471 471 }
472 472 }
473 473 }
474 474
475 475 //******************
476 476 // general functions
477 477
478 478 void reset_sm_status( void )
479 479 {
480 480 // error
481 481 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
482 482 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
483 483 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
484 484 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
485 485
486 486 spectral_matrix_regs->status = BITS_STATUS_REG; // [0111 1111 1111]
487 487 }
488 488
489 489 void reset_spectral_matrix_regs( void )
490 490 {
491 491 /** This function resets the spectral matrices module registers.
492 492 *
493 493 * The registers affected by this function are located at the following offset addresses:
494 494 *
495 495 * - 0x00 config
496 496 * - 0x04 status
497 497 * - 0x08 matrixF0_Address0
498 498 * - 0x10 matrixFO_Address1
499 499 * - 0x14 matrixF1_Address
500 500 * - 0x18 matrixF2_Address
501 501 *
502 502 */
503 503
504 504 set_sm_irq_onError( 0 );
505 505
506 506 set_sm_irq_onNewMatrix( 0 );
507 507
508 508 reset_sm_status();
509 509
510 510 // F1
511 511 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
512 512 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
513 513 // F2
514 514 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
515 515 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
516 516 // F3
517 517 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
518 518 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
519 519
520 520 spectral_matrix_regs->matrix_length = DEFAULT_MATRIX_LENGTH; // 25 * 128 / 16 = 200 = 0xc8
521 521 }
522 522
523 523 void set_time( unsigned char *time, unsigned char * timeInBuffer )
524 524 {
525 525 time[BYTE_0] = timeInBuffer[BYTE_0];
526 526 time[BYTE_1] = timeInBuffer[BYTE_1];
527 527 time[BYTE_2] = timeInBuffer[BYTE_2];
528 528 time[BYTE_3] = timeInBuffer[BYTE_3];
529 529 time[BYTE_4] = timeInBuffer[BYTE_6];
530 530 time[BYTE_5] = timeInBuffer[BYTE_7];
531 531 }
532 532
533 533 unsigned long long int get_acquisition_time( unsigned char *timePtr )
534 534 {
535 535 unsigned long long int acquisitionTimeAslong;
536 536 acquisitionTimeAslong = INIT_CHAR;
537 537 acquisitionTimeAslong =
538 538 ( (unsigned long long int) (timePtr[BYTE_0] & SYNC_BIT_MASK) << SHIFT_5_BYTES ) // [0111 1111] mask the synchronization bit
539 539 + ( (unsigned long long int) timePtr[BYTE_1] << SHIFT_4_BYTES )
540 540 + ( (unsigned long long int) timePtr[BYTE_2] << SHIFT_3_BYTES )
541 541 + ( (unsigned long long int) timePtr[BYTE_3] << SHIFT_2_BYTES )
542 542 + ( (unsigned long long int) timePtr[BYTE_6] << SHIFT_1_BYTE )
543 543 + ( (unsigned long long int) timePtr[BYTE_7] );
544 544 return acquisitionTimeAslong;
545 545 }
546 546
547 547 unsigned char getSID( rtems_event_set event )
548 548 {
549 549 unsigned char sid;
550 550
551 551 rtems_event_set eventSetBURST;
552 552 rtems_event_set eventSetSBM;
553 553
554 554 sid = 0;
555 555
556 556 //******
557 557 // BURST
558 558 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
559 559 | RTEMS_EVENT_BURST_BP1_F1
560 560 | RTEMS_EVENT_BURST_BP2_F0
561 561 | RTEMS_EVENT_BURST_BP2_F1;
562 562
563 563 //****
564 564 // SBM
565 565 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
566 566 | RTEMS_EVENT_SBM_BP1_F1
567 567 | RTEMS_EVENT_SBM_BP2_F0
568 568 | RTEMS_EVENT_SBM_BP2_F1;
569 569
570 570 if (event & eventSetBURST)
571 571 {
572 572 sid = SID_BURST_BP1_F0;
573 573 }
574 574 else if (event & eventSetSBM)
575 575 {
576 576 sid = SID_SBM1_BP1_F0;
577 577 }
578 578 else
579 579 {
580 580 sid = 0;
581 581 }
582 582
583 583 return sid;
584 584 }
585 585
586 586 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
587 587 {
588 588 unsigned int i;
589 589 float re;
590 590 float im;
591 591
592 592 for (i=0; i<NB_BINS_PER_SM; i++){
593 593 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) ];
594 594 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) + 1];
595 595 outputASM[ ( asmComponent *NB_BINS_PER_SM) + i] = re;
596 596 outputASM[ ((asmComponent+1)*NB_BINS_PER_SM) + i] = im;
597 597 }
598 598 }
599 599
600 600 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
601 601 {
602 602 unsigned int i;
603 603 float re;
604 604
605 605 for (i=0; i<NB_BINS_PER_SM; i++){
606 606 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
607 607 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
608 608 }
609 609 }
610 610
611 611 void ASM_patch( float *inputASM, float *outputASM )
612 612 {
613 613 extractReImVectors( inputASM, outputASM, ASM_COMP_B1B2); // b1b2
614 614 extractReImVectors( inputASM, outputASM, ASM_COMP_B1B3 ); // b1b3
615 615 extractReImVectors( inputASM, outputASM, ASM_COMP_B1E1 ); // b1e1
616 616 extractReImVectors( inputASM, outputASM, ASM_COMP_B1E2 ); // b1e2
617 617 extractReImVectors( inputASM, outputASM, ASM_COMP_B2B3 ); // b2b3
618 618 extractReImVectors( inputASM, outputASM, ASM_COMP_B2E1 ); // b2e1
619 619 extractReImVectors( inputASM, outputASM, ASM_COMP_B2E2 ); // b2e2
620 620 extractReImVectors( inputASM, outputASM, ASM_COMP_B3E1 ); // b3e1
621 621 extractReImVectors( inputASM, outputASM, ASM_COMP_B3E2 ); // b3e2
622 622 extractReImVectors( inputASM, outputASM, ASM_COMP_E1E2 ); // e1e2
623 623
624 624 copyReVectors(inputASM, outputASM, ASM_COMP_B1B1 ); // b1b1
625 625 copyReVectors(inputASM, outputASM, ASM_COMP_B2B2 ); // b2b2
626 626 copyReVectors(inputASM, outputASM, ASM_COMP_B3B3); // b3b3
627 627 copyReVectors(inputASM, outputASM, ASM_COMP_E1E1); // e1e1
628 628 copyReVectors(inputASM, outputASM, ASM_COMP_E2E2); // e2e2
629 629 }
630 630
631 631 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
632 632 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage,
633 633 unsigned char ASMIndexStart,
634 634 unsigned char channel )
635 635 {
636 636 //*************
637 637 // input format
638 638 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
639 639 //**************
640 640 // output format
641 641 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
642 642 //************
643 643 // compression
644 644 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
645 645 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
646 646
647 647 int frequencyBin;
648 648 int asmComponent;
649 649 int offsetASM;
650 650 int offsetCompressed;
651 651 int offsetFBin;
652 652 int fBinMask;
653 653 int k;
654 654
655 655 // BUILD DATA
656 656 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
657 657 {
658 658 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
659 659 {
660 660 offsetCompressed = // NO TIME OFFSET
661 661 (frequencyBin * NB_VALUES_PER_SM)
662 662 + asmComponent;
663 663 offsetASM = // NO TIME OFFSET
664 664 (asmComponent * NB_BINS_PER_SM)
665 665 + ASMIndexStart
666 666 + (frequencyBin * nbBinsToAverage);
667 667 offsetFBin = ASMIndexStart
668 668 + (frequencyBin * nbBinsToAverage);
669 669 compressed_spec_mat[ offsetCompressed ] = 0;
670 670 for ( k = 0; k < nbBinsToAverage; k++ )
671 671 {
672 672 fBinMask = getFBinMask( offsetFBin + k, channel );
673 673 compressed_spec_mat[offsetCompressed ] = compressed_spec_mat[ offsetCompressed ]
674 674 + (averaged_spec_mat[ offsetASM + k ] * fBinMask);
675 675 }
676 676 if (divider != 0)
677 677 {
678 678 compressed_spec_mat[ offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
679 679 }
680 680 else
681 681 {
682 682 compressed_spec_mat[ offsetCompressed ] = INIT_FLOAT;
683 683 }
684 684 }
685 685 }
686 686
687 687 }
688 688
689 689 int getFBinMask( int index, unsigned char channel )
690 690 {
691 691 unsigned int indexInChar;
692 692 unsigned int indexInTheChar;
693 693 int fbin;
694 694 unsigned char *sy_lfr_fbins_fx_word1;
695 695
696 696 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
697 697
698 698 switch(channel)
699 699 {
700 700 case CHANNELF0:
701 701 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0;
702 702 break;
703 703 case CHANNELF1:
704 704 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1;
705 705 break;
706 706 case CHANNELF2:
707 707 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2;
708 708 break;
709 709 default:
710 710 PRINTF("ERR *** in getFBinMask, wrong frequency channel")
711 711 }
712 712
713 713 indexInChar = index >> SHIFT_3_BITS;
714 714 indexInTheChar = index - (indexInChar * BITS_PER_BYTE);
715 715
716 716 fbin = (int) ((sy_lfr_fbins_fx_word1[ BYTES_PER_MASK - 1 - indexInChar] >> indexInTheChar) & 1);
717 717
718 718 return fbin;
719 719 }
720 720
721 721 unsigned char acquisitionTimeIsValid( unsigned int coarseTime, unsigned int fineTime, unsigned char channel)
722 722 {
723 723 u_int64_t acquisitionTimeStart;
724 724 u_int64_t acquisitionTimeStop;
725 725 u_int64_t timecodeReference;
726 726 u_int64_t offsetInFineTime;
727 727 u_int64_t shiftInFineTime;
728 728 u_int64_t tBadInFineTime;
729 729 u_int64_t acquisitionTimeRangeMin;
730 730 u_int64_t acquisitionTimeRangeMax;
731 731 unsigned char pasFilteringIsEnabled;
732 732 unsigned char ret;
733 733
734 734 pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 1); // [0000 0001]
735 735 ret = 1;
736 736
737 737 // compute acquisition time from caoarseTime and fineTime
738 738 acquisitionTimeStart = ( ((u_int64_t)coarseTime) << SHIFT_2_BYTES )
739 739 + (u_int64_t) fineTime;
740 740 switch(channel)
741 741 {
742 742 case CHANNELF0:
743 743 acquisitionTimeStop = acquisitionTimeStart + FINETIME_PER_SM_F0;
744 744 break;
745 745 case CHANNELF1:
746 746 acquisitionTimeStop = acquisitionTimeStart + FINETIME_PER_SM_F1;
747 747 break;
748 748 case CHANNELF2:
749 749 acquisitionTimeStop = acquisitionTimeStart + FINETIME_PER_SM_F2;
750 750 break;
751 751 }
752 752
753 753 // compute the timecode reference
754 754 timecodeReference = (u_int64_t) ( (floor( ((double) coarseTime) / ((double) filterPar.sy_lfr_pas_filter_modulus) )
755 755 * ((double) filterPar.sy_lfr_pas_filter_modulus)) * CONST_65536 );
756 756
757 757 // compute the acquitionTime range
758 758 offsetInFineTime = ((double) filterPar.sy_lfr_pas_filter_offset) * CONST_65536;
759 759 shiftInFineTime = ((double) filterPar.sy_lfr_pas_filter_shift) * CONST_65536;
760 760 tBadInFineTime = ((double) filterPar.sy_lfr_pas_filter_tbad) * CONST_65536;
761 761
762 762 acquisitionTimeRangeMin =
763 763 timecodeReference
764 764 + offsetInFineTime
765 765 + shiftInFineTime
766 766 - acquisitionDurations[channel];
767
767 768 acquisitionTimeRangeMax =
768 769 timecodeReference
769 770 + offsetInFineTime
770 771 + shiftInFineTime
771 772 + tBadInFineTime;
772 773
773 774 if ( (acquisitionTimeStart >= acquisitionTimeRangeMin)
774 775 && (acquisitionTimeStart <= acquisitionTimeRangeMax)
775 776 && (pasFilteringIsEnabled == 1) )
776 777 {
777 778 ret = 0; // the acquisition time is INSIDE the range, the matrix shall be ignored
778 779 }
779 780 else
780 781 {
781 782 ret = 1; // the acquisition time is OUTSIDE the range, the matrix can be used for the averaging
782 783 }
783 784
784 785 // the last sample of the data used to compute the matrix shall not be INSIDE the range, test it now, it depends on the channel
785 786 if (ret == 1)
786 787 {
787 788 if ( (acquisitionTimeStop >= acquisitionTimeRangeMin)
788 789 && (acquisitionTimeStop <= acquisitionTimeRangeMax)
789 790 && (pasFilteringIsEnabled == 1) )
790 791 {
791 792 ret = 0; // the acquisition time is INSIDE the range, the matrix shall be ignored
792 793 }
793 794 else
794 795 {
795 796 ret = 1; // the acquisition time is OUTSIDE the range, the matrix can be used for the averaging
796 797 }
797 798 }
798 799
799 800 return ret;
800 801 }
801 802
802 803 void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
803 804 {
804 805 unsigned char bin;
805 806 unsigned char kcoeff;
806 807
807 808 for (bin=0; bin<nb_bins_norm; bin++)
808 809 {
809 810 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
810 811 {
811 812 output_kcoeff[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff ) * SBM_COEFF_PER_NORM_COEFF ]
812 813 = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
813 814 output_kcoeff[ ( ( (bin * NB_K_COEFF_PER_BIN ) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ) + 1 ]
814 815 = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
815 816 }
816 817 }
817 818 }
@@ -1,1659 +1,1673
1 1 /** Functions and tasks related to TeleCommand handling.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TeleCommands:\n
7 7 * action launching\n
8 8 * TC parsing\n
9 9 * ...
10 10 *
11 11 */
12 12
13 13 #include "tc_handler.h"
14 14 #include "math.h"
15 15
16 16 //***********
17 17 // RTEMS TASK
18 18
19 19 rtems_task actn_task( rtems_task_argument unused )
20 20 {
21 21 /** This RTEMS task is responsible for launching actions upton the reception of valid TeleCommands.
22 22 *
23 23 * @param unused is the starting argument of the RTEMS task
24 24 *
25 25 * The ACTN task waits for data coming from an RTEMS msesage queue. When data arrives, it launches specific actions depending
26 26 * on the incoming TeleCommand.
27 27 *
28 28 */
29 29
30 30 int result;
31 31 rtems_status_code status; // RTEMS status code
32 32 ccsdsTelecommandPacket_t __attribute__((aligned(4))) TC; // TC sent to the ACTN task
33 33 size_t size; // size of the incoming TC packet
34 34 unsigned char subtype; // subtype of the current TC packet
35 35 unsigned char time[BYTES_PER_TIME];
36 36 rtems_id queue_rcv_id;
37 37 rtems_id queue_snd_id;
38 38
39 39 memset(&TC, 0, sizeof(ccsdsTelecommandPacket_t));
40 40 size = 0;
41 41 queue_rcv_id = RTEMS_ID_NONE;
42 42 queue_snd_id = RTEMS_ID_NONE;
43 43
44 44 status = get_message_queue_id_recv( &queue_rcv_id );
45 45 if (status != RTEMS_SUCCESSFUL)
46 46 {
47 47 PRINTF1("in ACTN *** ERR get_message_queue_id_recv %d\n", status)
48 48 }
49 49
50 50 status = get_message_queue_id_send( &queue_snd_id );
51 51 if (status != RTEMS_SUCCESSFUL)
52 52 {
53 53 PRINTF1("in ACTN *** ERR get_message_queue_id_send %d\n", status)
54 54 }
55 55
56 56 result = LFR_SUCCESSFUL;
57 57 subtype = 0; // subtype of the current TC packet
58 58
59 59 BOOT_PRINTF("in ACTN *** \n");
60 60
61 61 while(1)
62 62 {
63 63 status = rtems_message_queue_receive( queue_rcv_id, (char*) &TC, &size,
64 64 RTEMS_WAIT, RTEMS_NO_TIMEOUT);
65 65 getTime( time ); // set time to the current time
66 66 if (status!=RTEMS_SUCCESSFUL)
67 67 {
68 68 PRINTF1("ERR *** in task ACTN *** error receiving a message, code %d \n", status)
69 69 }
70 70 else
71 71 {
72 72 subtype = TC.serviceSubType;
73 73 switch(subtype)
74 74 {
75 75 case TC_SUBTYPE_RESET:
76 76 result = action_reset( &TC, queue_snd_id, time );
77 77 close_action( &TC, result, queue_snd_id );
78 78 break;
79 79 case TC_SUBTYPE_LOAD_COMM:
80 80 result = action_load_common_par( &TC );
81 81 close_action( &TC, result, queue_snd_id );
82 82 break;
83 83 case TC_SUBTYPE_LOAD_NORM:
84 84 result = action_load_normal_par( &TC, queue_snd_id, time );
85 85 close_action( &TC, result, queue_snd_id );
86 86 break;
87 87 case TC_SUBTYPE_LOAD_BURST:
88 88 result = action_load_burst_par( &TC, queue_snd_id, time );
89 89 close_action( &TC, result, queue_snd_id );
90 90 break;
91 91 case TC_SUBTYPE_LOAD_SBM1:
92 92 result = action_load_sbm1_par( &TC, queue_snd_id, time );
93 93 close_action( &TC, result, queue_snd_id );
94 94 break;
95 95 case TC_SUBTYPE_LOAD_SBM2:
96 96 result = action_load_sbm2_par( &TC, queue_snd_id, time );
97 97 close_action( &TC, result, queue_snd_id );
98 98 break;
99 99 case TC_SUBTYPE_DUMP:
100 100 result = action_dump_par( &TC, queue_snd_id );
101 101 close_action( &TC, result, queue_snd_id );
102 102 break;
103 103 case TC_SUBTYPE_ENTER:
104 104 result = action_enter_mode( &TC, queue_snd_id );
105 105 close_action( &TC, result, queue_snd_id );
106 106 break;
107 107 case TC_SUBTYPE_UPDT_INFO:
108 108 result = action_update_info( &TC, queue_snd_id );
109 109 close_action( &TC, result, queue_snd_id );
110 110 break;
111 111 case TC_SUBTYPE_EN_CAL:
112 112 result = action_enable_calibration( &TC, queue_snd_id, time );
113 113 close_action( &TC, result, queue_snd_id );
114 114 break;
115 115 case TC_SUBTYPE_DIS_CAL:
116 116 result = action_disable_calibration( &TC, queue_snd_id, time );
117 117 close_action( &TC, result, queue_snd_id );
118 118 break;
119 119 case TC_SUBTYPE_LOAD_K:
120 120 result = action_load_kcoefficients( &TC, queue_snd_id, time );
121 121 close_action( &TC, result, queue_snd_id );
122 122 break;
123 123 case TC_SUBTYPE_DUMP_K:
124 124 result = action_dump_kcoefficients( &TC, queue_snd_id, time );
125 125 close_action( &TC, result, queue_snd_id );
126 126 break;
127 127 case TC_SUBTYPE_LOAD_FBINS:
128 128 result = action_load_fbins_mask( &TC, queue_snd_id, time );
129 129 close_action( &TC, result, queue_snd_id );
130 130 break;
131 131 case TC_SUBTYPE_LOAD_FILTER_PAR:
132 132 result = action_load_filter_par( &TC, queue_snd_id, time );
133 133 close_action( &TC, result, queue_snd_id );
134 134 break;
135 135 case TC_SUBTYPE_UPDT_TIME:
136 136 result = action_update_time( &TC );
137 137 close_action( &TC, result, queue_snd_id );
138 138 break;
139 139 default:
140 140 break;
141 141 }
142 142 }
143 143 }
144 144 }
145 145
146 146 //***********
147 147 // TC ACTIONS
148 148
149 149 int action_reset(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
150 150 {
151 151 /** This function executes specific actions when a TC_LFR_RESET TeleCommand has been received.
152 152 *
153 153 * @param TC points to the TeleCommand packet that is being processed
154 154 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
155 155 *
156 156 */
157 157
158 158 PRINTF("this is the end!!!\n");
159 159 exit(0);
160 160
161 161 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
162 162
163 163 return LFR_DEFAULT;
164 164 }
165 165
166 166 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
167 167 {
168 168 /** This function executes specific actions when a TC_LFR_ENTER_MODE TeleCommand has been received.
169 169 *
170 170 * @param TC points to the TeleCommand packet that is being processed
171 171 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
172 172 *
173 173 */
174 174
175 175 rtems_status_code status;
176 176 unsigned char requestedMode;
177 177 unsigned int transitionCoarseTime;
178 178 unsigned char * bytePosPtr;
179 179
180 180 bytePosPtr = (unsigned char *) &TC->packetID;
181 181 requestedMode = bytePosPtr[ BYTE_POS_CP_MODE_LFR_SET ];
182 182 copyInt32ByChar( (char*) &transitionCoarseTime, &bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME ] );
183 183 transitionCoarseTime = transitionCoarseTime & COARSE_TIME_MASK;
184 184 status = check_mode_value( requestedMode );
185 185
186 186 if ( status != LFR_SUCCESSFUL ) // the mode value is inconsistent
187 187 {
188 188 send_tm_lfr_tc_exe_inconsistent( TC, queue_id, BYTE_POS_CP_MODE_LFR_SET, requestedMode );
189 189 }
190 190
191 191 else // the mode value is valid, check the transition
192 192 {
193 193 status = check_mode_transition(requestedMode);
194 194 if (status != LFR_SUCCESSFUL)
195 195 {
196 196 PRINTF("ERR *** in action_enter_mode *** check_mode_transition\n")
197 197 send_tm_lfr_tc_exe_not_executable( TC, queue_id );
198 198 }
199 199 }
200 200
201 201 if ( status == LFR_SUCCESSFUL ) // the transition is valid, check the date
202 202 {
203 203 status = check_transition_date( transitionCoarseTime );
204 204 if (status != LFR_SUCCESSFUL)
205 205 {
206 206 PRINTF("ERR *** in action_enter_mode *** check_transition_date\n");
207 207 send_tm_lfr_tc_exe_not_executable(TC, queue_id );
208 208 }
209 209 }
210 210
211 211 if ( status == LFR_SUCCESSFUL ) // the date is valid, enter the mode
212 212 {
213 213 PRINTF1("OK *** in action_enter_mode *** enter mode %d\n", requestedMode);
214 214
215 215 switch(requestedMode)
216 216 {
217 217 case LFR_MODE_STANDBY:
218 218 status = enter_mode_standby();
219 219 break;
220 220 case LFR_MODE_NORMAL:
221 221 status = enter_mode_normal( transitionCoarseTime );
222 222 break;
223 223 case LFR_MODE_BURST:
224 224 status = enter_mode_burst( transitionCoarseTime );
225 225 break;
226 226 case LFR_MODE_SBM1:
227 227 status = enter_mode_sbm1( transitionCoarseTime );
228 228 break;
229 229 case LFR_MODE_SBM2:
230 230 status = enter_mode_sbm2( transitionCoarseTime );
231 231 break;
232 232 default:
233 233 break;
234 234 }
235 235
236 236 if (status != RTEMS_SUCCESSFUL)
237 237 {
238 238 status = LFR_EXE_ERROR;
239 239 }
240 240 }
241 241
242 242 return status;
243 243 }
244 244
245 245 int action_update_info(ccsdsTelecommandPacket_t *TC, rtems_id queue_id)
246 246 {
247 247 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
248 248 *
249 249 * @param TC points to the TeleCommand packet that is being processed
250 250 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
251 251 *
252 252 * @return LFR directive status code:
253 253 * - LFR_DEFAULT
254 254 * - LFR_SUCCESSFUL
255 255 *
256 256 */
257 257
258 258 unsigned int val;
259 int result;
260 259 unsigned int status;
261 260 unsigned char mode;
262 261 unsigned char * bytePosPtr;
262 int pos;
263 float value;
264
265 pos = INIT_CHAR;
266 value = INIT_FLOAT;
267
268 status = LFR_DEFAULT;
263 269
264 270 bytePosPtr = (unsigned char *) &TC->packetID;
265 271
266 272 // check LFR mode
267 273 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET5 ] & BITS_LFR_MODE) >> SHIFT_LFR_MODE;
268 274 status = check_update_info_hk_lfr_mode( mode );
269 275 if (status == LFR_SUCCESSFUL) // check TDS mode
270 276 {
271 277 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & BITS_TDS_MODE) >> SHIFT_TDS_MODE;
272 278 status = check_update_info_hk_tds_mode( mode );
273 279 }
274 280 if (status == LFR_SUCCESSFUL) // check THR mode
275 281 {
276 282 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & BITS_THR_MODE);
277 283 status = check_update_info_hk_thr_mode( mode );
278 284 }
279 if (status == LFR_SUCCESSFUL) // if the parameter check is successful
285 if (status == LFR_SUCCESSFUL) // check reaction wheels frequencies
280 286 {
281 val = (housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * CONST_256)
282 + housekeeping_packet.hk_lfr_update_info_tc_cnt[1];
283 val++;
284 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
285 housekeeping_packet.hk_lfr_update_info_tc_cnt[1] = (unsigned char) (val);
287 status = check_all_sy_lfr_rw_f(TC, &pos, &value);
286 288 }
287 289
290 // if the parameters checking succeeds, udpate all parameters
291 if (status == LFR_SUCCESSFUL)
292 {
288 293 // pa_bia_status_info
289 294 // => pa_bia_mode_mux_set 3 bits
290 295 // => pa_bia_mode_hv_enabled 1 bit
291 296 // => pa_bia_mode_bias1_enabled 1 bit
292 297 // => pa_bia_mode_bias2_enabled 1 bit
293 298 // => pa_bia_mode_bias3_enabled 1 bit
294 299 // => pa_bia_on_off (cp_dpu_bias_on_off)
295 300 pa_bia_status_info = bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET2 ] & BITS_BIA; // [1111 1110]
296 301 pa_bia_status_info = pa_bia_status_info
297 302 | (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET1 ] & 1);
298 303
299 304 // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets)
300 305 getReactionWheelsFrequencies( TC );
301 306 set_hk_lfr_sc_rw_f_flags();
302 307 build_sy_lfr_rw_masks();
303 308
304 309 // once the masks are built, they have to be merged with the fbins_mask
305 310 merge_fbins_masks();
306 311
307 result = status;
312 // increase the TC_LFR_UPDATE_INFO counter
313 if (status == LFR_SUCCESSFUL) // if the parameter check is successful
314 {
315 val = (housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * CONST_256)
316 + housekeeping_packet.hk_lfr_update_info_tc_cnt[1];
317 val++;
318 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
319 housekeeping_packet.hk_lfr_update_info_tc_cnt[1] = (unsigned char) (val);
320 }
321 }
308 322
309 return result;
323 return status;
310 324 }
311 325
312 326 int action_enable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
313 327 {
314 328 /** This function executes specific actions when a TC_LFR_ENABLE_CALIBRATION TeleCommand has been received.
315 329 *
316 330 * @param TC points to the TeleCommand packet that is being processed
317 331 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
318 332 *
319 333 */
320 334
321 335 int result;
322 336
323 337 result = LFR_DEFAULT;
324 338
325 339 setCalibration( true );
326 340
327 341 result = LFR_SUCCESSFUL;
328 342
329 343 return result;
330 344 }
331 345
332 346 int action_disable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
333 347 {
334 348 /** This function executes specific actions when a TC_LFR_DISABLE_CALIBRATION TeleCommand has been received.
335 349 *
336 350 * @param TC points to the TeleCommand packet that is being processed
337 351 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
338 352 *
339 353 */
340 354
341 355 int result;
342 356
343 357 result = LFR_DEFAULT;
344 358
345 359 setCalibration( false );
346 360
347 361 result = LFR_SUCCESSFUL;
348 362
349 363 return result;
350 364 }
351 365
352 366 int action_update_time(ccsdsTelecommandPacket_t *TC)
353 367 {
354 368 /** This function executes specific actions when a TC_LFR_UPDATE_TIME TeleCommand has been received.
355 369 *
356 370 * @param TC points to the TeleCommand packet that is being processed
357 371 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
358 372 *
359 373 * @return LFR_SUCCESSFUL
360 374 *
361 375 */
362 376
363 377 unsigned int val;
364 378
365 379 time_management_regs->coarse_time_load = (TC->dataAndCRC[BYTE_0] << SHIFT_3_BYTES)
366 380 + (TC->dataAndCRC[BYTE_1] << SHIFT_2_BYTES)
367 381 + (TC->dataAndCRC[BYTE_2] << SHIFT_1_BYTE)
368 382 + TC->dataAndCRC[BYTE_3];
369 383
370 384 val = (housekeeping_packet.hk_lfr_update_time_tc_cnt[0] * CONST_256)
371 385 + housekeeping_packet.hk_lfr_update_time_tc_cnt[1];
372 386 val++;
373 387 housekeeping_packet.hk_lfr_update_time_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
374 388 housekeeping_packet.hk_lfr_update_time_tc_cnt[1] = (unsigned char) (val);
375 389
376 390 oneTcLfrUpdateTimeReceived = 1;
377 391
378 392 return LFR_SUCCESSFUL;
379 393 }
380 394
381 395 //*******************
382 396 // ENTERING THE MODES
383 397 int check_mode_value( unsigned char requestedMode )
384 398 {
385 399 int status;
386 400
387 401 status = LFR_DEFAULT;
388 402
389 403 if ( (requestedMode != LFR_MODE_STANDBY)
390 404 && (requestedMode != LFR_MODE_NORMAL) && (requestedMode != LFR_MODE_BURST)
391 405 && (requestedMode != LFR_MODE_SBM1) && (requestedMode != LFR_MODE_SBM2) )
392 406 {
393 407 status = LFR_DEFAULT;
394 408 }
395 409 else
396 410 {
397 411 status = LFR_SUCCESSFUL;
398 412 }
399 413
400 414 return status;
401 415 }
402 416
403 417 int check_mode_transition( unsigned char requestedMode )
404 418 {
405 419 /** This function checks the validity of the transition requested by the TC_LFR_ENTER_MODE.
406 420 *
407 421 * @param requestedMode is the mode requested by the TC_LFR_ENTER_MODE
408 422 *
409 423 * @return LFR directive status codes:
410 424 * - LFR_SUCCESSFUL - the transition is authorized
411 425 * - LFR_DEFAULT - the transition is not authorized
412 426 *
413 427 */
414 428
415 429 int status;
416 430
417 431 switch (requestedMode)
418 432 {
419 433 case LFR_MODE_STANDBY:
420 434 if ( lfrCurrentMode == LFR_MODE_STANDBY ) {
421 435 status = LFR_DEFAULT;
422 436 }
423 437 else
424 438 {
425 439 status = LFR_SUCCESSFUL;
426 440 }
427 441 break;
428 442 case LFR_MODE_NORMAL:
429 443 if ( lfrCurrentMode == LFR_MODE_NORMAL ) {
430 444 status = LFR_DEFAULT;
431 445 }
432 446 else {
433 447 status = LFR_SUCCESSFUL;
434 448 }
435 449 break;
436 450 case LFR_MODE_BURST:
437 451 if ( lfrCurrentMode == LFR_MODE_BURST ) {
438 452 status = LFR_DEFAULT;
439 453 }
440 454 else {
441 455 status = LFR_SUCCESSFUL;
442 456 }
443 457 break;
444 458 case LFR_MODE_SBM1:
445 459 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
446 460 status = LFR_DEFAULT;
447 461 }
448 462 else {
449 463 status = LFR_SUCCESSFUL;
450 464 }
451 465 break;
452 466 case LFR_MODE_SBM2:
453 467 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
454 468 status = LFR_DEFAULT;
455 469 }
456 470 else {
457 471 status = LFR_SUCCESSFUL;
458 472 }
459 473 break;
460 474 default:
461 475 status = LFR_DEFAULT;
462 476 break;
463 477 }
464 478
465 479 return status;
466 480 }
467 481
468 482 void update_last_valid_transition_date( unsigned int transitionCoarseTime )
469 483 {
470 484 if (transitionCoarseTime == 0)
471 485 {
472 486 lastValidEnterModeTime = time_management_regs->coarse_time + 1;
473 487 PRINTF1("lastValidEnterModeTime = 0x%x (transitionCoarseTime = 0 => coarse_time+1)\n", lastValidEnterModeTime);
474 488 }
475 489 else
476 490 {
477 491 lastValidEnterModeTime = transitionCoarseTime;
478 492 PRINTF1("lastValidEnterModeTime = 0x%x\n", transitionCoarseTime);
479 493 }
480 494 }
481 495
482 496 int check_transition_date( unsigned int transitionCoarseTime )
483 497 {
484 498 int status;
485 499 unsigned int localCoarseTime;
486 500 unsigned int deltaCoarseTime;
487 501
488 502 status = LFR_SUCCESSFUL;
489 503
490 504 if (transitionCoarseTime == 0) // transition time = 0 means an instant transition
491 505 {
492 506 status = LFR_SUCCESSFUL;
493 507 }
494 508 else
495 509 {
496 510 localCoarseTime = time_management_regs->coarse_time & COARSE_TIME_MASK;
497 511
498 512 PRINTF2("localTime = %x, transitionTime = %x\n", localCoarseTime, transitionCoarseTime);
499 513
500 514 if ( transitionCoarseTime <= localCoarseTime ) // SSS-CP-EQS-322
501 515 {
502 516 status = LFR_DEFAULT;
503 517 PRINTF("ERR *** in check_transition_date *** transitionCoarseTime <= localCoarseTime\n");
504 518 }
505 519
506 520 if (status == LFR_SUCCESSFUL)
507 521 {
508 522 deltaCoarseTime = transitionCoarseTime - localCoarseTime;
509 523 if ( deltaCoarseTime > MAX_DELTA_COARSE_TIME ) // SSS-CP-EQS-323
510 524 {
511 525 status = LFR_DEFAULT;
512 526 PRINTF1("ERR *** in check_transition_date *** deltaCoarseTime = %x\n", deltaCoarseTime)
513 527 }
514 528 }
515 529 }
516 530
517 531 return status;
518 532 }
519 533
520 534 int restart_asm_activities( unsigned char lfrRequestedMode )
521 535 {
522 536 rtems_status_code status;
523 537
524 538 status = stop_spectral_matrices();
525 539
526 540 thisIsAnASMRestart = 1;
527 541
528 542 status = restart_asm_tasks( lfrRequestedMode );
529 543
530 544 launch_spectral_matrix();
531 545
532 546 return status;
533 547 }
534 548
535 549 int stop_spectral_matrices( void )
536 550 {
537 551 /** This function stops and restarts the current mode average spectral matrices activities.
538 552 *
539 553 * @return RTEMS directive status codes:
540 554 * - RTEMS_SUCCESSFUL - task restarted successfully
541 555 * - RTEMS_INVALID_ID - task id invalid
542 556 * - RTEMS_ALREADY_SUSPENDED - task already suspended
543 557 *
544 558 */
545 559
546 560 rtems_status_code status;
547 561
548 562 status = RTEMS_SUCCESSFUL;
549 563
550 564 // (1) mask interruptions
551 565 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // mask spectral matrix interrupt
552 566
553 567 // (2) reset spectral matrices registers
554 568 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
555 569 reset_sm_status();
556 570
557 571 // (3) clear interruptions
558 572 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
559 573
560 574 // suspend several tasks
561 575 if (lfrCurrentMode != LFR_MODE_STANDBY) {
562 576 status = suspend_asm_tasks();
563 577 }
564 578
565 579 if (status != RTEMS_SUCCESSFUL)
566 580 {
567 581 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
568 582 }
569 583
570 584 return status;
571 585 }
572 586
573 587 int stop_current_mode( void )
574 588 {
575 589 /** This function stops the current mode by masking interrupt lines and suspending science tasks.
576 590 *
577 591 * @return RTEMS directive status codes:
578 592 * - RTEMS_SUCCESSFUL - task restarted successfully
579 593 * - RTEMS_INVALID_ID - task id invalid
580 594 * - RTEMS_ALREADY_SUSPENDED - task already suspended
581 595 *
582 596 */
583 597
584 598 rtems_status_code status;
585 599
586 600 status = RTEMS_SUCCESSFUL;
587 601
588 602 // (1) mask interruptions
589 603 LEON_Mask_interrupt( IRQ_WAVEFORM_PICKER ); // mask waveform picker interrupt
590 604 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
591 605
592 606 // (2) reset waveform picker registers
593 607 reset_wfp_burst_enable(); // reset burst and enable bits
594 608 reset_wfp_status(); // reset all the status bits
595 609
596 610 // (3) reset spectral matrices registers
597 611 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
598 612 reset_sm_status();
599 613
600 614 // reset lfr VHDL module
601 615 reset_lfr();
602 616
603 617 reset_extractSWF(); // reset the extractSWF flag to false
604 618
605 619 // (4) clear interruptions
606 620 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER ); // clear waveform picker interrupt
607 621 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
608 622
609 623 // suspend several tasks
610 624 if (lfrCurrentMode != LFR_MODE_STANDBY) {
611 625 status = suspend_science_tasks();
612 626 }
613 627
614 628 if (status != RTEMS_SUCCESSFUL)
615 629 {
616 630 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
617 631 }
618 632
619 633 return status;
620 634 }
621 635
622 636 int enter_mode_standby( void )
623 637 {
624 638 /** This function is used to put LFR in the STANDBY mode.
625 639 *
626 640 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
627 641 *
628 642 * @return RTEMS directive status codes:
629 643 * - RTEMS_SUCCESSFUL - task restarted successfully
630 644 * - RTEMS_INVALID_ID - task id invalid
631 645 * - RTEMS_INCORRECT_STATE - task never started
632 646 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
633 647 *
634 648 * The STANDBY mode does not depends on a specific transition date, the effect of the TC_LFR_ENTER_MODE
635 649 * is immediate.
636 650 *
637 651 */
638 652
639 653 int status;
640 654
641 655 status = stop_current_mode(); // STOP THE CURRENT MODE
642 656
643 657 #ifdef PRINT_TASK_STATISTICS
644 658 rtems_cpu_usage_report();
645 659 #endif
646 660
647 661 #ifdef PRINT_STACK_REPORT
648 662 PRINTF("stack report selected\n")
649 663 rtems_stack_checker_report_usage();
650 664 #endif
651 665
652 666 return status;
653 667 }
654 668
655 669 int enter_mode_normal( unsigned int transitionCoarseTime )
656 670 {
657 671 /** This function is used to start the NORMAL mode.
658 672 *
659 673 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
660 674 *
661 675 * @return RTEMS directive status codes:
662 676 * - RTEMS_SUCCESSFUL - task restarted successfully
663 677 * - RTEMS_INVALID_ID - task id invalid
664 678 * - RTEMS_INCORRECT_STATE - task never started
665 679 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
666 680 *
667 681 * The way the NORMAL mode is started depends on the LFR current mode. If LFR is in SBM1 or SBM2,
668 682 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected.
669 683 *
670 684 */
671 685
672 686 int status;
673 687
674 688 #ifdef PRINT_TASK_STATISTICS
675 689 rtems_cpu_usage_reset();
676 690 #endif
677 691
678 692 status = RTEMS_UNSATISFIED;
679 693
680 694 switch( lfrCurrentMode )
681 695 {
682 696 case LFR_MODE_STANDBY:
683 697 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart science tasks
684 698 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
685 699 {
686 700 launch_spectral_matrix( );
687 701 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
688 702 }
689 703 break;
690 704 case LFR_MODE_BURST:
691 705 status = stop_current_mode(); // stop the current mode
692 706 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart the science tasks
693 707 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
694 708 {
695 709 launch_spectral_matrix( );
696 710 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
697 711 }
698 712 break;
699 713 case LFR_MODE_SBM1:
700 714 status = restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
701 715 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
702 716 update_last_valid_transition_date( transitionCoarseTime );
703 717 break;
704 718 case LFR_MODE_SBM2:
705 719 status = restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
706 720 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
707 721 update_last_valid_transition_date( transitionCoarseTime );
708 722 break;
709 723 default:
710 724 break;
711 725 }
712 726
713 727 if (status != RTEMS_SUCCESSFUL)
714 728 {
715 729 PRINTF1("ERR *** in enter_mode_normal *** status = %d\n", status)
716 730 status = RTEMS_UNSATISFIED;
717 731 }
718 732
719 733 return status;
720 734 }
721 735
722 736 int enter_mode_burst( unsigned int transitionCoarseTime )
723 737 {
724 738 /** This function is used to start the BURST mode.
725 739 *
726 740 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
727 741 *
728 742 * @return RTEMS directive status codes:
729 743 * - RTEMS_SUCCESSFUL - task restarted successfully
730 744 * - RTEMS_INVALID_ID - task id invalid
731 745 * - RTEMS_INCORRECT_STATE - task never started
732 746 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
733 747 *
734 748 * The way the BURST mode is started does not depend on the LFR current mode.
735 749 *
736 750 */
737 751
738 752
739 753 int status;
740 754
741 755 #ifdef PRINT_TASK_STATISTICS
742 756 rtems_cpu_usage_reset();
743 757 #endif
744 758
745 759 status = stop_current_mode(); // stop the current mode
746 760 status = restart_science_tasks( LFR_MODE_BURST ); // restart the science tasks
747 761 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
748 762 {
749 763 launch_spectral_matrix( );
750 764 launch_waveform_picker( LFR_MODE_BURST, transitionCoarseTime );
751 765 }
752 766
753 767 if (status != RTEMS_SUCCESSFUL)
754 768 {
755 769 PRINTF1("ERR *** in enter_mode_burst *** status = %d\n", status)
756 770 status = RTEMS_UNSATISFIED;
757 771 }
758 772
759 773 return status;
760 774 }
761 775
762 776 int enter_mode_sbm1( unsigned int transitionCoarseTime )
763 777 {
764 778 /** This function is used to start the SBM1 mode.
765 779 *
766 780 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
767 781 *
768 782 * @return RTEMS directive status codes:
769 783 * - RTEMS_SUCCESSFUL - task restarted successfully
770 784 * - RTEMS_INVALID_ID - task id invalid
771 785 * - RTEMS_INCORRECT_STATE - task never started
772 786 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
773 787 *
774 788 * The way the SBM1 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM2,
775 789 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
776 790 * cases, the acquisition is completely restarted.
777 791 *
778 792 */
779 793
780 794 int status;
781 795
782 796 #ifdef PRINT_TASK_STATISTICS
783 797 rtems_cpu_usage_reset();
784 798 #endif
785 799
786 800 status = RTEMS_UNSATISFIED;
787 801
788 802 switch( lfrCurrentMode )
789 803 {
790 804 case LFR_MODE_STANDBY:
791 805 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart science tasks
792 806 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
793 807 {
794 808 launch_spectral_matrix( );
795 809 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
796 810 }
797 811 break;
798 812 case LFR_MODE_NORMAL: // lfrCurrentMode will be updated after the execution of close_action
799 813 status = restart_asm_activities( LFR_MODE_SBM1 );
800 814 status = LFR_SUCCESSFUL;
801 815 update_last_valid_transition_date( transitionCoarseTime );
802 816 break;
803 817 case LFR_MODE_BURST:
804 818 status = stop_current_mode(); // stop the current mode
805 819 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart the science tasks
806 820 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
807 821 {
808 822 launch_spectral_matrix( );
809 823 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
810 824 }
811 825 break;
812 826 case LFR_MODE_SBM2:
813 827 status = restart_asm_activities( LFR_MODE_SBM1 );
814 828 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
815 829 update_last_valid_transition_date( transitionCoarseTime );
816 830 break;
817 831 default:
818 832 break;
819 833 }
820 834
821 835 if (status != RTEMS_SUCCESSFUL)
822 836 {
823 837 PRINTF1("ERR *** in enter_mode_sbm1 *** status = %d\n", status);
824 838 status = RTEMS_UNSATISFIED;
825 839 }
826 840
827 841 return status;
828 842 }
829 843
830 844 int enter_mode_sbm2( unsigned int transitionCoarseTime )
831 845 {
832 846 /** This function is used to start the SBM2 mode.
833 847 *
834 848 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
835 849 *
836 850 * @return RTEMS directive status codes:
837 851 * - RTEMS_SUCCESSFUL - task restarted successfully
838 852 * - RTEMS_INVALID_ID - task id invalid
839 853 * - RTEMS_INCORRECT_STATE - task never started
840 854 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
841 855 *
842 856 * The way the SBM2 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM1,
843 857 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
844 858 * cases, the acquisition is completely restarted.
845 859 *
846 860 */
847 861
848 862 int status;
849 863
850 864 #ifdef PRINT_TASK_STATISTICS
851 865 rtems_cpu_usage_reset();
852 866 #endif
853 867
854 868 status = RTEMS_UNSATISFIED;
855 869
856 870 switch( lfrCurrentMode )
857 871 {
858 872 case LFR_MODE_STANDBY:
859 873 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart science tasks
860 874 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
861 875 {
862 876 launch_spectral_matrix( );
863 877 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
864 878 }
865 879 break;
866 880 case LFR_MODE_NORMAL:
867 881 status = restart_asm_activities( LFR_MODE_SBM2 );
868 882 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
869 883 update_last_valid_transition_date( transitionCoarseTime );
870 884 break;
871 885 case LFR_MODE_BURST:
872 886 status = stop_current_mode(); // stop the current mode
873 887 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart the science tasks
874 888 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
875 889 {
876 890 launch_spectral_matrix( );
877 891 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
878 892 }
879 893 break;
880 894 case LFR_MODE_SBM1:
881 895 status = restart_asm_activities( LFR_MODE_SBM2 );
882 896 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
883 897 update_last_valid_transition_date( transitionCoarseTime );
884 898 break;
885 899 default:
886 900 break;
887 901 }
888 902
889 903 if (status != RTEMS_SUCCESSFUL)
890 904 {
891 905 PRINTF1("ERR *** in enter_mode_sbm2 *** status = %d\n", status)
892 906 status = RTEMS_UNSATISFIED;
893 907 }
894 908
895 909 return status;
896 910 }
897 911
898 912 int restart_science_tasks( unsigned char lfrRequestedMode )
899 913 {
900 914 /** This function is used to restart all science tasks.
901 915 *
902 916 * @return RTEMS directive status codes:
903 917 * - RTEMS_SUCCESSFUL - task restarted successfully
904 918 * - RTEMS_INVALID_ID - task id invalid
905 919 * - RTEMS_INCORRECT_STATE - task never started
906 920 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
907 921 *
908 922 * Science tasks are AVF0, PRC0, WFRM, CWF3, CW2, CWF1
909 923 *
910 924 */
911 925
912 926 rtems_status_code status[NB_SCIENCE_TASKS];
913 927 rtems_status_code ret;
914 928
915 929 ret = RTEMS_SUCCESSFUL;
916 930
917 931 status[STATUS_0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
918 932 if (status[STATUS_0] != RTEMS_SUCCESSFUL)
919 933 {
920 934 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[STATUS_0])
921 935 }
922 936
923 937 status[STATUS_1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
924 938 if (status[STATUS_1] != RTEMS_SUCCESSFUL)
925 939 {
926 940 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[STATUS_1])
927 941 }
928 942
929 943 status[STATUS_2] = rtems_task_restart( Task_id[TASKID_WFRM],1 );
930 944 if (status[STATUS_2] != RTEMS_SUCCESSFUL)
931 945 {
932 946 PRINTF1("in restart_science_task *** WFRM ERR %d\n", status[STATUS_2])
933 947 }
934 948
935 949 status[STATUS_3] = rtems_task_restart( Task_id[TASKID_CWF3],1 );
936 950 if (status[STATUS_3] != RTEMS_SUCCESSFUL)
937 951 {
938 952 PRINTF1("in restart_science_task *** CWF3 ERR %d\n", status[STATUS_3])
939 953 }
940 954
941 955 status[STATUS_4] = rtems_task_restart( Task_id[TASKID_CWF2],1 );
942 956 if (status[STATUS_4] != RTEMS_SUCCESSFUL)
943 957 {
944 958 PRINTF1("in restart_science_task *** CWF2 ERR %d\n", status[STATUS_4])
945 959 }
946 960
947 961 status[STATUS_5] = rtems_task_restart( Task_id[TASKID_CWF1],1 );
948 962 if (status[STATUS_5] != RTEMS_SUCCESSFUL)
949 963 {
950 964 PRINTF1("in restart_science_task *** CWF1 ERR %d\n", status[STATUS_5])
951 965 }
952 966
953 967 status[STATUS_6] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
954 968 if (status[STATUS_6] != RTEMS_SUCCESSFUL)
955 969 {
956 970 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[STATUS_6])
957 971 }
958 972
959 973 status[STATUS_7] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
960 974 if (status[STATUS_7] != RTEMS_SUCCESSFUL)
961 975 {
962 976 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[STATUS_7])
963 977 }
964 978
965 979 status[STATUS_8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
966 980 if (status[STATUS_8] != RTEMS_SUCCESSFUL)
967 981 {
968 982 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[STATUS_8])
969 983 }
970 984
971 985 status[STATUS_9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
972 986 if (status[STATUS_9] != RTEMS_SUCCESSFUL)
973 987 {
974 988 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[STATUS_9])
975 989 }
976 990
977 991 if ( (status[STATUS_0] != RTEMS_SUCCESSFUL) || (status[STATUS_1] != RTEMS_SUCCESSFUL) ||
978 992 (status[STATUS_2] != RTEMS_SUCCESSFUL) || (status[STATUS_3] != RTEMS_SUCCESSFUL) ||
979 993 (status[STATUS_4] != RTEMS_SUCCESSFUL) || (status[STATUS_5] != RTEMS_SUCCESSFUL) ||
980 994 (status[STATUS_6] != RTEMS_SUCCESSFUL) || (status[STATUS_7] != RTEMS_SUCCESSFUL) ||
981 995 (status[STATUS_8] != RTEMS_SUCCESSFUL) || (status[STATUS_9] != RTEMS_SUCCESSFUL) )
982 996 {
983 997 ret = RTEMS_UNSATISFIED;
984 998 }
985 999
986 1000 return ret;
987 1001 }
988 1002
989 1003 int restart_asm_tasks( unsigned char lfrRequestedMode )
990 1004 {
991 1005 /** This function is used to restart average spectral matrices tasks.
992 1006 *
993 1007 * @return RTEMS directive status codes:
994 1008 * - RTEMS_SUCCESSFUL - task restarted successfully
995 1009 * - RTEMS_INVALID_ID - task id invalid
996 1010 * - RTEMS_INCORRECT_STATE - task never started
997 1011 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
998 1012 *
999 1013 * ASM tasks are AVF0, PRC0, AVF1, PRC1, AVF2 and PRC2
1000 1014 *
1001 1015 */
1002 1016
1003 1017 rtems_status_code status[NB_ASM_TASKS];
1004 1018 rtems_status_code ret;
1005 1019
1006 1020 ret = RTEMS_SUCCESSFUL;
1007 1021
1008 1022 status[STATUS_0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
1009 1023 if (status[STATUS_0] != RTEMS_SUCCESSFUL)
1010 1024 {
1011 1025 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[STATUS_0])
1012 1026 }
1013 1027
1014 1028 status[STATUS_1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
1015 1029 if (status[STATUS_1] != RTEMS_SUCCESSFUL)
1016 1030 {
1017 1031 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[STATUS_1])
1018 1032 }
1019 1033
1020 1034 status[STATUS_2] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
1021 1035 if (status[STATUS_2] != RTEMS_SUCCESSFUL)
1022 1036 {
1023 1037 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[STATUS_2])
1024 1038 }
1025 1039
1026 1040 status[STATUS_3] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
1027 1041 if (status[STATUS_3] != RTEMS_SUCCESSFUL)
1028 1042 {
1029 1043 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[STATUS_3])
1030 1044 }
1031 1045
1032 1046 status[STATUS_4] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
1033 1047 if (status[STATUS_4] != RTEMS_SUCCESSFUL)
1034 1048 {
1035 1049 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[STATUS_4])
1036 1050 }
1037 1051
1038 1052 status[STATUS_5] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
1039 1053 if (status[STATUS_5] != RTEMS_SUCCESSFUL)
1040 1054 {
1041 1055 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[STATUS_5])
1042 1056 }
1043 1057
1044 1058 if ( (status[STATUS_0] != RTEMS_SUCCESSFUL) || (status[STATUS_1] != RTEMS_SUCCESSFUL) ||
1045 1059 (status[STATUS_2] != RTEMS_SUCCESSFUL) || (status[STATUS_3] != RTEMS_SUCCESSFUL) ||
1046 1060 (status[STATUS_4] != RTEMS_SUCCESSFUL) || (status[STATUS_5] != RTEMS_SUCCESSFUL) )
1047 1061 {
1048 1062 ret = RTEMS_UNSATISFIED;
1049 1063 }
1050 1064
1051 1065 return ret;
1052 1066 }
1053 1067
1054 1068 int suspend_science_tasks( void )
1055 1069 {
1056 1070 /** This function suspends the science tasks.
1057 1071 *
1058 1072 * @return RTEMS directive status codes:
1059 1073 * - RTEMS_SUCCESSFUL - task restarted successfully
1060 1074 * - RTEMS_INVALID_ID - task id invalid
1061 1075 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1062 1076 *
1063 1077 */
1064 1078
1065 1079 rtems_status_code status;
1066 1080
1067 1081 PRINTF("in suspend_science_tasks\n")
1068 1082
1069 1083 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1070 1084 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1071 1085 {
1072 1086 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1073 1087 }
1074 1088 else
1075 1089 {
1076 1090 status = RTEMS_SUCCESSFUL;
1077 1091 }
1078 1092 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1079 1093 {
1080 1094 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1081 1095 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1082 1096 {
1083 1097 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1084 1098 }
1085 1099 else
1086 1100 {
1087 1101 status = RTEMS_SUCCESSFUL;
1088 1102 }
1089 1103 }
1090 1104 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1091 1105 {
1092 1106 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1093 1107 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1094 1108 {
1095 1109 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1096 1110 }
1097 1111 else
1098 1112 {
1099 1113 status = RTEMS_SUCCESSFUL;
1100 1114 }
1101 1115 }
1102 1116 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1103 1117 {
1104 1118 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1105 1119 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1106 1120 {
1107 1121 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1108 1122 }
1109 1123 else
1110 1124 {
1111 1125 status = RTEMS_SUCCESSFUL;
1112 1126 }
1113 1127 }
1114 1128 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1115 1129 {
1116 1130 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1117 1131 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1118 1132 {
1119 1133 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1120 1134 }
1121 1135 else
1122 1136 {
1123 1137 status = RTEMS_SUCCESSFUL;
1124 1138 }
1125 1139 }
1126 1140 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1127 1141 {
1128 1142 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1129 1143 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1130 1144 {
1131 1145 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1132 1146 }
1133 1147 else
1134 1148 {
1135 1149 status = RTEMS_SUCCESSFUL;
1136 1150 }
1137 1151 }
1138 1152 if (status == RTEMS_SUCCESSFUL) // suspend WFRM
1139 1153 {
1140 1154 status = rtems_task_suspend( Task_id[TASKID_WFRM] );
1141 1155 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1142 1156 {
1143 1157 PRINTF1("in suspend_science_task *** WFRM ERR %d\n", status)
1144 1158 }
1145 1159 else
1146 1160 {
1147 1161 status = RTEMS_SUCCESSFUL;
1148 1162 }
1149 1163 }
1150 1164 if (status == RTEMS_SUCCESSFUL) // suspend CWF3
1151 1165 {
1152 1166 status = rtems_task_suspend( Task_id[TASKID_CWF3] );
1153 1167 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1154 1168 {
1155 1169 PRINTF1("in suspend_science_task *** CWF3 ERR %d\n", status)
1156 1170 }
1157 1171 else
1158 1172 {
1159 1173 status = RTEMS_SUCCESSFUL;
1160 1174 }
1161 1175 }
1162 1176 if (status == RTEMS_SUCCESSFUL) // suspend CWF2
1163 1177 {
1164 1178 status = rtems_task_suspend( Task_id[TASKID_CWF2] );
1165 1179 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1166 1180 {
1167 1181 PRINTF1("in suspend_science_task *** CWF2 ERR %d\n", status)
1168 1182 }
1169 1183 else
1170 1184 {
1171 1185 status = RTEMS_SUCCESSFUL;
1172 1186 }
1173 1187 }
1174 1188 if (status == RTEMS_SUCCESSFUL) // suspend CWF1
1175 1189 {
1176 1190 status = rtems_task_suspend( Task_id[TASKID_CWF1] );
1177 1191 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1178 1192 {
1179 1193 PRINTF1("in suspend_science_task *** CWF1 ERR %d\n", status)
1180 1194 }
1181 1195 else
1182 1196 {
1183 1197 status = RTEMS_SUCCESSFUL;
1184 1198 }
1185 1199 }
1186 1200
1187 1201 return status;
1188 1202 }
1189 1203
1190 1204 int suspend_asm_tasks( void )
1191 1205 {
1192 1206 /** This function suspends the science tasks.
1193 1207 *
1194 1208 * @return RTEMS directive status codes:
1195 1209 * - RTEMS_SUCCESSFUL - task restarted successfully
1196 1210 * - RTEMS_INVALID_ID - task id invalid
1197 1211 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1198 1212 *
1199 1213 */
1200 1214
1201 1215 rtems_status_code status;
1202 1216
1203 1217 PRINTF("in suspend_science_tasks\n")
1204 1218
1205 1219 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1206 1220 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1207 1221 {
1208 1222 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1209 1223 }
1210 1224 else
1211 1225 {
1212 1226 status = RTEMS_SUCCESSFUL;
1213 1227 }
1214 1228
1215 1229 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1216 1230 {
1217 1231 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1218 1232 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1219 1233 {
1220 1234 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1221 1235 }
1222 1236 else
1223 1237 {
1224 1238 status = RTEMS_SUCCESSFUL;
1225 1239 }
1226 1240 }
1227 1241
1228 1242 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1229 1243 {
1230 1244 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1231 1245 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1232 1246 {
1233 1247 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1234 1248 }
1235 1249 else
1236 1250 {
1237 1251 status = RTEMS_SUCCESSFUL;
1238 1252 }
1239 1253 }
1240 1254
1241 1255 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1242 1256 {
1243 1257 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1244 1258 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1245 1259 {
1246 1260 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1247 1261 }
1248 1262 else
1249 1263 {
1250 1264 status = RTEMS_SUCCESSFUL;
1251 1265 }
1252 1266 }
1253 1267
1254 1268 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1255 1269 {
1256 1270 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1257 1271 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1258 1272 {
1259 1273 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1260 1274 }
1261 1275 else
1262 1276 {
1263 1277 status = RTEMS_SUCCESSFUL;
1264 1278 }
1265 1279 }
1266 1280
1267 1281 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1268 1282 {
1269 1283 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1270 1284 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1271 1285 {
1272 1286 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1273 1287 }
1274 1288 else
1275 1289 {
1276 1290 status = RTEMS_SUCCESSFUL;
1277 1291 }
1278 1292 }
1279 1293
1280 1294 return status;
1281 1295 }
1282 1296
1283 1297 void launch_waveform_picker( unsigned char mode, unsigned int transitionCoarseTime )
1284 1298 {
1285 1299
1286 1300 WFP_reset_current_ring_nodes();
1287 1301
1288 1302 reset_waveform_picker_regs();
1289 1303
1290 1304 set_wfp_burst_enable_register( mode );
1291 1305
1292 1306 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER );
1293 1307 LEON_Unmask_interrupt( IRQ_WAVEFORM_PICKER );
1294 1308
1295 1309 if (transitionCoarseTime == 0)
1296 1310 {
1297 1311 // instant transition means transition on the next valid date
1298 1312 // this is mandatory to have a good snapshot period and a good correction of the snapshot period
1299 1313 waveform_picker_regs->start_date = time_management_regs->coarse_time + 1;
1300 1314 }
1301 1315 else
1302 1316 {
1303 1317 waveform_picker_regs->start_date = transitionCoarseTime;
1304 1318 }
1305 1319
1306 1320 update_last_valid_transition_date(waveform_picker_regs->start_date);
1307 1321
1308 1322 }
1309 1323
1310 1324 void launch_spectral_matrix( void )
1311 1325 {
1312 1326 SM_reset_current_ring_nodes();
1313 1327
1314 1328 reset_spectral_matrix_regs();
1315 1329
1316 1330 reset_nb_sm();
1317 1331
1318 1332 set_sm_irq_onNewMatrix( 1 );
1319 1333
1320 1334 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX );
1321 1335 LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRIX );
1322 1336
1323 1337 }
1324 1338
1325 1339 void set_sm_irq_onNewMatrix( unsigned char value )
1326 1340 {
1327 1341 if (value == 1)
1328 1342 {
1329 1343 spectral_matrix_regs->config = spectral_matrix_regs->config | BIT_IRQ_ON_NEW_MATRIX;
1330 1344 }
1331 1345 else
1332 1346 {
1333 1347 spectral_matrix_regs->config = spectral_matrix_regs->config & MASK_IRQ_ON_NEW_MATRIX; // 1110
1334 1348 }
1335 1349 }
1336 1350
1337 1351 void set_sm_irq_onError( unsigned char value )
1338 1352 {
1339 1353 if (value == 1)
1340 1354 {
1341 1355 spectral_matrix_regs->config = spectral_matrix_regs->config | BIT_IRQ_ON_ERROR;
1342 1356 }
1343 1357 else
1344 1358 {
1345 1359 spectral_matrix_regs->config = spectral_matrix_regs->config & MASK_IRQ_ON_ERROR; // 1101
1346 1360 }
1347 1361 }
1348 1362
1349 1363 //*****************************
1350 1364 // CONFIGURE CALIBRATION SIGNAL
1351 1365 void setCalibrationPrescaler( unsigned int prescaler )
1352 1366 {
1353 1367 // prescaling of the master clock (25 MHz)
1354 1368 // master clock is divided by 2^prescaler
1355 1369 time_management_regs->calPrescaler = prescaler;
1356 1370 }
1357 1371
1358 1372 void setCalibrationDivisor( unsigned int divisionFactor )
1359 1373 {
1360 1374 // division of the prescaled clock by the division factor
1361 1375 time_management_regs->calDivisor = divisionFactor;
1362 1376 }
1363 1377
1364 1378 void setCalibrationData( void )
1365 1379 {
1366 1380 /** This function is used to store the values used to drive the DAC in order to generate the SCM calibration signal
1367 1381 *
1368 1382 * @param void
1369 1383 *
1370 1384 * @return void
1371 1385 *
1372 1386 */
1373 1387
1374 1388 unsigned int k;
1375 1389 unsigned short data;
1376 1390 float val;
1377 1391 float Ts;
1378 1392
1379 1393 time_management_regs->calDataPtr = INIT_CHAR;
1380 1394
1381 1395 Ts = 1 / CAL_FS;
1382 1396
1383 1397 // build the signal for the SCM calibration
1384 1398 for (k = 0; k < CAL_NB_PTS; k++)
1385 1399 {
1386 1400 val = CAL_A0 * sin( CAL_W0 * k * Ts )
1387 1401 + CAL_A1 * sin( CAL_W1 * k * Ts );
1388 1402 data = (unsigned short) ((val * CAL_SCALE_FACTOR) + CONST_2048);
1389 1403 time_management_regs->calData = data & CAL_DATA_MASK;
1390 1404 }
1391 1405 }
1392 1406
1393 1407 void setCalibrationDataInterleaved( void )
1394 1408 {
1395 1409 /** This function is used to store the values used to drive the DAC in order to generate the SCM calibration signal
1396 1410 *
1397 1411 * @param void
1398 1412 *
1399 1413 * @return void
1400 1414 *
1401 1415 * In interleaved mode, one can store more values than in normal mode.
1402 1416 * The data are stored in bunch of 18 bits, 12 bits from one sample and 6 bits from another sample.
1403 1417 * T store 3 values, one need two write operations.
1404 1418 * s1 [ b11 b10 b9 b8 b7 b6 ] s0 [ b11 b10 b9 b8 b7 b6 b5 b3 b2 b1 b0 ]
1405 1419 * s1 [ b5 b4 b3 b2 b1 b0 ] s2 [ b11 b10 b9 b8 b7 b6 b5 b3 b2 b1 b0 ]
1406 1420 *
1407 1421 */
1408 1422
1409 1423 unsigned int k;
1410 1424 float val;
1411 1425 float Ts;
1412 1426 unsigned short data[CAL_NB_PTS_INTER];
1413 1427 unsigned char *dataPtr;
1414 1428
1415 1429 Ts = 1 / CAL_FS_INTER;
1416 1430
1417 1431 time_management_regs->calDataPtr = INIT_CHAR;
1418 1432
1419 1433 // build the signal for the SCM calibration
1420 1434 for (k=0; k<CAL_NB_PTS_INTER; k++)
1421 1435 {
1422 1436 val = sin( 2 * pi * CAL_F0 * k * Ts )
1423 1437 + sin( 2 * pi * CAL_F1 * k * Ts );
1424 1438 data[k] = (unsigned short) ((val * CONST_512) + CONST_2048);
1425 1439 }
1426 1440
1427 1441 // write the signal in interleaved mode
1428 1442 for (k=0; k < STEPS_FOR_STORAGE_INTER; k++)
1429 1443 {
1430 1444 dataPtr = (unsigned char*) &data[ (k * BYTES_FOR_2_SAMPLES) + 2 ];
1431 1445 time_management_regs->calData = ( data[ k * BYTES_FOR_2_SAMPLES ] & CAL_DATA_MASK )
1432 1446 + ( (dataPtr[0] & CAL_DATA_MASK_INTER) << CAL_DATA_SHIFT_INTER);
1433 1447 time_management_regs->calData = ( data[(k * BYTES_FOR_2_SAMPLES) + 1] & CAL_DATA_MASK )
1434 1448 + ( (dataPtr[1] & CAL_DATA_MASK_INTER) << CAL_DATA_SHIFT_INTER);
1435 1449 }
1436 1450 }
1437 1451
1438 1452 void setCalibrationReload( bool state)
1439 1453 {
1440 1454 if (state == true)
1441 1455 {
1442 1456 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | BIT_CAL_RELOAD; // [0001 0000]
1443 1457 }
1444 1458 else
1445 1459 {
1446 1460 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & MASK_CAL_RELOAD; // [1110 1111]
1447 1461 }
1448 1462 }
1449 1463
1450 1464 void setCalibrationEnable( bool state )
1451 1465 {
1452 1466 // this bit drives the multiplexer
1453 1467 if (state == true)
1454 1468 {
1455 1469 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | BIT_CAL_ENABLE; // [0100 0000]
1456 1470 }
1457 1471 else
1458 1472 {
1459 1473 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & MASK_CAL_ENABLE; // [1011 1111]
1460 1474 }
1461 1475 }
1462 1476
1463 1477 void setCalibrationInterleaved( bool state )
1464 1478 {
1465 1479 // this bit drives the multiplexer
1466 1480 if (state == true)
1467 1481 {
1468 1482 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | BIT_SET_INTERLEAVED; // [0010 0000]
1469 1483 }
1470 1484 else
1471 1485 {
1472 1486 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & MASK_SET_INTERLEAVED; // [1101 1111]
1473 1487 }
1474 1488 }
1475 1489
1476 1490 void setCalibration( bool state )
1477 1491 {
1478 1492 if (state == true)
1479 1493 {
1480 1494 setCalibrationEnable( true );
1481 1495 setCalibrationReload( false );
1482 1496 set_hk_lfr_calib_enable( true );
1483 1497 }
1484 1498 else
1485 1499 {
1486 1500 setCalibrationEnable( false );
1487 1501 setCalibrationReload( true );
1488 1502 set_hk_lfr_calib_enable( false );
1489 1503 }
1490 1504 }
1491 1505
1492 1506 void configureCalibration( bool interleaved )
1493 1507 {
1494 1508 setCalibration( false );
1495 1509 if ( interleaved == true )
1496 1510 {
1497 1511 setCalibrationInterleaved( true );
1498 1512 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1499 1513 setCalibrationDivisor( CAL_F_DIVISOR_INTER ); // => 240 384
1500 1514 setCalibrationDataInterleaved();
1501 1515 }
1502 1516 else
1503 1517 {
1504 1518 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1505 1519 setCalibrationDivisor( CAL_F_DIVISOR ); // => 160 256 (39 - 1)
1506 1520 setCalibrationData();
1507 1521 }
1508 1522 }
1509 1523
1510 1524 //****************
1511 1525 // CLOSING ACTIONS
1512 1526 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC, unsigned char * time )
1513 1527 {
1514 1528 /** This function is used to update the HK packets statistics after a successful TC execution.
1515 1529 *
1516 1530 * @param TC points to the TC being processed
1517 1531 * @param time is the time used to date the TC execution
1518 1532 *
1519 1533 */
1520 1534
1521 1535 unsigned int val;
1522 1536
1523 1537 housekeeping_packet.hk_lfr_last_exe_tc_id[0] = TC->packetID[0];
1524 1538 housekeeping_packet.hk_lfr_last_exe_tc_id[1] = TC->packetID[1];
1525 1539 housekeeping_packet.hk_lfr_last_exe_tc_type[0] = INIT_CHAR;
1526 1540 housekeeping_packet.hk_lfr_last_exe_tc_type[1] = TC->serviceType;
1527 1541 housekeeping_packet.hk_lfr_last_exe_tc_subtype[0] = INIT_CHAR;
1528 1542 housekeeping_packet.hk_lfr_last_exe_tc_subtype[1] = TC->serviceSubType;
1529 1543 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_0] = time[BYTE_0];
1530 1544 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_1] = time[BYTE_1];
1531 1545 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_2] = time[BYTE_2];
1532 1546 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_3] = time[BYTE_3];
1533 1547 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_4] = time[BYTE_4];
1534 1548 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_5] = time[BYTE_5];
1535 1549
1536 1550 val = (housekeeping_packet.hk_lfr_exe_tc_cnt[0] * CONST_256) + housekeeping_packet.hk_lfr_exe_tc_cnt[1];
1537 1551 val++;
1538 1552 housekeeping_packet.hk_lfr_exe_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
1539 1553 housekeeping_packet.hk_lfr_exe_tc_cnt[1] = (unsigned char) (val);
1540 1554 }
1541 1555
1542 1556 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC, unsigned char * time )
1543 1557 {
1544 1558 /** This function is used to update the HK packets statistics after a TC rejection.
1545 1559 *
1546 1560 * @param TC points to the TC being processed
1547 1561 * @param time is the time used to date the TC rejection
1548 1562 *
1549 1563 */
1550 1564
1551 1565 unsigned int val;
1552 1566
1553 1567 housekeeping_packet.hk_lfr_last_rej_tc_id[0] = TC->packetID[0];
1554 1568 housekeeping_packet.hk_lfr_last_rej_tc_id[1] = TC->packetID[1];
1555 1569 housekeeping_packet.hk_lfr_last_rej_tc_type[0] = INIT_CHAR;
1556 1570 housekeeping_packet.hk_lfr_last_rej_tc_type[1] = TC->serviceType;
1557 1571 housekeeping_packet.hk_lfr_last_rej_tc_subtype[0] = INIT_CHAR;
1558 1572 housekeeping_packet.hk_lfr_last_rej_tc_subtype[1] = TC->serviceSubType;
1559 1573 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_0] = time[BYTE_0];
1560 1574 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_1] = time[BYTE_1];
1561 1575 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_2] = time[BYTE_2];
1562 1576 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_3] = time[BYTE_3];
1563 1577 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_4] = time[BYTE_4];
1564 1578 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_5] = time[BYTE_5];
1565 1579
1566 1580 val = (housekeeping_packet.hk_lfr_rej_tc_cnt[0] * CONST_256) + housekeeping_packet.hk_lfr_rej_tc_cnt[1];
1567 1581 val++;
1568 1582 housekeeping_packet.hk_lfr_rej_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
1569 1583 housekeeping_packet.hk_lfr_rej_tc_cnt[1] = (unsigned char) (val);
1570 1584 }
1571 1585
1572 1586 void close_action(ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id )
1573 1587 {
1574 1588 /** This function is the last step of the TC execution workflow.
1575 1589 *
1576 1590 * @param TC points to the TC being processed
1577 1591 * @param result is the result of the TC execution (LFR_SUCCESSFUL / LFR_DEFAULT)
1578 1592 * @param queue_id is the id of the RTEMS message queue used to send TM packets
1579 1593 * @param time is the time used to date the TC execution
1580 1594 *
1581 1595 */
1582 1596
1583 1597 unsigned char requestedMode;
1584 1598
1585 1599 if (result == LFR_SUCCESSFUL)
1586 1600 {
1587 1601 if ( !( (TC->serviceType==TC_TYPE_TIME) & (TC->serviceSubType==TC_SUBTYPE_UPDT_TIME) )
1588 1602 &
1589 1603 !( (TC->serviceType==TC_TYPE_GEN) & (TC->serviceSubType==TC_SUBTYPE_UPDT_INFO))
1590 1604 )
1591 1605 {
1592 1606 send_tm_lfr_tc_exe_success( TC, queue_id );
1593 1607 }
1594 1608 if ( (TC->serviceType == TC_TYPE_GEN) & (TC->serviceSubType == TC_SUBTYPE_ENTER) )
1595 1609 {
1596 1610 //**********************************
1597 1611 // UPDATE THE LFRMODE LOCAL VARIABLE
1598 1612 requestedMode = TC->dataAndCRC[1];
1599 1613 updateLFRCurrentMode( requestedMode );
1600 1614 }
1601 1615 }
1602 1616 else if (result == LFR_EXE_ERROR)
1603 1617 {
1604 1618 send_tm_lfr_tc_exe_error( TC, queue_id );
1605 1619 }
1606 1620 }
1607 1621
1608 1622 //***************************
1609 1623 // Interrupt Service Routines
1610 1624 rtems_isr commutation_isr1( rtems_vector_number vector )
1611 1625 {
1612 1626 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1613 1627 PRINTF("In commutation_isr1 *** Error sending event to DUMB\n")
1614 1628 }
1615 1629 }
1616 1630
1617 1631 rtems_isr commutation_isr2( rtems_vector_number vector )
1618 1632 {
1619 1633 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1620 1634 PRINTF("In commutation_isr2 *** Error sending event to DUMB\n")
1621 1635 }
1622 1636 }
1623 1637
1624 1638 //****************
1625 1639 // OTHER FUNCTIONS
1626 1640 void updateLFRCurrentMode( unsigned char requestedMode )
1627 1641 {
1628 1642 /** This function updates the value of the global variable lfrCurrentMode.
1629 1643 *
1630 1644 * lfrCurrentMode is a parameter used by several functions to know in which mode LFR is running.
1631 1645 *
1632 1646 */
1633 1647
1634 1648 // update the local value of lfrCurrentMode with the value contained in the housekeeping_packet structure
1635 1649 housekeeping_packet.lfr_status_word[0] = (housekeeping_packet.lfr_status_word[0] & STATUS_WORD_LFR_MODE_MASK)
1636 1650 + (unsigned char) ( requestedMode << STATUS_WORD_LFR_MODE_SHIFT );
1637 1651 lfrCurrentMode = requestedMode;
1638 1652 }
1639 1653
1640 1654 void set_lfr_soft_reset( unsigned char value )
1641 1655 {
1642 1656 if (value == 1)
1643 1657 {
1644 1658 time_management_regs->ctrl = time_management_regs->ctrl | BIT_SOFT_RESET; // [0100]
1645 1659 }
1646 1660 else
1647 1661 {
1648 1662 time_management_regs->ctrl = time_management_regs->ctrl & MASK_SOFT_RESET; // [1011]
1649 1663 }
1650 1664 }
1651 1665
1652 1666 void reset_lfr( void )
1653 1667 {
1654 1668 set_lfr_soft_reset( 1 );
1655 1669
1656 1670 set_lfr_soft_reset( 0 );
1657 1671
1658 1672 set_hk_lfr_sc_potential_flag( true );
1659 1673 }
@@ -1,1951 +1,2061
1 1 /** Functions to load and dump parameters in the LFR registers.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TC related to parameter loading and dumping.\n
7 7 * TC_LFR_LOAD_COMMON_PAR\n
8 8 * TC_LFR_LOAD_NORMAL_PAR\n
9 9 * TC_LFR_LOAD_BURST_PAR\n
10 10 * TC_LFR_LOAD_SBM1_PAR\n
11 11 * TC_LFR_LOAD_SBM2_PAR\n
12 12 *
13 13 */
14 14
15 15 #include "tc_load_dump_parameters.h"
16 16
17 17 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_1 = {0};
18 18 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2 = {0};
19 19 ring_node kcoefficient_node_1 = {0};
20 20 ring_node kcoefficient_node_2 = {0};
21 21
22 22 int action_load_common_par(ccsdsTelecommandPacket_t *TC)
23 23 {
24 24 /** This function updates the LFR registers with the incoming common parameters.
25 25 *
26 26 * @param TC points to the TeleCommand packet that is being processed
27 27 *
28 28 *
29 29 */
30 30
31 31 parameter_dump_packet.sy_lfr_common_parameters_spare = TC->dataAndCRC[0];
32 32 parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
33 33 set_wfp_data_shaping( );
34 34 return LFR_SUCCESSFUL;
35 35 }
36 36
37 37 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
38 38 {
39 39 /** This function updates the LFR registers with the incoming normal parameters.
40 40 *
41 41 * @param TC points to the TeleCommand packet that is being processed
42 42 * @param queue_id is the id of the queue which handles TM related to this execution step
43 43 *
44 44 */
45 45
46 46 int result;
47 47 int flag;
48 48 rtems_status_code status;
49 49
50 50 flag = LFR_SUCCESSFUL;
51 51
52 52 if ( (lfrCurrentMode == LFR_MODE_NORMAL) ||
53 53 (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) {
54 54 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
55 55 flag = LFR_DEFAULT;
56 56 }
57 57
58 58 // CHECK THE PARAMETERS SET CONSISTENCY
59 59 if (flag == LFR_SUCCESSFUL)
60 60 {
61 61 flag = check_normal_par_consistency( TC, queue_id );
62 62 }
63 63
64 64 // SET THE PARAMETERS IF THEY ARE CONSISTENT
65 65 if (flag == LFR_SUCCESSFUL)
66 66 {
67 67 result = set_sy_lfr_n_swf_l( TC );
68 68 result = set_sy_lfr_n_swf_p( TC );
69 69 result = set_sy_lfr_n_bp_p0( TC );
70 70 result = set_sy_lfr_n_bp_p1( TC );
71 71 result = set_sy_lfr_n_asm_p( TC );
72 72 result = set_sy_lfr_n_cwf_long_f3( TC );
73 73 }
74 74
75 75 return flag;
76 76 }
77 77
78 78 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
79 79 {
80 80 /** This function updates the LFR registers with the incoming burst parameters.
81 81 *
82 82 * @param TC points to the TeleCommand packet that is being processed
83 83 * @param queue_id is the id of the queue which handles TM related to this execution step
84 84 *
85 85 */
86 86
87 87 int flag;
88 88 rtems_status_code status;
89 89 unsigned char sy_lfr_b_bp_p0;
90 90 unsigned char sy_lfr_b_bp_p1;
91 91 float aux;
92 92
93 93 flag = LFR_SUCCESSFUL;
94 94
95 95 if ( lfrCurrentMode == LFR_MODE_BURST ) {
96 96 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
97 97 flag = LFR_DEFAULT;
98 98 }
99 99
100 100 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
101 101 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
102 102
103 103 // sy_lfr_b_bp_p0 shall not be lower than its default value
104 104 if (flag == LFR_SUCCESSFUL)
105 105 {
106 106 if (sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0 )
107 107 {
108 108 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0 + DATAFIELD_OFFSET, sy_lfr_b_bp_p0 );
109 109 flag = WRONG_APP_DATA;
110 110 }
111 111 }
112 112 // sy_lfr_b_bp_p1 shall not be lower than its default value
113 113 if (flag == LFR_SUCCESSFUL)
114 114 {
115 115 if (sy_lfr_b_bp_p1 < DEFAULT_SY_LFR_B_BP_P1 )
116 116 {
117 117 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P1 + DATAFIELD_OFFSET, sy_lfr_b_bp_p1 );
118 118 flag = WRONG_APP_DATA;
119 119 }
120 120 }
121 121 //****************************************************************
122 122 // check the consistency between sy_lfr_b_bp_p0 and sy_lfr_b_bp_p1
123 123 if (flag == LFR_SUCCESSFUL)
124 124 {
125 125 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
126 126 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
127 127 aux = ( (float ) sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0 ) - floor(sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0);
128 128 if (aux > FLOAT_EQUAL_ZERO)
129 129 {
130 130 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0 + DATAFIELD_OFFSET, sy_lfr_b_bp_p0 );
131 131 flag = LFR_DEFAULT;
132 132 }
133 133 }
134 134
135 135 // SET THE PARAMETERS
136 136 if (flag == LFR_SUCCESSFUL)
137 137 {
138 138 flag = set_sy_lfr_b_bp_p0( TC );
139 139 flag = set_sy_lfr_b_bp_p1( TC );
140 140 }
141 141
142 142 return flag;
143 143 }
144 144
145 145 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
146 146 {
147 147 /** This function updates the LFR registers with the incoming sbm1 parameters.
148 148 *
149 149 * @param TC points to the TeleCommand packet that is being processed
150 150 * @param queue_id is the id of the queue which handles TM related to this execution step
151 151 *
152 152 */
153 153
154 154 int flag;
155 155 rtems_status_code status;
156 156 unsigned char sy_lfr_s1_bp_p0;
157 157 unsigned char sy_lfr_s1_bp_p1;
158 158 float aux;
159 159
160 160 flag = LFR_SUCCESSFUL;
161 161
162 162 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
163 163 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
164 164 flag = LFR_DEFAULT;
165 165 }
166 166
167 167 sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
168 168 sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
169 169
170 170 // sy_lfr_s1_bp_p0
171 171 if (flag == LFR_SUCCESSFUL)
172 172 {
173 173 if (sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0 )
174 174 {
175 175 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p0 );
176 176 flag = WRONG_APP_DATA;
177 177 }
178 178 }
179 179 // sy_lfr_s1_bp_p1
180 180 if (flag == LFR_SUCCESSFUL)
181 181 {
182 182 if (sy_lfr_s1_bp_p1 < DEFAULT_SY_LFR_S1_BP_P1 )
183 183 {
184 184 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P1 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p1 );
185 185 flag = WRONG_APP_DATA;
186 186 }
187 187 }
188 188 //******************************************************************
189 189 // check the consistency between sy_lfr_s1_bp_p0 and sy_lfr_s1_bp_p1
190 190 if (flag == LFR_SUCCESSFUL)
191 191 {
192 192 aux = ( (float ) sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE) )
193 193 - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE));
194 194 if (aux > FLOAT_EQUAL_ZERO)
195 195 {
196 196 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p0 );
197 197 flag = LFR_DEFAULT;
198 198 }
199 199 }
200 200
201 201 // SET THE PARAMETERS
202 202 if (flag == LFR_SUCCESSFUL)
203 203 {
204 204 flag = set_sy_lfr_s1_bp_p0( TC );
205 205 flag = set_sy_lfr_s1_bp_p1( TC );
206 206 }
207 207
208 208 return flag;
209 209 }
210 210
211 211 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
212 212 {
213 213 /** This function updates the LFR registers with the incoming sbm2 parameters.
214 214 *
215 215 * @param TC points to the TeleCommand packet that is being processed
216 216 * @param queue_id is the id of the queue which handles TM related to this execution step
217 217 *
218 218 */
219 219
220 220 int flag;
221 221 rtems_status_code status;
222 222 unsigned char sy_lfr_s2_bp_p0;
223 223 unsigned char sy_lfr_s2_bp_p1;
224 224 float aux;
225 225
226 226 flag = LFR_SUCCESSFUL;
227 227
228 228 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
229 229 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
230 230 flag = LFR_DEFAULT;
231 231 }
232 232
233 233 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
234 234 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
235 235
236 236 // sy_lfr_s2_bp_p0
237 237 if (flag == LFR_SUCCESSFUL)
238 238 {
239 239 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
240 240 {
241 241 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p0 );
242 242 flag = WRONG_APP_DATA;
243 243 }
244 244 }
245 245 // sy_lfr_s2_bp_p1
246 246 if (flag == LFR_SUCCESSFUL)
247 247 {
248 248 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
249 249 {
250 250 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P1 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p1 );
251 251 flag = WRONG_APP_DATA;
252 252 }
253 253 }
254 254 //******************************************************************
255 255 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
256 256 if (flag == LFR_SUCCESSFUL)
257 257 {
258 258 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
259 259 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
260 260 aux = ( (float ) sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0 ) - floor(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
261 261 if (aux > FLOAT_EQUAL_ZERO)
262 262 {
263 263 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p0 );
264 264 flag = LFR_DEFAULT;
265 265 }
266 266 }
267 267
268 268 // SET THE PARAMETERS
269 269 if (flag == LFR_SUCCESSFUL)
270 270 {
271 271 flag = set_sy_lfr_s2_bp_p0( TC );
272 272 flag = set_sy_lfr_s2_bp_p1( TC );
273 273 }
274 274
275 275 return flag;
276 276 }
277 277
278 278 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
279 279 {
280 280 /** This function updates the LFR registers with the incoming sbm2 parameters.
281 281 *
282 282 * @param TC points to the TeleCommand packet that is being processed
283 283 * @param queue_id is the id of the queue which handles TM related to this execution step
284 284 *
285 285 */
286 286
287 287 int flag;
288 288
289 289 flag = LFR_DEFAULT;
290 290
291 291 flag = set_sy_lfr_kcoeff( TC, queue_id );
292 292
293 293 return flag;
294 294 }
295 295
296 296 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
297 297 {
298 298 /** This function updates the LFR registers with the incoming sbm2 parameters.
299 299 *
300 300 * @param TC points to the TeleCommand packet that is being processed
301 301 * @param queue_id is the id of the queue which handles TM related to this execution step
302 302 *
303 303 */
304 304
305 305 int flag;
306 306
307 307 flag = LFR_DEFAULT;
308 308
309 309 flag = set_sy_lfr_fbins( TC );
310 310
311 311 // once the fbins masks have been stored, they have to be merged with the masks which handle the reaction wheels frequencies filtering
312 312 merge_fbins_masks();
313 313
314 314 return flag;
315 315 }
316 316
317 317 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
318 318 {
319 319 /** This function updates the LFR registers with the incoming sbm2 parameters.
320 320 *
321 321 * @param TC points to the TeleCommand packet that is being processed
322 322 * @param queue_id is the id of the queue which handles TM related to this execution step
323 323 *
324 324 */
325 325
326 326 int flag;
327 327 unsigned char k;
328 328
329 329 flag = LFR_DEFAULT;
330 330 k = INIT_CHAR;
331 331
332 332 flag = check_sy_lfr_filter_parameters( TC, queue_id );
333 333
334 334 if (flag == LFR_SUCCESSFUL)
335 335 {
336 336 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ];
337 337 parameter_dump_packet.sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
338 338 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_0 ];
339 339 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_1 ];
340 340 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_2 ];
341 341 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_3 ];
342 342 parameter_dump_packet.sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
343 343 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_0 ];
344 344 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_1 ];
345 345 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_2 ];
346 346 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_3 ];
347 347 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_0 ];
348 348 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_1 ];
349 349 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_2 ];
350 350 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_3 ];
351 351
352 352 //****************************
353 353 // store PAS filter parameters
354 354 // sy_lfr_pas_filter_enabled
355 355 filterPar.spare_sy_lfr_pas_filter_enabled = parameter_dump_packet.spare_sy_lfr_pas_filter_enabled;
356 356 set_sy_lfr_pas_filter_enabled( parameter_dump_packet.spare_sy_lfr_pas_filter_enabled & BIT_PAS_FILTER_ENABLED );
357 357 // sy_lfr_pas_filter_modulus
358 358 filterPar.sy_lfr_pas_filter_modulus = parameter_dump_packet.sy_lfr_pas_filter_modulus;
359 359 // sy_lfr_pas_filter_tbad
360 360 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_tbad,
361 361 parameter_dump_packet.sy_lfr_pas_filter_tbad );
362 362 // sy_lfr_pas_filter_offset
363 363 filterPar.sy_lfr_pas_filter_offset = parameter_dump_packet.sy_lfr_pas_filter_offset;
364 364 // sy_lfr_pas_filter_shift
365 365 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_shift,
366 366 parameter_dump_packet.sy_lfr_pas_filter_shift );
367 367
368 368 //****************************************************
369 369 // store the parameter sy_lfr_sc_rw_delta_f as a float
370 370 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f,
371 371 parameter_dump_packet.sy_lfr_sc_rw_delta_f );
372 372
373 373 // copy rw.._k.. from the incoming TC to the local parameter_dump_packet
374 374 for (k = 0; k < NB_RW_K_COEFFS * NB_BYTES_PER_RW_K_COEFF; k++)
375 375 {
376 376 parameter_dump_packet.sy_lfr_rw1_k1[k] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_RW1_K1 + k ];
377 377 }
378 378
379 379 //***********************************************
380 380 // store the parameter sy_lfr_rw.._k.. as a float
381 381 // rw1_k
382 382 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k1, parameter_dump_packet.sy_lfr_rw1_k1 );
383 383 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k2, parameter_dump_packet.sy_lfr_rw1_k2 );
384 384 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k3, parameter_dump_packet.sy_lfr_rw1_k3 );
385 385 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k4, parameter_dump_packet.sy_lfr_rw1_k4 );
386 386 // rw2_k
387 387 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k1, parameter_dump_packet.sy_lfr_rw2_k1 );
388 388 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k2, parameter_dump_packet.sy_lfr_rw2_k2 );
389 389 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k3, parameter_dump_packet.sy_lfr_rw2_k3 );
390 390 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k4, parameter_dump_packet.sy_lfr_rw2_k4 );
391 391 // rw3_k
392 392 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k1, parameter_dump_packet.sy_lfr_rw3_k1 );
393 393 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k2, parameter_dump_packet.sy_lfr_rw3_k2 );
394 394 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k3, parameter_dump_packet.sy_lfr_rw3_k3 );
395 395 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k4, parameter_dump_packet.sy_lfr_rw3_k4 );
396 396 // rw4_k
397 397 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k1, parameter_dump_packet.sy_lfr_rw4_k1 );
398 398 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k2, parameter_dump_packet.sy_lfr_rw4_k2 );
399 399 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k3, parameter_dump_packet.sy_lfr_rw4_k3 );
400 400 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k4, parameter_dump_packet.sy_lfr_rw4_k4 );
401 401
402 402 }
403 403
404 404 return flag;
405 405 }
406 406
407 407 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
408 408 {
409 409 /** This function updates the LFR registers with the incoming sbm2 parameters.
410 410 *
411 411 * @param TC points to the TeleCommand packet that is being processed
412 412 * @param queue_id is the id of the queue which handles TM related to this execution step
413 413 *
414 414 */
415 415
416 416 unsigned int address;
417 417 rtems_status_code status;
418 418 unsigned int freq;
419 419 unsigned int bin;
420 420 unsigned int coeff;
421 421 unsigned char *kCoeffPtr;
422 422 unsigned char *kCoeffDumpPtr;
423 423
424 424 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
425 425 // F0 => 11 bins
426 426 // F1 => 13 bins
427 427 // F2 => 12 bins
428 428 // 36 bins to dump in two packets (30 bins max per packet)
429 429
430 430 //*********
431 431 // PACKET 1
432 432 // 11 F0 bins, 13 F1 bins and 6 F2 bins
433 433 kcoefficients_dump_1.destinationID = TC->sourceID;
434 434 increment_seq_counter_destination_id_dump( kcoefficients_dump_1.packetSequenceControl, TC->sourceID );
435 435 for( freq = 0;
436 436 freq < NB_BINS_COMPRESSED_SM_F0;
437 437 freq++ )
438 438 {
439 439 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1] = freq;
440 440 bin = freq;
441 441 // printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
442 442 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
443 443 {
444 444 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
445 445 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
446 446 ]; // 2 for the kcoeff_frequency
447 447 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
448 448 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
449 449 }
450 450 }
451 451 for( freq = NB_BINS_COMPRESSED_SM_F0;
452 452 freq < ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
453 453 freq++ )
454 454 {
455 455 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = freq;
456 456 bin = freq - NB_BINS_COMPRESSED_SM_F0;
457 457 // printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
458 458 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
459 459 {
460 460 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
461 461 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
462 462 ]; // 2 for the kcoeff_frequency
463 463 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
464 464 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
465 465 }
466 466 }
467 467 for( freq = ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
468 468 freq < KCOEFF_BLK_NR_PKT1 ;
469 469 freq++ )
470 470 {
471 471 kcoefficients_dump_1.kcoeff_blks[ (freq * KCOEFF_BLK_SIZE) + 1 ] = freq;
472 472 bin = freq - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
473 473 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
474 474 for ( coeff = 0; coeff <NB_K_COEFF_PER_BIN; coeff++ )
475 475 {
476 476 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
477 477 (freq * KCOEFF_BLK_SIZE) + (coeff * NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
478 478 ]; // 2 for the kcoeff_frequency
479 479 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
480 480 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
481 481 }
482 482 }
483 483 kcoefficients_dump_1.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
484 484 kcoefficients_dump_1.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
485 485 kcoefficients_dump_1.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
486 486 kcoefficients_dump_1.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
487 487 kcoefficients_dump_1.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
488 488 kcoefficients_dump_1.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
489 489 // SEND DATA
490 490 kcoefficient_node_1.status = 1;
491 491 address = (unsigned int) &kcoefficient_node_1;
492 492 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
493 493 if (status != RTEMS_SUCCESSFUL) {
494 494 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
495 495 }
496 496
497 497 //********
498 498 // PACKET 2
499 499 // 6 F2 bins
500 500 kcoefficients_dump_2.destinationID = TC->sourceID;
501 501 increment_seq_counter_destination_id_dump( kcoefficients_dump_2.packetSequenceControl, TC->sourceID );
502 502 for( freq = 0;
503 503 freq < KCOEFF_BLK_NR_PKT2;
504 504 freq++ )
505 505 {
506 506 kcoefficients_dump_2.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = KCOEFF_BLK_NR_PKT1 + freq;
507 507 bin = freq + KCOEFF_BLK_NR_PKT2;
508 508 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
509 509 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
510 510 {
511 511 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[
512 512 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ ]; // 2 for the kcoeff_frequency
513 513 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
514 514 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
515 515 }
516 516 }
517 517 kcoefficients_dump_2.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
518 518 kcoefficients_dump_2.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
519 519 kcoefficients_dump_2.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
520 520 kcoefficients_dump_2.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
521 521 kcoefficients_dump_2.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
522 522 kcoefficients_dump_2.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
523 523 // SEND DATA
524 524 kcoefficient_node_2.status = 1;
525 525 address = (unsigned int) &kcoefficient_node_2;
526 526 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
527 527 if (status != RTEMS_SUCCESSFUL) {
528 528 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
529 529 }
530 530
531 531 return status;
532 532 }
533 533
534 534 int action_dump_par( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
535 535 {
536 536 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
537 537 *
538 538 * @param queue_id is the id of the queue which handles TM related to this execution step.
539 539 *
540 540 * @return RTEMS directive status codes:
541 541 * - RTEMS_SUCCESSFUL - message sent successfully
542 542 * - RTEMS_INVALID_ID - invalid queue id
543 543 * - RTEMS_INVALID_SIZE - invalid message size
544 544 * - RTEMS_INVALID_ADDRESS - buffer is NULL
545 545 * - RTEMS_UNSATISFIED - out of message buffers
546 546 * - RTEMS_TOO_MANY - queue s limit has been reached
547 547 *
548 548 */
549 549
550 550 int status;
551 551
552 552 increment_seq_counter_destination_id_dump( parameter_dump_packet.packetSequenceControl, TC->sourceID );
553 553 parameter_dump_packet.destinationID = TC->sourceID;
554 554
555 555 // UPDATE TIME
556 556 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
557 557 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
558 558 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
559 559 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
560 560 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
561 561 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
562 562 // SEND DATA
563 563 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
564 564 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
565 565 if (status != RTEMS_SUCCESSFUL) {
566 566 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
567 567 }
568 568
569 569 return status;
570 570 }
571 571
572 572 //***********************
573 573 // NORMAL MODE PARAMETERS
574 574
575 575 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
576 576 {
577 577 unsigned char msb;
578 578 unsigned char lsb;
579 579 int flag;
580 580 float aux;
581 581 rtems_status_code status;
582 582
583 583 unsigned int sy_lfr_n_swf_l;
584 584 unsigned int sy_lfr_n_swf_p;
585 585 unsigned int sy_lfr_n_asm_p;
586 586 unsigned char sy_lfr_n_bp_p0;
587 587 unsigned char sy_lfr_n_bp_p1;
588 588 unsigned char sy_lfr_n_cwf_long_f3;
589 589
590 590 flag = LFR_SUCCESSFUL;
591 591
592 592 //***************
593 593 // get parameters
594 594 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
595 595 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
596 596 sy_lfr_n_swf_l = (msb * CONST_256) + lsb;
597 597
598 598 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
599 599 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
600 600 sy_lfr_n_swf_p = (msb * CONST_256) + lsb;
601 601
602 602 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
603 603 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
604 604 sy_lfr_n_asm_p = (msb * CONST_256) + lsb;
605 605
606 606 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
607 607
608 608 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
609 609
610 610 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
611 611
612 612 //******************
613 613 // check consistency
614 614 // sy_lfr_n_swf_l
615 615 if (sy_lfr_n_swf_l != DFLT_SY_LFR_N_SWF_L)
616 616 {
617 617 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L + DATAFIELD_OFFSET, sy_lfr_n_swf_l );
618 618 flag = WRONG_APP_DATA;
619 619 }
620 620 // sy_lfr_n_swf_p
621 621 if (flag == LFR_SUCCESSFUL)
622 622 {
623 623 if ( sy_lfr_n_swf_p < MIN_SY_LFR_N_SWF_P )
624 624 {
625 625 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P + DATAFIELD_OFFSET, sy_lfr_n_swf_p );
626 626 flag = WRONG_APP_DATA;
627 627 }
628 628 }
629 629 // sy_lfr_n_bp_p0
630 630 if (flag == LFR_SUCCESSFUL)
631 631 {
632 632 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
633 633 {
634 634 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0 + DATAFIELD_OFFSET, sy_lfr_n_bp_p0 );
635 635 flag = WRONG_APP_DATA;
636 636 }
637 637 }
638 638 // sy_lfr_n_asm_p
639 639 if (flag == LFR_SUCCESSFUL)
640 640 {
641 641 if (sy_lfr_n_asm_p == 0)
642 642 {
643 643 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
644 644 flag = WRONG_APP_DATA;
645 645 }
646 646 }
647 647 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
648 648 if (flag == LFR_SUCCESSFUL)
649 649 {
650 650 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
651 651 if (aux > FLOAT_EQUAL_ZERO)
652 652 {
653 653 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
654 654 flag = WRONG_APP_DATA;
655 655 }
656 656 }
657 657 // sy_lfr_n_bp_p1
658 658 if (flag == LFR_SUCCESSFUL)
659 659 {
660 660 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
661 661 {
662 662 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
663 663 flag = WRONG_APP_DATA;
664 664 }
665 665 }
666 666 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
667 667 if (flag == LFR_SUCCESSFUL)
668 668 {
669 669 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
670 670 if (aux > FLOAT_EQUAL_ZERO)
671 671 {
672 672 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
673 673 flag = LFR_DEFAULT;
674 674 }
675 675 }
676 676 // sy_lfr_n_cwf_long_f3
677 677
678 678 return flag;
679 679 }
680 680
681 681 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
682 682 {
683 683 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
684 684 *
685 685 * @param TC points to the TeleCommand packet that is being processed
686 686 * @param queue_id is the id of the queue which handles TM related to this execution step
687 687 *
688 688 */
689 689
690 690 int result;
691 691
692 692 result = LFR_SUCCESSFUL;
693 693
694 694 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
695 695 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
696 696
697 697 return result;
698 698 }
699 699
700 700 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
701 701 {
702 702 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
703 703 *
704 704 * @param TC points to the TeleCommand packet that is being processed
705 705 * @param queue_id is the id of the queue which handles TM related to this execution step
706 706 *
707 707 */
708 708
709 709 int result;
710 710
711 711 result = LFR_SUCCESSFUL;
712 712
713 713 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
714 714 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
715 715
716 716 return result;
717 717 }
718 718
719 719 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
720 720 {
721 721 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
722 722 *
723 723 * @param TC points to the TeleCommand packet that is being processed
724 724 * @param queue_id is the id of the queue which handles TM related to this execution step
725 725 *
726 726 */
727 727
728 728 int result;
729 729
730 730 result = LFR_SUCCESSFUL;
731 731
732 732 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
733 733 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
734 734
735 735 return result;
736 736 }
737 737
738 738 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
739 739 {
740 740 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
741 741 *
742 742 * @param TC points to the TeleCommand packet that is being processed
743 743 * @param queue_id is the id of the queue which handles TM related to this execution step
744 744 *
745 745 */
746 746
747 747 int status;
748 748
749 749 status = LFR_SUCCESSFUL;
750 750
751 751 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
752 752
753 753 return status;
754 754 }
755 755
756 756 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
757 757 {
758 758 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
759 759 *
760 760 * @param TC points to the TeleCommand packet that is being processed
761 761 * @param queue_id is the id of the queue which handles TM related to this execution step
762 762 *
763 763 */
764 764
765 765 int status;
766 766
767 767 status = LFR_SUCCESSFUL;
768 768
769 769 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
770 770
771 771 return status;
772 772 }
773 773
774 774 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
775 775 {
776 776 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
777 777 *
778 778 * @param TC points to the TeleCommand packet that is being processed
779 779 * @param queue_id is the id of the queue which handles TM related to this execution step
780 780 *
781 781 */
782 782
783 783 int status;
784 784
785 785 status = LFR_SUCCESSFUL;
786 786
787 787 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
788 788
789 789 return status;
790 790 }
791 791
792 792 //**********************
793 793 // BURST MODE PARAMETERS
794
794 795 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
795 796 {
796 797 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
797 798 *
798 799 * @param TC points to the TeleCommand packet that is being processed
799 800 * @param queue_id is the id of the queue which handles TM related to this execution step
800 801 *
801 802 */
802 803
803 804 int status;
804 805
805 806 status = LFR_SUCCESSFUL;
806 807
807 808 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
808 809
809 810 return status;
810 811 }
811 812
812 813 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
813 814 {
814 815 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
815 816 *
816 817 * @param TC points to the TeleCommand packet that is being processed
817 818 * @param queue_id is the id of the queue which handles TM related to this execution step
818 819 *
819 820 */
820 821
821 822 int status;
822 823
823 824 status = LFR_SUCCESSFUL;
824 825
825 826 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
826 827
827 828 return status;
828 829 }
829 830
830 831 //*********************
831 832 // SBM1 MODE PARAMETERS
833
832 834 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
833 835 {
834 836 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
835 837 *
836 838 * @param TC points to the TeleCommand packet that is being processed
837 839 * @param queue_id is the id of the queue which handles TM related to this execution step
838 840 *
839 841 */
840 842
841 843 int status;
842 844
843 845 status = LFR_SUCCESSFUL;
844 846
845 847 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
846 848
847 849 return status;
848 850 }
849 851
850 852 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
851 853 {
852 854 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
853 855 *
854 856 * @param TC points to the TeleCommand packet that is being processed
855 857 * @param queue_id is the id of the queue which handles TM related to this execution step
856 858 *
857 859 */
858 860
859 861 int status;
860 862
861 863 status = LFR_SUCCESSFUL;
862 864
863 865 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
864 866
865 867 return status;
866 868 }
867 869
868 870 //*********************
869 871 // SBM2 MODE PARAMETERS
872
870 873 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC )
871 874 {
872 875 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
873 876 *
874 877 * @param TC points to the TeleCommand packet that is being processed
875 878 * @param queue_id is the id of the queue which handles TM related to this execution step
876 879 *
877 880 */
878 881
879 882 int status;
880 883
881 884 status = LFR_SUCCESSFUL;
882 885
883 886 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
884 887
885 888 return status;
886 889 }
887 890
888 891 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
889 892 {
890 893 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
891 894 *
892 895 * @param TC points to the TeleCommand packet that is being processed
893 896 * @param queue_id is the id of the queue which handles TM related to this execution step
894 897 *
895 898 */
896 899
897 900 int status;
898 901
899 902 status = LFR_SUCCESSFUL;
900 903
901 904 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
902 905
903 906 return status;
904 907 }
905 908
906 909 //*******************
907 910 // TC_LFR_UPDATE_INFO
911
908 912 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
909 913 {
910 914 unsigned int status;
911 915
912 916 status = LFR_DEFAULT;
913 917
914 918 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
915 919 || (mode == LFR_MODE_BURST)
916 920 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
917 921 {
918 922 status = LFR_SUCCESSFUL;
919 923 }
920 924 else
921 925 {
922 926 status = LFR_DEFAULT;
923 927 }
924 928
925 929 return status;
926 930 }
927 931
928 932 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
929 933 {
930 934 unsigned int status;
931 935
932 936 status = LFR_DEFAULT;
933 937
934 938 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
935 939 || (mode == TDS_MODE_BURST)
936 940 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
937 941 || (mode == TDS_MODE_LFM))
938 942 {
939 943 status = LFR_SUCCESSFUL;
940 944 }
941 945 else
942 946 {
943 947 status = LFR_DEFAULT;
944 948 }
945 949
946 950 return status;
947 951 }
948 952
949 953 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
950 954 {
951 955 unsigned int status;
952 956
953 957 status = LFR_DEFAULT;
954 958
955 959 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
956 960 || (mode == THR_MODE_BURST))
957 961 {
958 962 status = LFR_SUCCESSFUL;
959 963 }
960 964 else
961 965 {
962 966 status = LFR_DEFAULT;
963 967 }
964 968
965 969 return status;
966 970 }
967 971
968 972 void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value )
969 973 {
970 974 unsigned char flag;
971 975 unsigned char flagPosInByte;
972 976 unsigned char newFlag;
973 977 unsigned char flagMask;
974 978
975 979 // if the frequency value is not a number, the flag is set to 0 and the frequency RWx_Fy is not filtered
976 980 if (isnan(value))
977 981 {
978 982 flag = FLAG_NAN;
979 983 }
980 984 else
981 985 {
982 986 flag = FLAG_IAN;
983 987 }
984 988
985 989 switch(wheel)
986 990 {
987 991 case WHEEL_1:
988 992 flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
989 993 flagMask = ~(1 << flagPosInByte);
990 994 newFlag = flag << flagPosInByte;
991 995 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
992 996 break;
993 997 case WHEEL_2:
994 998 flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
995 999 flagMask = ~(1 << flagPosInByte);
996 1000 newFlag = flag << flagPosInByte;
997 1001 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
998 1002 break;
999 1003 case WHEEL_3:
1000 1004 flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
1001 1005 flagMask = ~(1 << flagPosInByte);
1002 1006 newFlag = flag << flagPosInByte;
1003 1007 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1004 1008 break;
1005 1009 case WHEEL_4:
1006 1010 flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
1007 1011 flagMask = ~(1 << flagPosInByte);
1008 1012 newFlag = flag << flagPosInByte;
1009 1013 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1010 1014 break;
1011 1015 default:
1012 1016 break;
1013 1017 }
1014 1018 }
1015 1019
1016 1020 void set_hk_lfr_sc_rw_f_flags( void )
1017 1021 {
1018 1022 // RW1
1019 1023 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_1, rw_f.cp_rpw_sc_rw1_f1 );
1020 1024 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_2, rw_f.cp_rpw_sc_rw1_f2 );
1021 1025 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_3, rw_f.cp_rpw_sc_rw1_f3 );
1022 1026 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_4, rw_f.cp_rpw_sc_rw1_f4 );
1023 1027
1024 1028 // RW2
1025 1029 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_1, rw_f.cp_rpw_sc_rw2_f1 );
1026 1030 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_2, rw_f.cp_rpw_sc_rw2_f2 );
1027 1031 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_3, rw_f.cp_rpw_sc_rw2_f3 );
1028 1032 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_4, rw_f.cp_rpw_sc_rw2_f4 );
1029 1033
1030 1034 // RW3
1031 1035 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_1, rw_f.cp_rpw_sc_rw3_f1 );
1032 1036 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_2, rw_f.cp_rpw_sc_rw3_f2 );
1033 1037 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_3, rw_f.cp_rpw_sc_rw3_f3 );
1034 1038 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_4, rw_f.cp_rpw_sc_rw3_f4 );
1035 1039
1036 1040 // RW4
1037 1041 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_1, rw_f.cp_rpw_sc_rw4_f1 );
1038 1042 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_2, rw_f.cp_rpw_sc_rw4_f2 );
1039 1043 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_3, rw_f.cp_rpw_sc_rw4_f3 );
1040 1044 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_4, rw_f.cp_rpw_sc_rw4_f4 );
1041 1045 }
1042 1046
1047 int check_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value )
1048 {
1049 float rw_k;
1050 int ret;
1051
1052 ret = LFR_SUCCESSFUL;
1053 rw_k = INIT_FLOAT;
1054
1055 copyFloatByChar( (unsigned char*) &rw_k, (unsigned char*) &TC->packetID[ offset ] );
1056
1057 *pos = offset;
1058 *value = rw_k;
1059
1060 if (rw_k < MIN_SY_LFR_RW_F)
1061 {
1062 ret = WRONG_APP_DATA;
1063 }
1064
1065 return ret;
1066 }
1067
1068 int check_all_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int *pos, float*value )
1069 {
1070 int ret;
1071
1072 ret = LFR_SUCCESSFUL;
1073
1074 //****
1075 //****
1076 // RW1
1077 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1, pos, value ); // F1
1078 if (ret == LFR_SUCCESSFUL) // F2
1079 {
1080 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2, pos, value );
1081 }
1082 if (ret == LFR_SUCCESSFUL) // F3
1083 {
1084 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3, pos, value );
1085 }
1086 if (ret == LFR_SUCCESSFUL) // F4
1087 {
1088 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4, pos, value );
1089 }
1090
1091 //****
1092 //****
1093 // RW2
1094 if (ret == LFR_SUCCESSFUL) // F1
1095 {
1096 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1, pos, value );
1097 }
1098 if (ret == LFR_SUCCESSFUL) // F2
1099 {
1100 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2, pos, value );
1101 }
1102 if (ret == LFR_SUCCESSFUL) // F3
1103 {
1104 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3, pos, value );
1105 }
1106 if (ret == LFR_SUCCESSFUL) // F4
1107 {
1108 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4, pos, value );
1109 }
1110
1111 //****
1112 //****
1113 // RW3
1114 if (ret == LFR_SUCCESSFUL) // F1
1115 {
1116 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1, pos, value );
1117 }
1118 if (ret == LFR_SUCCESSFUL) // F2
1119 {
1120 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2, pos, value );
1121 }
1122 if (ret == LFR_SUCCESSFUL) // F3
1123 {
1124 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3, pos, value );
1125 }
1126 if (ret == LFR_SUCCESSFUL) // F4
1127 {
1128 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4, pos, value );
1129 }
1130
1131 //****
1132 //****
1133 // RW4
1134 if (ret == LFR_SUCCESSFUL) // F1
1135 {
1136 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1, pos, value );
1137 }
1138 if (ret == LFR_SUCCESSFUL) // F2
1139 {
1140 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2, pos, value );
1141 }
1142 if (ret == LFR_SUCCESSFUL) // F3
1143 {
1144 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3, pos, value );
1145 }
1146 if (ret == LFR_SUCCESSFUL) // F4
1147 {
1148 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4, pos, value );
1149 }
1150
1151 return ret;
1152 }
1153
1043 1154 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC )
1044 1155 {
1045 1156 /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally.
1046 1157 *
1047 1158 * @param TC points to the TeleCommand packet that is being processed
1048 1159 *
1049 1160 */
1050 1161
1051 1162 unsigned char * bytePosPtr; // pointer to the beginning of the incoming TC packet
1052 1163
1053 1164 bytePosPtr = (unsigned char *) &TC->packetID;
1054 1165
1055 1166 // rw1_f
1056 1167 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] );
1057 1168 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] );
1058 1169 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3 ] );
1059 1170 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4 ] );
1060 1171
1061 1172 // rw2_f
1062 1173 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] );
1063 1174 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] );
1064 1175 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3 ] );
1065 1176 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4 ] );
1066 1177
1067 1178 // rw3_f
1068 1179 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] );
1069 1180 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] );
1070 1181 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3 ] );
1071 1182 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4 ] );
1072 1183
1073 1184 // rw4_f
1074 1185 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] );
1075 1186 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] );
1076 1187 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3 ] );
1077 1188 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4 ] );
1078 1189
1079 1190 // test each reaction wheel frequency value. NaN means that the frequency is not filtered
1080 1191
1081 1192 }
1082 1193
1083 1194 void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, float sy_lfr_rw_k )
1084 1195 {
1085 1196 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
1086 1197 *
1087 1198 * @param fbins_mask
1088 1199 * @param rw_f is the reaction wheel frequency to filter
1089 1200 * @param delta_f is the frequency step between the frequency bins, it depends on the frequency channel
1090 1201 * @param flag [true] filtering enabled [false] filtering disabled
1091 1202 *
1092 1203 * @return void
1093 1204 *
1094 1205 */
1095 1206
1096 1207 float f_RW_min;
1097 1208 float f_RW_MAX;
1098 1209 float fi_min;
1099 1210 float fi_MAX;
1100 1211 float fi;
1101 1212 float deltaBelow;
1102 1213 float deltaAbove;
1103 1214 float freqToFilterOut;
1104 1215 int binBelow;
1105 1216 int binAbove;
1106 1217 int closestBin;
1107 1218 unsigned int whichByte;
1108 1219 int selectedByte;
1109 1220 int bin;
1110 1221 int binToRemove[NB_BINS_TO_REMOVE];
1111 1222 int k;
1112 1223 bool filteringSet;
1113 1224
1114 1225 closestBin = 0;
1115 1226 whichByte = 0;
1116 1227 bin = 0;
1117 1228 filteringSet = false;
1118 1229
1119 1230 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1120 1231 {
1121 1232 binToRemove[k] = -1;
1122 1233 }
1123 1234
1124 1235 if (!isnan(rw_f))
1125 1236 {
1126 1237 // compute the frequency range to filter [ rw_f - delta_f; rw_f + delta_f ]
1127 1238 f_RW_min = rw_f - ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
1128 1239 f_RW_MAX = rw_f + ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
1129 1240
1130 1241 freqToFilterOut = f_RW_min;
1131 1242 while ( filteringSet == false )
1132 1243 {
1133 1244 // compute the index of the frequency bin immediately below rw_f
1134 1245 binBelow = (int) ( floor( ((double) freqToFilterOut) / ((double) deltaFreq)) );
1135 1246 deltaBelow = freqToFilterOut - binBelow * deltaFreq;
1136 1247
1137 1248 // compute the index of the frequency bin immediately above rw_f
1138 1249 binAbove = (int) ( ceil( ((double) freqToFilterOut) / ((double) deltaFreq)) );
1139 1250 deltaAbove = binAbove * deltaFreq - freqToFilterOut;
1140 1251
1141 1252 // search the closest bin
1142 1253 if (deltaAbove > deltaBelow)
1143 1254 {
1144 1255 closestBin = binBelow;
1145 1256 }
1146 1257 else
1147 1258 {
1148 1259 closestBin = binAbove;
1149 1260 }
1150 1261
1151 1262 // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285]
1152 1263 fi = closestBin * deltaFreq;
1153 1264 fi_min = fi - (deltaFreq * FI_INTERVAL_COEFF);
1154 1265 fi_MAX = fi + (deltaFreq * FI_INTERVAL_COEFF);
1155 1266
1156 1267 //**************************************************************************************
1157 1268 // be careful here, one shall take into account that the bin 0 IS DROPPED in the spectra
1158 1269 // thus, the index 0 in a mask corresponds to the bin 1 of the spectrum
1159 1270 //**************************************************************************************
1160 1271
1161 1272 // 1. IF freqToFilterOut is included in [ fi_min; fi_MAX ]
1162 1273 // => remove f_(i), f_(i-1) and f_(i+1)
1163 1274 if ( ( freqToFilterOut > fi_min ) && ( freqToFilterOut < fi_MAX ) )
1164 1275 {
1165 1276 binToRemove[0] = (closestBin - 1) - 1;
1166 1277 binToRemove[1] = (closestBin) - 1;
1167 1278 binToRemove[2] = (closestBin + 1) - 1;
1168 1279 }
1169 1280 // 2. ELSE
1170 1281 // => remove the two f_(i) which are around f_RW
1171 1282 else
1172 1283 {
1173 1284 binToRemove[0] = (binBelow) - 1;
1174 1285 binToRemove[1] = (binAbove) - 1;
1175 1286 binToRemove[2] = (-1);
1176 1287 }
1177 1288
1178 1289 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1179 1290 {
1180 1291 bin = binToRemove[k];
1181 1292 if ( (bin >= BIN_MIN) && (bin <= BIN_MAX) )
1182 1293 {
1183 1294 whichByte = (bin >> SHIFT_3_BITS); // division by 8
1184 1295 selectedByte = ( 1 << (bin - (whichByte * BITS_PER_BYTE)) );
1185 1296 fbins_mask[BYTES_PER_MASK - 1 - whichByte] =
1186 1297 fbins_mask[BYTES_PER_MASK - 1 - whichByte] & ((unsigned char) (~selectedByte)); // bytes are ordered MSB first in the packets
1187 1298
1188 1299 }
1189 1300 }
1190 1301
1191 1302 // update freqToFilterOut
1192 1303 if ( freqToFilterOut == f_RW_MAX )
1193 1304 {
1194 1305 filteringSet = true; // end of the loop
1195 1306 }
1196 1307 else
1197 1308 {
1198 1309 freqToFilterOut = freqToFilterOut + deltaFreq;
1199 1310 }
1200 1311
1201 1312 if ( freqToFilterOut > f_RW_MAX)
1202 1313 {
1203 1314 freqToFilterOut = f_RW_MAX;
1204 1315 }
1205 1316 }
1206 1317 }
1207 1318 }
1208 1319
1209 1320 void build_sy_lfr_rw_mask( unsigned int channel )
1210 1321 {
1211 1322 unsigned char local_rw_fbins_mask[BYTES_PER_MASK];
1212 1323 unsigned char *maskPtr;
1213 1324 double deltaF;
1214 1325 unsigned k;
1215 1326
1216 1327 maskPtr = NULL;
1217 1328 deltaF = DELTAF_F2;
1218 1329
1219 1330 switch (channel)
1220 1331 {
1221 1332 case CHANNELF0:
1222 1333 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1223 1334 deltaF = DELTAF_F0;
1224 1335 break;
1225 1336 case CHANNELF1:
1226 1337 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1227 1338 deltaF = DELTAF_F1;
1228 1339 break;
1229 1340 case CHANNELF2:
1230 1341 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1231 1342 deltaF = DELTAF_F2;
1232 1343 break;
1233 1344 default:
1234 1345 break;
1235 1346 }
1236 1347
1237 1348 for (k = 0; k < BYTES_PER_MASK; k++)
1238 1349 {
1239 1350 local_rw_fbins_mask[k] = INT8_ALL_F;
1240 1351 }
1241 1352
1242 1353 // RW1
1243 1354 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, filterPar.sy_lfr_rw1_k1 );
1244 1355 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, filterPar.sy_lfr_rw1_k2 );
1245 1356 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f3, deltaF, filterPar.sy_lfr_rw1_k3 );
1246 1357 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f4, deltaF, filterPar.sy_lfr_rw1_k4 );
1247 1358
1248 1359 // RW2
1249 1360 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, filterPar.sy_lfr_rw2_k1 );
1250 1361 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, filterPar.sy_lfr_rw2_k2 );
1251 1362 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f3, deltaF, filterPar.sy_lfr_rw2_k3 );
1252 1363 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f4, deltaF, filterPar.sy_lfr_rw2_k4 );
1253 1364
1254 1365 // RW3
1255 1366 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, filterPar.sy_lfr_rw3_k1 );
1256 1367 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, filterPar.sy_lfr_rw3_k2 );
1257 1368 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f3, deltaF, filterPar.sy_lfr_rw3_k3 );
1258 1369 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f4, deltaF, filterPar.sy_lfr_rw3_k4 );
1259 1370
1260 1371 // RW4
1261 1372 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, filterPar.sy_lfr_rw4_k1 );
1262 1373 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, filterPar.sy_lfr_rw4_k2 );
1263 1374 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f3, deltaF, filterPar.sy_lfr_rw4_k3 );
1264 1375 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f4, deltaF, filterPar.sy_lfr_rw4_k4 );
1265 1376
1266 1377 // update the value of the fbins related to reaction wheels frequency filtering
1267 1378 if (maskPtr != NULL)
1268 1379 {
1269 1380 for (k = 0; k < BYTES_PER_MASK; k++)
1270 1381 {
1271 1382 maskPtr[k] = local_rw_fbins_mask[k];
1272 1383 }
1273 1384 }
1274 1385 }
1275 1386
1276 1387 void build_sy_lfr_rw_masks( void )
1277 1388 {
1278 1389 build_sy_lfr_rw_mask( CHANNELF0 );
1279 1390 build_sy_lfr_rw_mask( CHANNELF1 );
1280 1391 build_sy_lfr_rw_mask( CHANNELF2 );
1281 1392 }
1282 1393
1283 1394 void merge_fbins_masks( void )
1284 1395 {
1285 1396 unsigned char k;
1286 1397
1287 1398 unsigned char *fbins_f0;
1288 1399 unsigned char *fbins_f1;
1289 1400 unsigned char *fbins_f2;
1290 1401 unsigned char *rw_mask_f0;
1291 1402 unsigned char *rw_mask_f1;
1292 1403 unsigned char *rw_mask_f2;
1293 1404
1294 1405 fbins_f0 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1295 1406 fbins_f1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
1296 1407 fbins_f2 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
1297 1408 rw_mask_f0 = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1298 1409 rw_mask_f1 = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1299 1410 rw_mask_f2 = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1300 1411
1301 1412 for( k=0; k < BYTES_PER_MASK; k++ )
1302 1413 {
1303 1414 fbins_masks.merged_fbins_mask_f0[k] = fbins_f0[k] & rw_mask_f0[k];
1304 1415 fbins_masks.merged_fbins_mask_f1[k] = fbins_f1[k] & rw_mask_f1[k];
1305 1416 fbins_masks.merged_fbins_mask_f2[k] = fbins_f2[k] & rw_mask_f2[k];
1306 1417 }
1307 1418 }
1308 1419
1309 1420 //***********
1310 1421 // FBINS MASK
1311 1422
1312 1423 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
1313 1424 {
1314 1425 int status;
1315 1426 unsigned int k;
1316 1427 unsigned char *fbins_mask_dump;
1317 1428 unsigned char *fbins_mask_TC;
1318 1429
1319 1430 status = LFR_SUCCESSFUL;
1320 1431
1321 1432 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1322 1433 fbins_mask_TC = TC->dataAndCRC;
1323 1434
1324 1435 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1325 1436 {
1326 1437 fbins_mask_dump[k] = fbins_mask_TC[k];
1327 1438 }
1328 1439
1329 1440 return status;
1330 1441 }
1331 1442
1332 1443 //***************************
1333 1444 // TC_LFR_LOAD_PAS_FILTER_PAR
1334 1445
1335 1446 int check_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value )
1336 1447 {
1337 1448 float rw_k;
1338 1449 int ret;
1339 1450
1340 1451 ret = LFR_SUCCESSFUL;
1341 1452 rw_k = INIT_FLOAT;
1342 1453
1343 1454 copyFloatByChar( (unsigned char*) &rw_k, (unsigned char*) &TC->dataAndCRC[ offset ] );
1344 1455
1345 1456 *pos = offset;
1346 1457 *value = rw_k;
1347 1458
1348 if (rw_k < MIN_SY_LFR_RW_K)
1459 if (rw_k < MIN_SY_LFR_RW_F)
1349 1460 {
1350 1461 ret = WRONG_APP_DATA;
1351 1462 }
1352 1463
1353 1464 return ret;
1354 1465 }
1355 1466
1356 1467 int check_all_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int *pos, float*value )
1357 1468 {
1358 1469 int ret;
1359 1470
1360 1471 ret = LFR_SUCCESSFUL;
1361 1472
1362 1473 //****
1363 1474 //****
1364 1475 // RW1
1365 1476 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K1, pos, value ); // K1
1366 1477 if (ret == LFR_SUCCESSFUL) // K2
1367 1478 {
1368 1479 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K2, pos, value );
1369 1480 }
1370 1481 if (ret == LFR_SUCCESSFUL) // K3
1371 1482 {
1372 1483 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K3, pos, value );
1373 1484 }
1374 1485 if (ret == LFR_SUCCESSFUL) // K4
1375 1486 {
1376 1487 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K4, pos, value );
1377 1488 }
1378 1489
1379 1490 //****
1380 1491 //****
1381 1492 // RW2
1382 1493 if (ret == LFR_SUCCESSFUL) // K1
1383 1494 {
1384 1495 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K1, pos, value );
1385 1496 }
1386 1497 if (ret == LFR_SUCCESSFUL) // K2
1387 1498 {
1388 1499 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K2, pos, value );
1389 1500 }
1390 1501 if (ret == LFR_SUCCESSFUL) // K3
1391 1502 {
1392 1503 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K3, pos, value );
1393 1504 }
1394 1505 if (ret == LFR_SUCCESSFUL) // K4
1395 1506 {
1396 1507 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K4, pos, value );
1397 1508 }
1398 1509
1399 1510 //****
1400 1511 //****
1401 1512 // RW3
1402 1513 if (ret == LFR_SUCCESSFUL) // K1
1403 1514 {
1404 1515 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K1, pos, value );
1405 1516 }
1406 1517 if (ret == LFR_SUCCESSFUL) // K2
1407 1518 {
1408 1519 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K2, pos, value );
1409 1520 }
1410 1521 if (ret == LFR_SUCCESSFUL) // K3
1411 1522 {
1412 1523 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K3, pos, value );
1413 1524 }
1414 1525 if (ret == LFR_SUCCESSFUL) // K4
1415 1526 {
1416 1527 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K4, pos, value );
1417 1528 }
1418 1529
1419 1530 //****
1420 1531 //****
1421 1532 // RW4
1422 1533 if (ret == LFR_SUCCESSFUL) // K1
1423 1534 {
1424 1535 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K1, pos, value );
1425 1536 }
1426 1537 if (ret == LFR_SUCCESSFUL) // K2
1427 1538 {
1428 1539 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K2, pos, value );
1429 1540 }
1430 1541 if (ret == LFR_SUCCESSFUL) // K3
1431 1542 {
1432 1543 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K3, pos, value );
1433 1544 }
1434 1545 if (ret == LFR_SUCCESSFUL) // K4
1435 1546 {
1436 1547 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K4, pos, value );
1437 1548 }
1438 1549
1439
1440
1441 1550 return ret;
1442 1551 }
1443 1552
1444 1553 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
1445 1554 {
1446 1555 int flag;
1447 1556 rtems_status_code status;
1448 1557
1449 1558 unsigned char sy_lfr_pas_filter_enabled;
1450 1559 unsigned char sy_lfr_pas_filter_modulus;
1451 1560 float sy_lfr_pas_filter_tbad;
1452 1561 unsigned char sy_lfr_pas_filter_offset;
1453 1562 float sy_lfr_pas_filter_shift;
1454 1563 float sy_lfr_sc_rw_delta_f;
1455 1564 char *parPtr;
1456 int *datafield_pos;
1457 float *rw_k;
1565 int datafield_pos;
1566 float rw_k;
1458 1567
1459 1568 flag = LFR_SUCCESSFUL;
1460 1569 sy_lfr_pas_filter_tbad = INIT_FLOAT;
1461 1570 sy_lfr_pas_filter_shift = INIT_FLOAT;
1462 1571 sy_lfr_sc_rw_delta_f = INIT_FLOAT;
1463 1572 parPtr = NULL;
1464 datafield_pos = NULL;
1465 rw_k = NULL;
1466
1467 *datafield_pos = LFR_DEFAULT_ALT;
1468 *rw_k = INIT_FLOAT;
1573 datafield_pos = INIT_INT;
1574 rw_k = INIT_FLOAT;
1469 1575
1470 1576 //***************
1471 1577 // get parameters
1472 1578 sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & BIT_PAS_FILTER_ENABLED; // [0000 0001]
1473 1579 sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
1474 1580 copyFloatByChar(
1475 1581 (unsigned char*) &sy_lfr_pas_filter_tbad,
1476 1582 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD ]
1477 1583 );
1478 1584 sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
1479 1585 copyFloatByChar(
1480 1586 (unsigned char*) &sy_lfr_pas_filter_shift,
1481 1587 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT ]
1482 1588 );
1483 1589 copyFloatByChar(
1484 1590 (unsigned char*) &sy_lfr_sc_rw_delta_f,
1485 1591 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F ]
1486 1592 );
1487 1593
1488 1594 //******************
1489 1595 // CHECK CONSISTENCY
1490 1596
1491 1597 //**************************
1492 1598 // sy_lfr_pas_filter_enabled
1493 1599 // nothing to check, value is 0 or 1
1494 1600
1495 1601 //**************************
1496 1602 // sy_lfr_pas_filter_modulus
1497 1603 if ( (sy_lfr_pas_filter_modulus < MIN_PAS_FILTER_MODULUS) || (sy_lfr_pas_filter_modulus > MAX_PAS_FILTER_MODULUS) )
1498 1604 {
1499 1605 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1500 1606 flag = WRONG_APP_DATA;
1501 1607 }
1502 1608
1503 1609 //***********************
1504 1610 // sy_lfr_pas_filter_tbad
1611 if (flag == LFR_SUCCESSFUL)
1612 {
1505 1613 if ( (sy_lfr_pas_filter_tbad < MIN_PAS_FILTER_TBAD) || (sy_lfr_pas_filter_tbad > MAX_PAS_FILTER_TBAD) )
1506 1614 {
1507 1615 parPtr = (char*) &sy_lfr_pas_filter_tbad;
1508 1616 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1509 1617 flag = WRONG_APP_DATA;
1510 1618 }
1619 }
1511 1620
1512 1621 //*************************
1513 1622 // sy_lfr_pas_filter_offset
1514 1623 if (flag == LFR_SUCCESSFUL)
1515 1624 {
1516 1625 if ( (sy_lfr_pas_filter_offset < MIN_PAS_FILTER_OFFSET) || (sy_lfr_pas_filter_offset > MAX_PAS_FILTER_OFFSET) )
1517 1626 {
1518 1627 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET + DATAFIELD_OFFSET, sy_lfr_pas_filter_offset );
1519 1628 flag = WRONG_APP_DATA;
1520 1629 }
1521 1630 }
1522 1631
1523 1632 //************************
1524 1633 // sy_lfr_pas_filter_shift
1525 1634 if (flag == LFR_SUCCESSFUL)
1526 1635 {
1527 1636 if ( (sy_lfr_pas_filter_shift < MIN_PAS_FILTER_SHIFT) || (sy_lfr_pas_filter_shift > MAX_PAS_FILTER_SHIFT) )
1528 1637 {
1529 1638 parPtr = (char*) &sy_lfr_pas_filter_shift;
1530 1639 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1531 1640 flag = WRONG_APP_DATA;
1532 1641 }
1533 1642 }
1534 1643
1535 1644 //*************************************
1536 1645 // check global coherency of the values
1537 1646 if (flag == LFR_SUCCESSFUL)
1538 1647 {
1539 1648 if ( (sy_lfr_pas_filter_tbad + sy_lfr_pas_filter_offset + sy_lfr_pas_filter_shift) > sy_lfr_pas_filter_modulus )
1540 1649 {
1541 1650 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1542 1651 flag = WRONG_APP_DATA;
1543 1652 }
1544 1653 }
1545 1654
1546 1655 //*********************
1547 1656 // sy_lfr_sc_rw_delta_f
1548 1657 if (flag == LFR_SUCCESSFUL)
1549 1658 {
1550 1659 if ( sy_lfr_sc_rw_delta_f < MIN_SY_LFR_SC_RW_DELTA_F )
1551 1660 {
1661 parPtr = (char*) &sy_lfr_pas_filter_shift;
1552 1662 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + DATAFIELD_OFFSET, sy_lfr_sc_rw_delta_f );
1553 1663 flag = WRONG_APP_DATA;
1554 1664 }
1555 1665 }
1556 1666
1557 1667 //************
1558 1668 // sy_lfr_rw_k
1559 1669 if (flag == LFR_SUCCESSFUL)
1560 1670 {
1561 flag = check_all_sy_lfr_rw_k( TC, datafield_pos, rw_k );
1671 flag = check_all_sy_lfr_rw_k( TC, &datafield_pos, &rw_k );
1562 1672 if (flag != LFR_SUCCESSFUL)
1563 1673 {
1564 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, *datafield_pos + DATAFIELD_OFFSET, *rw_k );
1674 parPtr = (char*) &sy_lfr_pas_filter_shift;
1675 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, datafield_pos + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1565 1676 }
1566 1677 }
1567 1678
1568
1569 1679 return flag;
1570 1680 }
1571 1681
1572 1682 //**************
1573 1683 // KCOEFFICIENTS
1574 1684 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC,rtems_id queue_id )
1575 1685 {
1576 1686 unsigned int kcoeff;
1577 1687 unsigned short sy_lfr_kcoeff_frequency;
1578 1688 unsigned short bin;
1579 1689 float *kcoeffPtr_norm;
1580 1690 float *kcoeffPtr_sbm;
1581 1691 int status;
1582 1692 unsigned char *kcoeffLoadPtr;
1583 1693 unsigned char *kcoeffNormPtr;
1584 1694 unsigned char *kcoeffSbmPtr_a;
1585 1695 unsigned char *kcoeffSbmPtr_b;
1586 1696
1587 1697 sy_lfr_kcoeff_frequency = 0;
1588 1698 bin = 0;
1589 1699 kcoeffPtr_norm = NULL;
1590 1700 kcoeffPtr_sbm = NULL;
1591 1701 status = LFR_SUCCESSFUL;
1592 1702
1593 1703 // copy the value of the frequency byte by byte DO NOT USE A SHORT* POINTER
1594 1704 copyInt16ByChar( (unsigned char*) &sy_lfr_kcoeff_frequency, &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY] );
1595 1705
1596 1706
1597 1707 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
1598 1708 {
1599 1709 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
1600 1710 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + DATAFIELD_OFFSET + 1,
1601 1711 TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 1] ); // +1 to get the LSB instead of the MSB
1602 1712 status = LFR_DEFAULT;
1603 1713 }
1604 1714 else
1605 1715 {
1606 1716 if ( ( sy_lfr_kcoeff_frequency >= 0 )
1607 1717 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
1608 1718 {
1609 1719 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
1610 1720 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
1611 1721 bin = sy_lfr_kcoeff_frequency;
1612 1722 }
1613 1723 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
1614 1724 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
1615 1725 {
1616 1726 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
1617 1727 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
1618 1728 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
1619 1729 }
1620 1730 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
1621 1731 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
1622 1732 {
1623 1733 kcoeffPtr_norm = k_coeff_intercalib_f2;
1624 1734 kcoeffPtr_sbm = NULL;
1625 1735 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
1626 1736 }
1627 1737 }
1628 1738
1629 1739 if (kcoeffPtr_norm != NULL ) // update K coefficient for NORMAL data products
1630 1740 {
1631 1741 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1632 1742 {
1633 1743 // destination
1634 1744 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + kcoeff ];
1635 1745 // source
1636 1746 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1637 1747 // copy source to destination
1638 1748 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
1639 1749 }
1640 1750 }
1641 1751
1642 1752 if (kcoeffPtr_sbm != NULL ) // update K coefficient for SBM data products
1643 1753 {
1644 1754 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1645 1755 {
1646 1756 // destination
1647 1757 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ];
1648 1758 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ (((bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_KCOEFF_PER_NORM_KCOEFF) + 1 ];
1649 1759 // source
1650 1760 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1651 1761 // copy source to destination
1652 1762 copyFloatByChar( kcoeffSbmPtr_a, kcoeffLoadPtr );
1653 1763 copyFloatByChar( kcoeffSbmPtr_b, kcoeffLoadPtr );
1654 1764 }
1655 1765 }
1656 1766
1657 1767 // print_k_coeff();
1658 1768
1659 1769 return status;
1660 1770 }
1661 1771
1662 1772 void copyFloatByChar( unsigned char *destination, unsigned char *source )
1663 1773 {
1664 1774 destination[BYTE_0] = source[BYTE_0];
1665 1775 destination[BYTE_1] = source[BYTE_1];
1666 1776 destination[BYTE_2] = source[BYTE_2];
1667 1777 destination[BYTE_3] = source[BYTE_3];
1668 1778 }
1669 1779
1670 1780 void copyInt32ByChar( unsigned char *destination, unsigned char *source )
1671 1781 {
1672 1782 destination[BYTE_0] = source[BYTE_0];
1673 1783 destination[BYTE_1] = source[BYTE_1];
1674 1784 destination[BYTE_2] = source[BYTE_2];
1675 1785 destination[BYTE_3] = source[BYTE_3];
1676 1786 }
1677 1787
1678 1788 void copyInt16ByChar( unsigned char *destination, unsigned char *source )
1679 1789 {
1680 1790 destination[BYTE_0] = source[BYTE_0];
1681 1791 destination[BYTE_1] = source[BYTE_1];
1682 1792 }
1683 1793
1684 1794 void floatToChar( float value, unsigned char* ptr)
1685 1795 {
1686 1796 unsigned char* valuePtr;
1687 1797
1688 1798 valuePtr = (unsigned char*) &value;
1689 1799
1690 1800 ptr[BYTE_0] = valuePtr[BYTE_0];
1691 1801 ptr[BYTE_1] = valuePtr[BYTE_1];
1692 1802 ptr[BYTE_2] = valuePtr[BYTE_2];
1693 1803 ptr[BYTE_3] = valuePtr[BYTE_3];
1694 1804 }
1695 1805
1696 1806 //**********
1697 1807 // init dump
1698 1808
1699 1809 void init_parameter_dump( void )
1700 1810 {
1701 1811 /** This function initialize the parameter_dump_packet global variable with default values.
1702 1812 *
1703 1813 */
1704 1814
1705 1815 unsigned int k;
1706 1816
1707 1817 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
1708 1818 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
1709 1819 parameter_dump_packet.reserved = CCSDS_RESERVED;
1710 1820 parameter_dump_packet.userApplication = CCSDS_USER_APP;
1711 1821 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1712 1822 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1713 1823 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1714 1824 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1715 1825 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> SHIFT_1_BYTE);
1716 1826 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
1717 1827 // DATA FIELD HEADER
1718 1828 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1719 1829 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
1720 1830 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
1721 1831 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
1722 1832 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
1723 1833 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
1724 1834 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
1725 1835 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
1726 1836 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
1727 1837 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
1728 1838 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
1729 1839
1730 1840 //******************
1731 1841 // COMMON PARAMETERS
1732 1842 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
1733 1843 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
1734 1844
1735 1845 //******************
1736 1846 // NORMAL PARAMETERS
1737 1847 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> SHIFT_1_BYTE);
1738 1848 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1739 1849 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> SHIFT_1_BYTE);
1740 1850 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1741 1851 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> SHIFT_1_BYTE);
1742 1852 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1743 1853 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1744 1854 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1745 1855 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1746 1856
1747 1857 //*****************
1748 1858 // BURST PARAMETERS
1749 1859 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1750 1860 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1751 1861
1752 1862 //****************
1753 1863 // SBM1 PARAMETERS
1754 1864 parameter_dump_packet.sy_lfr_s1_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P0; // min value is 0.25 s for the period
1755 1865 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1756 1866
1757 1867 //****************
1758 1868 // SBM2 PARAMETERS
1759 1869 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1760 1870 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1761 1871
1762 1872 //************
1763 1873 // FBINS MASKS
1764 1874 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1765 1875 {
1766 1876 parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = INT8_ALL_F;
1767 1877 }
1768 1878
1769 1879 // PAS FILTER PARAMETERS
1770 1880 parameter_dump_packet.pa_rpw_spare8_2 = INIT_CHAR;
1771 1881 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = INIT_CHAR;
1772 1882 parameter_dump_packet.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
1773 1883 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_TBAD, parameter_dump_packet.sy_lfr_pas_filter_tbad );
1774 1884 parameter_dump_packet.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
1775 1885 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift );
1776 1886 floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f );
1777 1887
1778 1888 // RW1_K
1779 1889 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw1_k1);
1780 1890 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw1_k2);
1781 1891 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw1_k3);
1782 1892 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw1_k4);
1783 1893 // RW2_K
1784 1894 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw2_k1);
1785 1895 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw2_k2);
1786 1896 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw2_k3);
1787 1897 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw2_k4);
1788 1898 // RW3_K
1789 1899 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw3_k1);
1790 1900 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw3_k2);
1791 1901 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw3_k3);
1792 1902 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw3_k4);
1793 1903 // RW4_K
1794 1904 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw4_k1);
1795 1905 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw4_k2);
1796 1906 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw4_k3);
1797 1907 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw4_k4);
1798 1908
1799 1909 // LFR_RW_MASK
1800 1910 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1801 1911 {
1802 1912 parameter_dump_packet.sy_lfr_rw_mask_f0_word1[k] = INT8_ALL_F;
1803 1913 }
1804 1914
1805 1915 // once the reaction wheels masks have been initialized, they have to be merged with the fbins masks
1806 1916 merge_fbins_masks();
1807 1917 }
1808 1918
1809 1919 void init_kcoefficients_dump( void )
1810 1920 {
1811 1921 init_kcoefficients_dump_packet( &kcoefficients_dump_1, PKTNR_1, KCOEFF_BLK_NR_PKT1 );
1812 1922 init_kcoefficients_dump_packet( &kcoefficients_dump_2, PKTNR_2, KCOEFF_BLK_NR_PKT2 );
1813 1923
1814 1924 kcoefficient_node_1.previous = NULL;
1815 1925 kcoefficient_node_1.next = NULL;
1816 1926 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1817 1927 kcoefficient_node_1.coarseTime = INIT_CHAR;
1818 1928 kcoefficient_node_1.fineTime = INIT_CHAR;
1819 1929 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1820 1930 kcoefficient_node_1.status = INIT_CHAR;
1821 1931
1822 1932 kcoefficient_node_2.previous = NULL;
1823 1933 kcoefficient_node_2.next = NULL;
1824 1934 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1825 1935 kcoefficient_node_2.coarseTime = INIT_CHAR;
1826 1936 kcoefficient_node_2.fineTime = INIT_CHAR;
1827 1937 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1828 1938 kcoefficient_node_2.status = INIT_CHAR;
1829 1939 }
1830 1940
1831 1941 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1832 1942 {
1833 1943 unsigned int k;
1834 1944 unsigned int packetLength;
1835 1945
1836 1946 packetLength =
1837 1947 ((blk_nr * KCOEFF_BLK_SIZE) + BYTE_POS_KCOEFFICIENTS_PARAMETES) - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1838 1948
1839 1949 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1840 1950 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1841 1951 kcoefficients_dump->reserved = CCSDS_RESERVED;
1842 1952 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1843 1953 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1844 1954 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1845 1955 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1846 1956 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1847 1957 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
1848 1958 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1849 1959 // DATA FIELD HEADER
1850 1960 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1851 1961 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1852 1962 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1853 1963 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1854 1964 kcoefficients_dump->time[BYTE_0] = INIT_CHAR;
1855 1965 kcoefficients_dump->time[BYTE_1] = INIT_CHAR;
1856 1966 kcoefficients_dump->time[BYTE_2] = INIT_CHAR;
1857 1967 kcoefficients_dump->time[BYTE_3] = INIT_CHAR;
1858 1968 kcoefficients_dump->time[BYTE_4] = INIT_CHAR;
1859 1969 kcoefficients_dump->time[BYTE_5] = INIT_CHAR;
1860 1970 kcoefficients_dump->sid = SID_K_DUMP;
1861 1971
1862 1972 kcoefficients_dump->pkt_cnt = KCOEFF_PKTCNT;
1863 1973 kcoefficients_dump->pkt_nr = PKTNR_1;
1864 1974 kcoefficients_dump->blk_nr = blk_nr;
1865 1975
1866 1976 //******************
1867 1977 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1868 1978 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1869 1979 for (k=0; k<(KCOEFF_BLK_NR_PKT1 * KCOEFF_BLK_SIZE); k++)
1870 1980 {
1871 1981 kcoefficients_dump->kcoeff_blks[k] = INIT_CHAR;
1872 1982 }
1873 1983 }
1874 1984
1875 1985 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id )
1876 1986 {
1877 1987 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
1878 1988 *
1879 1989 * @param packet_sequence_control points to the packet sequence control which will be incremented
1880 1990 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
1881 1991 *
1882 1992 * If the destination ID is not known, a dedicated counter is incremented.
1883 1993 *
1884 1994 */
1885 1995
1886 1996 unsigned short sequence_cnt;
1887 1997 unsigned short segmentation_grouping_flag;
1888 1998 unsigned short new_packet_sequence_control;
1889 1999 unsigned char i;
1890 2000
1891 2001 switch (destination_id)
1892 2002 {
1893 2003 case SID_TC_GROUND:
1894 2004 i = GROUND;
1895 2005 break;
1896 2006 case SID_TC_MISSION_TIMELINE:
1897 2007 i = MISSION_TIMELINE;
1898 2008 break;
1899 2009 case SID_TC_TC_SEQUENCES:
1900 2010 i = TC_SEQUENCES;
1901 2011 break;
1902 2012 case SID_TC_RECOVERY_ACTION_CMD:
1903 2013 i = RECOVERY_ACTION_CMD;
1904 2014 break;
1905 2015 case SID_TC_BACKUP_MISSION_TIMELINE:
1906 2016 i = BACKUP_MISSION_TIMELINE;
1907 2017 break;
1908 2018 case SID_TC_DIRECT_CMD:
1909 2019 i = DIRECT_CMD;
1910 2020 break;
1911 2021 case SID_TC_SPARE_GRD_SRC1:
1912 2022 i = SPARE_GRD_SRC1;
1913 2023 break;
1914 2024 case SID_TC_SPARE_GRD_SRC2:
1915 2025 i = SPARE_GRD_SRC2;
1916 2026 break;
1917 2027 case SID_TC_OBCP:
1918 2028 i = OBCP;
1919 2029 break;
1920 2030 case SID_TC_SYSTEM_CONTROL:
1921 2031 i = SYSTEM_CONTROL;
1922 2032 break;
1923 2033 case SID_TC_AOCS:
1924 2034 i = AOCS;
1925 2035 break;
1926 2036 case SID_TC_RPW_INTERNAL:
1927 2037 i = RPW_INTERNAL;
1928 2038 break;
1929 2039 default:
1930 2040 i = GROUND;
1931 2041 break;
1932 2042 }
1933 2043
1934 2044 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
1935 2045 sequence_cnt = sequenceCounters_TM_DUMP[ i ] & SEQ_CNT_MASK;
1936 2046
1937 2047 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
1938 2048
1939 2049 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> SHIFT_1_BYTE);
1940 2050 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1941 2051
1942 2052 // increment the sequence counter
1943 2053 if ( sequenceCounters_TM_DUMP[ i ] < SEQ_CNT_MAX )
1944 2054 {
1945 2055 sequenceCounters_TM_DUMP[ i ] = sequenceCounters_TM_DUMP[ i ] + 1;
1946 2056 }
1947 2057 else
1948 2058 {
1949 2059 sequenceCounters_TM_DUMP[ i ] = 0;
1950 2060 }
1951 2061 }
General Comments 0
You need to be logged in to leave comments. Login now