##// END OF EJS Templates
HG_REPOSITORIES » LPP » INSTRUMENTATION » SOLO_LFR » DEV_PLE
RSS

Clone from http://pc-instru.lpp.polytechnique.fr:5000/DEV_PLE

correction de bug 485
paul -
r223:48f1f70fe1bc R3
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,325 +1,329
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> // printf()
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 biaStatusInfo;
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 biaStatusInfo;
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 volatile int sm_f0[ ];
86 86 extern volatile int sm_f1[ ];
87 87 extern volatile int sm_f2[ ];
88 88
89 89 // parameters
90 90 extern struct param_local_str param_local;
91 91 extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
92 92
93 93 // registers
94 94 extern time_management_regs_t *time_management_regs;
95 95 extern volatile spectral_matrix_regs_t *spectral_matrix_regs;
96 96
97 97 extern rtems_name misc_name[5];
98 98 extern rtems_id Task_id[20]; /* array of task ids */
99 99
100 100 //
101 101 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel);
102 102 // ISR
103 103 rtems_isr spectral_matrices_isr( rtems_vector_number vector );
104 104 rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector );
105 105
106 106 //******************
107 107 // Spectral Matrices
108 108 void reset_nb_sm( void );
109 109 // SM
110 110 void SM_init_rings( void );
111 111 void SM_reset_current_ring_nodes( void );
112 112 // ASM
113 113 void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes );
114 114
115 115 //*****************
116 116 // Basic Parameters
117 117
118 118 void BP_reset_current_ring_nodes( void );
119 119 void BP_init_header(bp_packet *packet,
120 120 unsigned int apid, unsigned char sid,
121 121 unsigned int packetLength , unsigned char blkNr);
122 122 void BP_init_header_with_spare(bp_packet_with_spare *packet,
123 123 unsigned int apid, unsigned char sid,
124 124 unsigned int packetLength, unsigned char blkNr );
125 125 void BP_send( char *data,
126 126 rtems_id queue_id ,
127 127 unsigned int nbBytesToSend , unsigned int sid );
128 128
129 129 //******************
130 130 // general functions
131 131 void reset_sm_status( void );
132 132 void reset_spectral_matrix_regs( void );
133 133 void set_time(unsigned char *time, unsigned char *timeInBuffer );
134 134 unsigned long long int get_acquisition_time( unsigned char *timePtr );
135 135 unsigned char getSID( rtems_event_set event );
136 136
137 137 extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
138 138 extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
139 139
140 140 //***************************************
141 141 // DEFINITIONS OF STATIC INLINE FUNCTIONS
142 142 static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
143 143 ring_node *ring_node_tab[],
144 144 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
145 145 asm_msg *msgForMATR );
146 146
147 147 static inline void SM_average_debug(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
148 148 ring_node *ring_node_tab[],
149 149 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
150 150 asm_msg *msgForMATR );
151 151
152 152 void ASM_patch( float *inputASM, float *outputASM );
153 153
154 154 void extractReImVectors(float *inputASM, float *outputASM, unsigned int asmComponent );
155 155
156 156 static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized,
157 157 float divider );
158 158
159 159 static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat,
160 160 float divider,
161 161 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
162 162
163 163 static inline void ASM_convert(volatile float *input_matrix, char *output_matrix);
164 164
165 165 void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
166 166 ring_node *ring_node_tab[],
167 167 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
168 168 asm_msg *msgForMATR )
169 169 {
170 170 float sum;
171 171 unsigned int i;
172 172
173 173 for(i=0; i<TOTAL_SIZE_SM; i++)
174 174 {
175 175 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ]
176 176 + ( (int *) (ring_node_tab[1]->buffer_address) ) [ i ]
177 177 + ( (int *) (ring_node_tab[2]->buffer_address) ) [ i ]
178 178 + ( (int *) (ring_node_tab[3]->buffer_address) ) [ i ]
179 179 + ( (int *) (ring_node_tab[4]->buffer_address) ) [ i ]
180 180 + ( (int *) (ring_node_tab[5]->buffer_address) ) [ i ]
181 181 + ( (int *) (ring_node_tab[6]->buffer_address) ) [ i ]
182 182 + ( (int *) (ring_node_tab[7]->buffer_address) ) [ i ];
183 183
184 184 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
185 185 {
186 186 averaged_spec_mat_NORM[ i ] = sum;
187 187 averaged_spec_mat_SBM[ i ] = sum;
188 188 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
189 189 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
190 190 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
191 191 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
192 192 }
193 193 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
194 194 {
195 195 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
196 196 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
197 197 }
198 198 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
199 199 {
200 200 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
201 201 averaged_spec_mat_SBM[ i ] = sum;
202 202 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
203 203 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
204 204 }
205 205 else
206 206 {
207 PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
207 averaged_spec_mat_NORM[ i ] = sum;
208 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
209 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
210 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
211 // PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
208 212 }
209 213 }
210 214 }
211 215
212 216 void SM_average_debug( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
213 217 ring_node *ring_node_tab[],
214 218 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
215 219 asm_msg *msgForMATR )
216 220 {
217 221 float sum;
218 222 unsigned int i;
219 223
220 224 for(i=0; i<TOTAL_SIZE_SM; i++)
221 225 {
222 226 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ];
223 227 averaged_spec_mat_NORM[ i ] = sum;
224 228 averaged_spec_mat_SBM[ i ] = sum;
225 229 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
226 230 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
227 231 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
228 232 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
229 233 }
230 234 }
231 235
232 236 void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
233 237 {
234 238 int frequencyBin;
235 239 int asmComponent;
236 240 unsigned int offsetASM;
237 241 unsigned int offsetASMReorganized;
238 242
239 243 // BUILD DATA
240 244 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
241 245 {
242 246 for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
243 247 {
244 248 offsetASMReorganized =
245 249 frequencyBin * NB_VALUES_PER_SM
246 250 + asmComponent;
247 251 offsetASM =
248 252 asmComponent * NB_BINS_PER_SM
249 253 + frequencyBin;
250 254 averaged_spec_mat_reorganized[offsetASMReorganized ] =
251 255 averaged_spec_mat[ offsetASM ] / divider;
252 256 }
253 257 }
254 258 }
255 259
256 260 void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
257 261 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
258 262 {
259 263 int frequencyBin;
260 264 int asmComponent;
261 265 int offsetASM;
262 266 int offsetCompressed;
263 267 int k;
264 268
265 269 // BUILD DATA
266 270 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
267 271 {
268 272 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
269 273 {
270 274 offsetCompressed = // NO TIME OFFSET
271 275 frequencyBin * NB_VALUES_PER_SM
272 276 + asmComponent;
273 277 offsetASM = // NO TIME OFFSET
274 278 asmComponent * NB_BINS_PER_SM
275 279 + ASMIndexStart
276 280 + frequencyBin * nbBinsToAverage;
277 281 compressed_spec_mat[ offsetCompressed ] = 0;
278 282 for ( k = 0; k < nbBinsToAverage; k++ )
279 283 {
280 284 compressed_spec_mat[offsetCompressed ] =
281 285 ( compressed_spec_mat[ offsetCompressed ]
282 286 + averaged_spec_mat[ offsetASM + k ] );
283 287 }
284 288 compressed_spec_mat[ offsetCompressed ] =
285 289 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
286 290 }
287 291 }
288 292 }
289 293
290 294 void ASM_convert( volatile float *input_matrix, char *output_matrix)
291 295 {
292 296 unsigned int frequencyBin;
293 297 unsigned int asmComponent;
294 298 char * pt_char_input;
295 299 char * pt_char_output;
296 300 unsigned int offsetInput;
297 301 unsigned int offsetOutput;
298 302
299 303 pt_char_input = (char*) &input_matrix;
300 304 pt_char_output = (char*) &output_matrix;
301 305
302 306 // convert all other data
303 307 for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++)
304 308 {
305 309 for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++)
306 310 {
307 311 offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ;
308 312 offsetOutput = 2 * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ;
309 313 pt_char_input = (char*) &input_matrix [ offsetInput ];
310 314 pt_char_output = (char*) &output_matrix[ offsetOutput ];
311 315 pt_char_output[0] = pt_char_input[0]; // bits 31 downto 24 of the float
312 316 pt_char_output[1] = pt_char_input[1]; // bits 23 downto 16 of the float
313 317 }
314 318 }
315 319 }
316 320
317 321 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat,
318 322 float divider,
319 323 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
320 324
321 325 int getFBinMask(int k);
322 326
323 327 void init_kcoeff_sbm_from_kcoeff_norm( float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm);
324 328
325 329 #endif // FSW_PROCESSING_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,1175 +1,1175
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_common_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 // sy_lfr_b_bp_p0
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 // sy_lfr_b_bp_p1
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 // SET HTE PARAMETERS
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 void printKCoefficients(unsigned int freq, unsigned int bin, float *k_coeff)
314 314 {
315 315 printf("freq = %d *** bin = %d *** (0) %f *** (1) %f *** (2) %f *** (3) %f *** (4) %f\n",
316 316 freq,
317 317 bin,
318 318 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 0 ],
319 319 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 1 ],
320 320 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 2 ],
321 321 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 3 ],
322 322 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 4 ]);
323 323 }
324 324
325 325 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
326 326 {
327 327 /** This function updates the LFR registers with the incoming sbm2 parameters.
328 328 *
329 329 * @param TC points to the TeleCommand packet that is being processed
330 330 * @param queue_id is the id of the queue which handles TM related to this execution step
331 331 *
332 332 */
333 333
334 334 unsigned int address;
335 335 rtems_status_code status;
336 336 unsigned int freq;
337 337 unsigned int bin;
338 338 unsigned int coeff;
339 339 unsigned char *kCoeffPtr;
340 340 unsigned char *kCoeffDumpPtr;
341 341
342 342 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
343 343 // F0 => 11 bins
344 344 // F1 => 13 bins
345 345 // F2 => 12 bins
346 346 // 36 bins to dump in two packets (30 bins max per packet)
347 347
348 348 //*********
349 349 // PACKET 1
350 350 // 11 F0 bins, 13 F1 bins and 6 F2 bins
351 351 kcoefficients_dump_1.packetSequenceControl[0] = (unsigned char) (sequenceCounterParameterDump >> 8);
352 352 kcoefficients_dump_1.packetSequenceControl[1] = (unsigned char) (sequenceCounterParameterDump );
353 353 kcoefficients_dump_1.destinationID = TC->sourceID;
354 354 increment_seq_counter( &sequenceCounterParameterDump );
355 355 for( freq=0;
356 356 freq<NB_BINS_COMPRESSED_SM_F0;
357 357 freq++ )
358 358 {
359 359 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1] = freq;
360 360 bin = freq;
361 361 // printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
362 362 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
363 363 {
364 364 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
365 365 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
366 366 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
367 367 }
368 368 }
369 369 for( freq=NB_BINS_COMPRESSED_SM_F0;
370 370 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
371 371 freq++ )
372 372 {
373 373 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
374 374 bin = freq - NB_BINS_COMPRESSED_SM_F0;
375 375 // printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
376 376 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
377 377 {
378 378 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
379 379 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
380 380 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
381 381 }
382 382 }
383 383 for( freq=(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
384 384 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1+6);
385 385 freq++ )
386 386 {
387 387 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
388 388 bin = freq - (NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
389 389 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
390 390 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
391 391 {
392 392 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
393 393 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
394 394 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
395 395 }
396 396 }
397 397 kcoefficients_dump_1.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
398 398 kcoefficients_dump_1.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
399 399 kcoefficients_dump_1.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
400 400 kcoefficients_dump_1.time[3] = (unsigned char) (time_management_regs->coarse_time);
401 401 kcoefficients_dump_1.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
402 402 kcoefficients_dump_1.time[5] = (unsigned char) (time_management_regs->fine_time);
403 403 // SEND DATA
404 404 kcoefficient_node_1.status = 1;
405 405 address = (unsigned int) &kcoefficient_node_1;
406 406 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
407 407 if (status != RTEMS_SUCCESSFUL) {
408 408 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
409 409 }
410 410
411 411 //********
412 412 // PACKET 2
413 413 // 6 F2 bins
414 414 kcoefficients_dump_2.packetSequenceControl[0] = (unsigned char) (sequenceCounterParameterDump >> 8);
415 415 kcoefficients_dump_2.packetSequenceControl[1] = (unsigned char) (sequenceCounterParameterDump );
416 416 kcoefficients_dump_2.destinationID = TC->sourceID;
417 417 increment_seq_counter( &sequenceCounterParameterDump );
418 418 for( freq=0; freq<6; freq++ )
419 419 {
420 420 kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + 6 + freq;
421 421 bin = freq + 6;
422 422 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
423 423 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
424 424 {
425 425 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
426 426 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
427 427 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
428 428 }
429 429 }
430 430 kcoefficients_dump_2.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
431 431 kcoefficients_dump_2.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
432 432 kcoefficients_dump_2.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
433 433 kcoefficients_dump_2.time[3] = (unsigned char) (time_management_regs->coarse_time);
434 434 kcoefficients_dump_2.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
435 435 kcoefficients_dump_2.time[5] = (unsigned char) (time_management_regs->fine_time);
436 436 // SEND DATA
437 437 kcoefficient_node_2.status = 1;
438 438 address = (unsigned int) &kcoefficient_node_2;
439 439 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
440 440 if (status != RTEMS_SUCCESSFUL) {
441 441 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
442 442 }
443 443
444 444 return status;
445 445 }
446 446
447 447 int action_dump_par( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
448 448 {
449 449 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
450 450 *
451 451 * @param queue_id is the id of the queue which handles TM related to this execution step.
452 452 *
453 453 * @return RTEMS directive status codes:
454 454 * - RTEMS_SUCCESSFUL - message sent successfully
455 455 * - RTEMS_INVALID_ID - invalid queue id
456 456 * - RTEMS_INVALID_SIZE - invalid message size
457 457 * - RTEMS_INVALID_ADDRESS - buffer is NULL
458 458 * - RTEMS_UNSATISFIED - out of message buffers
459 459 * - RTEMS_TOO_MANY - queue s limit has been reached
460 460 *
461 461 */
462 462
463 463 int status;
464 464
465 465 // UPDATE TIME
466 466 parameter_dump_packet.packetSequenceControl[0] = (unsigned char) (sequenceCounterParameterDump >> 8);
467 467 parameter_dump_packet.packetSequenceControl[1] = (unsigned char) (sequenceCounterParameterDump );
468 468 increment_seq_counter( &sequenceCounterParameterDump );
469 469 parameter_dump_packet.destinationID = TC->sourceID;
470 470
471 471 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
472 472 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
473 473 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
474 474 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
475 475 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
476 476 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
477 477 // SEND DATA
478 478 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
479 479 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
480 480 if (status != RTEMS_SUCCESSFUL) {
481 481 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
482 482 }
483 483
484 484 return status;
485 485 }
486 486
487 487 //***********************
488 488 // NORMAL MODE PARAMETERS
489 489
490 490 int check_common_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
491 491 {
492 492 unsigned char msb;
493 493 unsigned char lsb;
494 494 int flag;
495 495 float aux;
496 496 rtems_status_code status;
497 497
498 498 unsigned int sy_lfr_n_swf_l;
499 499 unsigned int sy_lfr_n_swf_p;
500 500 unsigned int sy_lfr_n_asm_p;
501 501 unsigned char sy_lfr_n_bp_p0;
502 502 unsigned char sy_lfr_n_bp_p1;
503 503 unsigned char sy_lfr_n_cwf_long_f3;
504 504
505 505 flag = LFR_SUCCESSFUL;
506 506
507 507 //***************
508 508 // get parameters
509 509 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
510 510 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
511 511 sy_lfr_n_swf_l = msb * 256 + lsb;
512 512
513 513 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
514 514 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
515 515 sy_lfr_n_swf_p = msb * 256 + lsb;
516 516
517 517 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
518 518 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
519 519 sy_lfr_n_asm_p = msb * 256 + lsb;
520 520
521 521 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
522 522
523 523 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
524 524
525 525 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
526 526
527 527 //******************
528 528 // check consistency
529 529 // sy_lfr_n_swf_l
530 530 if (sy_lfr_n_swf_l != 2048)
531 531 {
532 532 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L+10, sy_lfr_n_swf_l );
533 533 flag = WRONG_APP_DATA;
534 534 }
535 535 // sy_lfr_n_swf_p
536 536 if (flag == LFR_SUCCESSFUL)
537 537 {
538 538 if ( sy_lfr_n_swf_p < 16 )
539 539 {
540 540 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P+10, sy_lfr_n_swf_p );
541 541 flag = WRONG_APP_DATA;
542 542 }
543 543 }
544 544 // sy_lfr_n_bp_p0
545 545 if (flag == LFR_SUCCESSFUL)
546 546 {
547 547 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
548 548 {
549 549 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0+10, sy_lfr_n_bp_p0 );
550 550 flag = WRONG_APP_DATA;
551 551 }
552 552 }
553 553 // sy_lfr_n_asm_p
554 554 if (flag == LFR_SUCCESSFUL)
555 555 {
556 556 if (sy_lfr_n_asm_p == 0)
557 557 {
558 558 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
559 559 flag = WRONG_APP_DATA;
560 560 }
561 561 }
562 562 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
563 563 if (flag == LFR_SUCCESSFUL)
564 564 {
565 565 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
566 566 if (aux > FLOAT_EQUAL_ZERO)
567 567 {
568 568 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
569 569 flag = WRONG_APP_DATA;
570 570 }
571 571 }
572 572 // sy_lfr_n_bp_p1
573 573 if (flag == LFR_SUCCESSFUL)
574 574 {
575 575 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
576 576 {
577 577 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
578 578 flag = WRONG_APP_DATA;
579 579 }
580 580 }
581 581 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
582 582 if (flag == LFR_SUCCESSFUL)
583 583 {
584 584 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
585 585 if (aux > FLOAT_EQUAL_ZERO)
586 586 {
587 587 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
588 588 flag = LFR_DEFAULT;
589 589 }
590 590 }
591 591 // sy_lfr_n_cwf_long_f3
592 592
593 593 return flag;
594 594 }
595 595
596 596 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
597 597 {
598 598 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
599 599 *
600 600 * @param TC points to the TeleCommand packet that is being processed
601 601 * @param queue_id is the id of the queue which handles TM related to this execution step
602 602 *
603 603 */
604 604
605 605 int result;
606 606
607 607 result = LFR_SUCCESSFUL;
608 608
609 609 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
610 610 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
611 611
612 612 return result;
613 613 }
614 614
615 615 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
616 616 {
617 617 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
618 618 *
619 619 * @param TC points to the TeleCommand packet that is being processed
620 620 * @param queue_id is the id of the queue which handles TM related to this execution step
621 621 *
622 622 */
623 623
624 624 int result;
625 625
626 626 result = LFR_SUCCESSFUL;
627 627
628 628 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
629 629 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
630 630
631 631 return result;
632 632 }
633 633
634 634 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
635 635 {
636 636 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
637 637 *
638 638 * @param TC points to the TeleCommand packet that is being processed
639 639 * @param queue_id is the id of the queue which handles TM related to this execution step
640 640 *
641 641 */
642 642
643 643 int result;
644 644
645 645 result = LFR_SUCCESSFUL;
646 646
647 647 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
648 648 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
649 649
650 650 return result;
651 651 }
652 652
653 653 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
654 654 {
655 655 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
656 656 *
657 657 * @param TC points to the TeleCommand packet that is being processed
658 658 * @param queue_id is the id of the queue which handles TM related to this execution step
659 659 *
660 660 */
661 661
662 662 int status;
663 663
664 664 status = LFR_SUCCESSFUL;
665 665
666 666 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
667 667
668 668 return status;
669 669 }
670 670
671 671 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
672 672 {
673 673 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
674 674 *
675 675 * @param TC points to the TeleCommand packet that is being processed
676 676 * @param queue_id is the id of the queue which handles TM related to this execution step
677 677 *
678 678 */
679 679
680 680 int status;
681 681
682 682 status = LFR_SUCCESSFUL;
683 683
684 684 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
685 685
686 686 return status;
687 687 }
688 688
689 689 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
690 690 {
691 691 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
692 692 *
693 693 * @param TC points to the TeleCommand packet that is being processed
694 694 * @param queue_id is the id of the queue which handles TM related to this execution step
695 695 *
696 696 */
697 697
698 698 int status;
699 699
700 700 status = LFR_SUCCESSFUL;
701 701
702 702 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
703 703
704 704 return status;
705 705 }
706 706
707 707 //**********************
708 708 // BURST MODE PARAMETERS
709 709 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
710 710 {
711 711 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
712 712 *
713 713 * @param TC points to the TeleCommand packet that is being processed
714 714 * @param queue_id is the id of the queue which handles TM related to this execution step
715 715 *
716 716 */
717 717
718 718 int status;
719 719
720 720 status = LFR_SUCCESSFUL;
721 721
722 722 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
723 723
724 724 return status;
725 725 }
726 726
727 727 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
728 728 {
729 729 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
730 730 *
731 731 * @param TC points to the TeleCommand packet that is being processed
732 732 * @param queue_id is the id of the queue which handles TM related to this execution step
733 733 *
734 734 */
735 735
736 736 int status;
737 737
738 738 status = LFR_SUCCESSFUL;
739 739
740 740 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
741 741
742 742 return status;
743 743 }
744 744
745 745 //*********************
746 746 // SBM1 MODE PARAMETERS
747 747 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
748 748 {
749 749 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
750 750 *
751 751 * @param TC points to the TeleCommand packet that is being processed
752 752 * @param queue_id is the id of the queue which handles TM related to this execution step
753 753 *
754 754 */
755 755
756 756 int status;
757 757
758 758 status = LFR_SUCCESSFUL;
759 759
760 760 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
761 761
762 762 return status;
763 763 }
764 764
765 765 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
766 766 {
767 767 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
768 768 *
769 769 * @param TC points to the TeleCommand packet that is being processed
770 770 * @param queue_id is the id of the queue which handles TM related to this execution step
771 771 *
772 772 */
773 773
774 774 int status;
775 775
776 776 status = LFR_SUCCESSFUL;
777 777
778 778 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
779 779
780 780 return status;
781 781 }
782 782
783 783 //*********************
784 784 // SBM2 MODE PARAMETERS
785 785 int set_sy_lfr_s2_bp_p0(ccsdsTelecommandPacket_t *TC)
786 786 {
787 787 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
788 788 *
789 789 * @param TC points to the TeleCommand packet that is being processed
790 790 * @param queue_id is the id of the queue which handles TM related to this execution step
791 791 *
792 792 */
793 793
794 794 int status;
795 795
796 796 status = LFR_SUCCESSFUL;
797 797
798 798 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
799 799
800 800 return status;
801 801 }
802 802
803 803 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
804 804 {
805 805 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
806 806 *
807 807 * @param TC points to the TeleCommand packet that is being processed
808 808 * @param queue_id is the id of the queue which handles TM related to this execution step
809 809 *
810 810 */
811 811
812 812 int status;
813 813
814 814 status = LFR_SUCCESSFUL;
815 815
816 816 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
817 817
818 818 return status;
819 819 }
820 820
821 821 //*******************
822 822 // TC_LFR_UPDATE_INFO
823 823 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
824 824 {
825 825 unsigned int status;
826 826
827 827 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
828 828 || (mode == LFR_MODE_BURST)
829 829 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
830 830 {
831 831 status = LFR_SUCCESSFUL;
832 832 }
833 833 else
834 834 {
835 835 status = LFR_DEFAULT;
836 836 }
837 837
838 838 return status;
839 839 }
840 840
841 841 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
842 842 {
843 843 unsigned int status;
844 844
845 845 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
846 846 || (mode == TDS_MODE_BURST)
847 847 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
848 848 || (mode == TDS_MODE_LFM))
849 849 {
850 850 status = LFR_SUCCESSFUL;
851 851 }
852 852 else
853 853 {
854 854 status = LFR_DEFAULT;
855 855 }
856 856
857 857 return status;
858 858 }
859 859
860 860 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
861 861 {
862 862 unsigned int status;
863 863
864 864 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
865 865 || (mode == THR_MODE_BURST))
866 866 {
867 867 status = LFR_SUCCESSFUL;
868 868 }
869 869 else
870 870 {
871 871 status = LFR_DEFAULT;
872 872 }
873 873
874 874 return status;
875 875 }
876 876
877 877 //***********
878 878 // FBINS MASK
879 879
880 880 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
881 881 {
882 882 int status;
883 883 unsigned int k;
884 884 unsigned char *fbins_mask_dump;
885 885 unsigned char *fbins_mask_TC;
886 886
887 887 status = LFR_SUCCESSFUL;
888 888
889 889 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
890 890 fbins_mask_TC = TC->dataAndCRC;
891 891
892 892 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
893 893 {
894 894 fbins_mask_dump[k] = fbins_mask_TC[k];
895 895 }
896 896 for (k=0; k < NB_FBINS_MASKS; k++)
897 897 {
898 898 unsigned char *auxPtr;
899 899 auxPtr = &parameter_dump_packet.sy_lfr_fbins_f0_word1[k*NB_BYTES_PER_FBINS_MASK];
900 900 printf("%x %x %x %x\n", auxPtr[0], auxPtr[1], auxPtr[2], auxPtr[3]);
901 901 }
902 902
903 903
904 904 return status;
905 905 }
906 906
907 907 //**************
908 908 // KCOEFFICIENTS
909 909 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC,rtems_id queue_id )
910 910 {
911 911 unsigned int kcoeff;
912 912 unsigned short sy_lfr_kcoeff_frequency;
913 913 unsigned short bin;
914 914 unsigned short *freqPtr;
915 915 float *kcoeffPtr_norm;
916 916 float *kcoeffPtr_sbm;
917 917 int status;
918 918 unsigned char *kcoeffLoadPtr;
919 919 unsigned char *kcoeffNormPtr;
920 920 unsigned char *kcoeffSbmPtr_a;
921 921 unsigned char *kcoeffSbmPtr_b;
922 922
923 923 status = LFR_SUCCESSFUL;
924 924
925 925 kcoeffPtr_norm = NULL;
926 926 kcoeffPtr_sbm = NULL;
927 927 bin = 0;
928 928
929 929 freqPtr = (unsigned short *) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY];
930 930 sy_lfr_kcoeff_frequency = *freqPtr;
931 931
932 932 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
933 933 {
934 934 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
935 935 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 10 + 1,
936 936 TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 1] ); // +1 to get the LSB instead of the MSB
937 937 status = LFR_DEFAULT;
938 938 }
939 939 else
940 940 {
941 941 if ( ( sy_lfr_kcoeff_frequency >= 0 )
942 942 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
943 943 {
944 944 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
945 945 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
946 946 bin = sy_lfr_kcoeff_frequency;
947 947 }
948 948 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
949 949 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
950 950 {
951 951 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
952 952 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
953 953 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
954 954 }
955 955 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
956 956 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
957 957 {
958 958 kcoeffPtr_norm = k_coeff_intercalib_f2;
959 959 kcoeffPtr_sbm = NULL;
960 960 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
961 961 }
962 962 }
963 963
964 964 printf("in set_sy_lfr_kcoeff *** freq = %d, bin = %d\n", sy_lfr_kcoeff_frequency, bin);
965 965
966 966 if (kcoeffPtr_norm != NULL ) // update K coefficient for NORMAL data products
967 967 {
968 968 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
969 969 {
970 970 // destination
971 971 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + kcoeff ];
972 972 // source
973 973 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * kcoeff];
974 974 // copy source to destination
975 975 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
976 976 }
977 977 }
978 978
979 979 if (kcoeffPtr_sbm != NULL ) // update K coefficient for SBM data products
980 980 {
981 981 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
982 982 {
983 983 // destination
984 984 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * 2 ];
985 985 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * 2 + 1 ];
986 986 // source
987 987 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * kcoeff];
988 988 // copy source to destination
989 989 copyFloatByChar( kcoeffSbmPtr_a, kcoeffLoadPtr );
990 990 copyFloatByChar( kcoeffSbmPtr_b, kcoeffLoadPtr );
991 991 }
992 992 }
993 993
994 994 // print_k_coeff();
995 995
996 996 return status;
997 997 }
998 998
999 999 void copyFloatByChar( unsigned char *destination, unsigned char *source )
1000 1000 {
1001 1001 destination[0] = source[0];
1002 1002 destination[1] = source[1];
1003 1003 destination[2] = source[2];
1004 1004 destination[3] = source[3];
1005 1005 }
1006 1006
1007 1007 //**********
1008 1008 // init dump
1009 1009
1010 1010 void init_parameter_dump( void )
1011 1011 {
1012 1012 /** This function initialize the parameter_dump_packet global variable with default values.
1013 1013 *
1014 1014 */
1015 1015
1016 1016 unsigned int k;
1017 1017
1018 1018 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
1019 1019 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
1020 1020 parameter_dump_packet.reserved = CCSDS_RESERVED;
1021 1021 parameter_dump_packet.userApplication = CCSDS_USER_APP;
1022 1022 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);
1023 1023 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1024 1024 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1025 1025 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1026 1026 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> 8);
1027 1027 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
1028 1028 // DATA FIELD HEADER
1029 1029 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1030 1030 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
1031 1031 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
1032 1032 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
1033 1033 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
1034 1034 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
1035 1035 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
1036 1036 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
1037 1037 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
1038 1038 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
1039 1039 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
1040 1040
1041 1041 //******************
1042 1042 // COMMON PARAMETERS
1043 1043 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
1044 1044 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
1045 1045
1046 1046 //******************
1047 1047 // NORMAL PARAMETERS
1048 1048 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> 8);
1049 1049 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1050 1050 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> 8);
1051 1051 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1052 1052 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> 8);
1053 1053 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1054 1054 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1055 1055 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1056 1056 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1057 1057
1058 1058 //*****************
1059 1059 // BURST PARAMETERS
1060 1060 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1061 1061 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1062 1062
1063 1063 //****************
1064 1064 // SBM1 PARAMETERS
1065 1065 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
1066 1066 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1067 1067
1068 1068 //****************
1069 1069 // SBM2 PARAMETERS
1070 1070 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1071 1071 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1072 1072
1073 1073 //************
1074 1074 // FBINS MASKS
1075 1075 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
1076 1076 {
1077 1077 parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = 0xff;
1078 1078 }
1079 1079 }
1080 1080
1081 1081 void init_kcoefficients_dump( void )
1082 1082 {
1083 1083 init_kcoefficients_dump_packet( &kcoefficients_dump_1, 1, 30 );
1084 1084 init_kcoefficients_dump_packet( &kcoefficients_dump_2, 2, 6 );
1085 1085
1086 1086 kcoefficient_node_1.previous = NULL;
1087 1087 kcoefficient_node_1.next = NULL;
1088 1088 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1089 1089 kcoefficient_node_1.coarseTime = 0x00;
1090 1090 kcoefficient_node_1.fineTime = 0x00;
1091 1091 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1092 1092 kcoefficient_node_1.status = 0x00;
1093 1093
1094 1094 kcoefficient_node_2.previous = NULL;
1095 1095 kcoefficient_node_2.next = NULL;
1096 1096 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1097 1097 kcoefficient_node_2.coarseTime = 0x00;
1098 1098 kcoefficient_node_2.fineTime = 0x00;
1099 1099 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1100 1100 kcoefficient_node_2.status = 0x00;
1101 1101 }
1102 1102
1103 1103 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1104 1104 {
1105 1105 unsigned int k;
1106 1106 unsigned int packetLength;
1107 1107
1108 1108 packetLength = blk_nr * 130 + 20 - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1109 1109
1110 1110 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1111 1111 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1112 1112 kcoefficients_dump->reserved = CCSDS_RESERVED;
1113 1113 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1114 1114 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);;
1115 1115 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;;
1116 1116 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1117 1117 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1118 1118 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> 8);
1119 1119 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1120 1120 // DATA FIELD HEADER
1121 1121 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1122 1122 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1123 1123 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1124 1124 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1125 1125 kcoefficients_dump->time[0] = 0x00;
1126 1126 kcoefficients_dump->time[1] = 0x00;
1127 1127 kcoefficients_dump->time[2] = 0x00;
1128 1128 kcoefficients_dump->time[3] = 0x00;
1129 1129 kcoefficients_dump->time[4] = 0x00;
1130 1130 kcoefficients_dump->time[5] = 0x00;
1131 1131 kcoefficients_dump->sid = SID_K_DUMP;
1132 1132
1133 1133 kcoefficients_dump->pkt_cnt = 2;
1134 1134 kcoefficients_dump->pkt_nr = pkt_nr;
1135 1135 kcoefficients_dump->blk_nr = blk_nr;
1136 1136
1137 1137 //******************
1138 1138 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1139 1139 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1140 1140 for (k=0; k<3900; k++)
1141 1141 {
1142 1142 kcoefficients_dump->kcoeff_blks[k] = 0x00;
1143 1143 }
1144 1144 }
1145 1145
1146 1146 void print_k_coeff()
1147 1147 {
1148 1148 unsigned int kcoeff;
1149 1149 unsigned int bin;
1150 1150
1151 1151 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1152 1152 {
1153 1153 printf("kcoeff = %d *** ", kcoeff);
1154 1154 for (bin=0; bin<NB_BINS_COMPRESSED_SM_F0; bin++)
1155 1155 {
1156 1156 printf( "%f ", k_coeff_intercalib_f0_norm[bin*NB_K_COEFF_PER_BIN+kcoeff] );
1157 1157 }
1158 1158 printf("\n");
1159 1159 }
1160 1160
1161 1161 printf("\n");
1162 1162
1163 1163 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1164 1164 {
1165 1165 printf("kcoeff = %d *** ", kcoeff);
1166 1166 for (bin=0; bin<NB_BINS_COMPRESSED_SM_F0; bin++)
1167 1167 {
1168 1168 printf( "[%f, %f] ",
1169 1169 k_coeff_intercalib_f0_sbm[(bin*NB_K_COEFF_PER_BIN )*2 + kcoeff],
1170 1170 k_coeff_intercalib_f0_sbm[(bin*NB_K_COEFF_PER_BIN+1)*2 + kcoeff]);
1171 1171 }
1172 1172 printf("\n");
1173 1173 }
1174 1174 }
1175 1175
General Comments 0
You need to be logged in to leave comments. Login now