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

Fork of HG_REPOSITORIES/LPP/INSTRUMENTATION/SOLO_LFR/DEV_PLE

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

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