##// END OF EJS Templates
Minor updates to use the time management VHDL module
paul@pc-solar1.lab-lpp.local -
r12:334ab051262f default
parent child
Show More
@@ -1,46 +1,46
1 <?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
1 <?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
2 <CodeBlocks_layout_file>
2 <CodeBlocks_layout_file>
3 <ActiveTarget name="Debug" />
3 <ActiveTarget name="Debug" />
4 <File name="../header/ccsds_types.h" open="1" top="0" tabpos="3">
4 <File name="../header/ccsds_types.h" open="0" top="0" tabpos="3">
5 <Cursor position="3403" topLine="101" />
5 <Cursor position="3403" topLine="101" />
6 </File>
6 </File>
7 <File name="../header/fsw_init.h" open="0" top="0" tabpos="3">
7 <File name="../header/fsw_init.h" open="0" top="0" tabpos="3">
8 <Cursor position="899" topLine="0" />
8 <Cursor position="378" topLine="0" />
9 </File>
9 </File>
10 <File name="../header/fsw_misc.h" open="0" top="0" tabpos="0">
10 <File name="../header/fsw_misc.h" open="0" top="0" tabpos="0">
11 <Cursor position="114" topLine="0" />
11 <Cursor position="114" topLine="0" />
12 </File>
12 </File>
13 <File name="../header/fsw_params.h" open="0" top="0" tabpos="0">
13 <File name="../header/fsw_params.h" open="0" top="0" tabpos="2">
14 <Cursor position="2405" topLine="52" />
14 <Cursor position="1358" topLine="31" />
15 </File>
15 </File>
16 <File name="../header/fsw_processing.h" open="0" top="0" tabpos="0">
16 <File name="../header/fsw_processing.h" open="0" top="0" tabpos="0">
17 <Cursor position="576" topLine="0" />
17 <Cursor position="628" topLine="7" />
18 </File>
18 </File>
19 <File name="../header/grlib_regs.h" open="0" top="0" tabpos="0">
19 <File name="../header/grlib_regs.h" open="0" top="0" tabpos="4">
20 <Cursor position="1072" topLine="0" />
20 <Cursor position="1153" topLine="11" />
21 </File>
21 </File>
22 <File name="../header/tc_handler.h" open="0" top="0" tabpos="4">
22 <File name="../header/tc_handler.h" open="0" top="0" tabpos="4">
23 <Cursor position="481" topLine="0" />
23 <Cursor position="624" topLine="2" />
24 </File>
24 </File>
25 <File name="../header/wf_handler.h" open="0" top="0" tabpos="0">
25 <File name="../header/wf_handler.h" open="0" top="0" tabpos="0">
26 <Cursor position="212" topLine="0" />
26 <Cursor position="318" topLine="0" />
27 </File>
27 </File>
28 <File name="../src/fsw_globals.c" open="0" top="0" tabpos="4">
28 <File name="../src/fsw_globals.c" open="0" top="0" tabpos="3">
29 <Cursor position="1531" topLine="1" />
29 <Cursor position="378" topLine="1" />
30 </File>
30 </File>
31 <File name="../src/fsw_init.c" open="1" top="0" tabpos="1">
31 <File name="../src/fsw_init.c" open="1" top="0" tabpos="1">
32 <Cursor position="9387" topLine="83" />
32 <Cursor position="2303" topLine="54" />
33 </File>
33 </File>
34 <File name="../src/fsw_misc.c" open="0" top="0" tabpos="0">
34 <File name="../src/fsw_misc.c" open="0" top="0" tabpos="2">
35 <Cursor position="20" topLine="0" />
35 <Cursor position="475" topLine="0" />
36 </File>
36 </File>
37 <File name="../src/fsw_processing.c" open="0" top="0" tabpos="3">
37 <File name="../src/fsw_processing.c" open="0" top="0" tabpos="0">
38 <Cursor position="1797" topLine="73" />
38 <Cursor position="1696" topLine="59" />
39 </File>
39 </File>
40 <File name="../src/tc_handler.c" open="1" top="1" tabpos="2">
40 <File name="../src/tc_handler.c" open="1" top="1" tabpos="2">
41 <Cursor position="10603" topLine="21" />
41 <Cursor position="17607" topLine="36" />
42 </File>
42 </File>
43 <File name="../src/wf_handler.c" open="0" top="0" tabpos="0">
43 <File name="../src/wf_handler.c" open="0" top="0" tabpos="4">
44 <Cursor position="542" topLine="0" />
44 <Cursor position="1660" topLine="24" />
45 </File>
45 </File>
46 </CodeBlocks_layout_file>
46 </CodeBlocks_layout_file>
1 NO CONTENT: modified file, binary diff hidden
NO CONTENT: modified file, binary diff hidden
@@ -21,11 +21,17
21 #define REGS_ADDR_SPECTRAL_MATRICES 0x80000700
21 #define REGS_ADDR_SPECTRAL_MATRICES 0x80000700
22
22
23 #define APBUART_CTRL_REG_MASK_DB 0xfffff7ff
23 #define APBUART_CTRL_REG_MASK_DB 0xfffff7ff
24
24
25 //**********
26 // IRQ LINES
25 #define IRQ_SM 9
27 #define IRQ_SM 9
26 #define IRQ_SPARC_SM 0x19 // see sparcv8.pdf p.76 for interrupt levels
28 #define IRQ_SPARC_SM 0x19 // see sparcv8.pdf p.76 for interrupt levels
27 #define IRQ_WF 10
29 #define IRQ_WF 10
28 #define IRQ_SPARC_WF 0x1a // see sparcv8.pdf p.76 for interrupt levels
30 #define IRQ_SPARC_WF 0x1a // see sparcv8.pdf p.76 for interrupt levels
31 #define IRQ_TIME1 12
32 #define IRQ_SPARC_TIME1 0x1c // see sparcv8.pdf p.76 for interrupt levels
33 #define IRQ_TIME2 13
34 #define IRQ_SPARC_TIME2 0x1d // see sparcv8.pdf p.76 for interrupt levels
29
35
30 #define CLKDIV_SM_SIMULATOR 9999
36 #define CLKDIV_SM_SIMULATOR 9999
31 #define CLKDIV_WF_SIMULATOR 9999999
37 #define CLKDIV_WF_SIMULATOR 9999999
@@ -42,11 +48,13
42 #define TASKID_AVF0 6
48 #define TASKID_AVF0 6
43 #define TASKID_BPF0 7
49 #define TASKID_BPF0 7
44 #define TASKID_WFRM 8
50 #define TASKID_WFRM 8
51 #define TASKID_DUMB 9
45
52
46 #define ACTION_MSG_QUEUE_COUNT 10
53 #define ACTION_MSG_QUEUE_COUNT 10
47
54
48 //*******
55 //*******
49 // MACROS
56 // MACROS
57 //#define PRINT_TASK_STATISTICS
50 #define PRINT_MESSAGES_ON_CONSOLE // enable or disable the printf instructions
58 #define PRINT_MESSAGES_ON_CONSOLE // enable or disable the printf instructions
51 #ifdef PRINT_MESSAGES_ON_CONSOLE
59 #ifdef PRINT_MESSAGES_ON_CONSOLE
52 #define PRINTF(x) printf(x);
60 #define PRINTF(x) printf(x);
@@ -27,6 +27,11 unsigned int currentTC_LEN_RCV_AsUnsigne
27 unsigned int currentTM_length;
27 unsigned int currentTM_length;
28 unsigned char currentTC_processedFlag;
28 unsigned char currentTC_processedFlag;
29
29
30 //****
31 // ISR
32 rtems_isr commutation_isr1( rtems_vector_number vector );
33 rtems_isr commutation_isr2( rtems_vector_number vector );
34
30 //**********************
35 //**********************
31 // GENERAL USE FUNCTIONS
36 // GENERAL USE FUNCTIONS
32 unsigned int lookUpTableForCRC[256];
37 unsigned int lookUpTableForCRC[256];
@@ -46,6 +51,7 int TC_checker(ccsdsTelecommandPacket_t
46 // RTEMS TASK
51 // RTEMS TASK
47 rtems_task recv_task( rtems_task_argument unused );
52 rtems_task recv_task( rtems_task_argument unused );
48 rtems_task actn_task( rtems_task_argument unused );
53 rtems_task actn_task( rtems_task_argument unused );
54 rtems_task dumb_task( rtems_task_argument unused );
49 int create_message_queue();
55 int create_message_queue();
50
56
51 //***********
57 //***********
@@ -56,7 +56,8 char *lstates[6] = {"Error-reset",
56
56
57 rtems_task Init( rtems_task_argument ignored )
57 rtems_task Init( rtems_task_argument ignored )
58 {
58 {
59 rtems_status_code status;
59 rtems_status_code status;
60 rtems_isr_entry old_isr_handler;
60
61
61 //send_console_outputs_on_serial_port();
62 //send_console_outputs_on_serial_port();
62
63
@@ -71,6 +72,22 rtems_task Init( rtems_task_argument ign
71 IRQ_SPARC_SM, spectral_matrices_isr );
72 IRQ_SPARC_SM, spectral_matrices_isr );
72 configure_timer((gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_WF_SIMULATOR, CLKDIV_WF_SIMULATOR,
73 configure_timer((gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_WF_SIMULATOR, CLKDIV_WF_SIMULATOR,
73 IRQ_SPARC_WF, waveforms_isr );
74 IRQ_SPARC_WF, waveforms_isr );
75
76 // irq handling of the time management unit
77 /*status = rtems_interrupt_catch( commutation_isr1,
78 IRQ_SPARC_TIME1,
79 &old_isr_handler) ; // see sparcv8.pdf p.76 for interrupt levels
80 if (status==RTEMS_SUCCESSFUL)
81 PRINTF("commutation_isr1 *** rtems_interrupt_catch successfullly configured\n")
82
83 status = rtems_interrupt_catch( commutation_isr2,
84 IRQ_SPARC_TIME2,
85 &old_isr_handler) ; // see sparcv8.pdf p.76 for interrupt levels
86 if (status==RTEMS_SUCCESSFUL)
87 PRINTF("commutation_isr2 *** rtems_interrupt_catch successfullly configured\n")
88
89 LEON_Unmask_interrupt( IRQ_TIME1 );
90 LEON_Unmask_interrupt( IRQ_TIME2 );*/
74
91
75 status = rtems_task_delete(RTEMS_SELF);
92 status = rtems_task_delete(RTEMS_SELF);
76 }
93 }
@@ -128,7 +145,8 int create_all_tasks()
128 Task_name[5] = rtems_build_name( 'S', 'T', 'A', 'T' );
145 Task_name[5] = rtems_build_name( 'S', 'T', 'A', 'T' );
129 Task_name[6] = rtems_build_name( 'A', 'V', 'F', '0' );
146 Task_name[6] = rtems_build_name( 'A', 'V', 'F', '0' );
130 Task_name[7] = rtems_build_name( 'B', 'P', 'F', '0' );
147 Task_name[7] = rtems_build_name( 'B', 'P', 'F', '0' );
131 Task_name[8] = rtems_build_name( 'W', 'F', 'R', 'M' );
148 Task_name[8] = rtems_build_name( 'W', 'F', 'R', 'M' );
149 //Task_name[9] = rtems_build_name( 'D', 'U', 'M', 'B' );
132
150
133 // RECV
151 // RECV
134 status = rtems_task_create(
152 status = rtems_task_create(
@@ -156,7 +174,7 int create_all_tasks()
156 );
174 );
157 // STAT
175 // STAT
158 status = rtems_task_create(
176 status = rtems_task_create(
159 Task_name[5], 200, RTEMS_MINIMUM_STACK_SIZE * 2,
177 Task_name[5], 150, RTEMS_MINIMUM_STACK_SIZE * 2,
160 RTEMS_DEFAULT_MODES,
178 RTEMS_DEFAULT_MODES,
161 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[5]
179 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[5]
162 );
180 );
@@ -178,6 +196,12 int create_all_tasks()
178 RTEMS_DEFAULT_MODES,
196 RTEMS_DEFAULT_MODES,
179 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[8]
197 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[8]
180 );
198 );
199 // DUMB
200 /*status = rtems_task_create(
201 Task_name[9], 200, RTEMS_MINIMUM_STACK_SIZE * 2,
202 RTEMS_DEFAULT_MODES,
203 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[9]
204 );*/
181
205
182 return 0;
206 return 0;
183 }
207 }
@@ -210,6 +234,9 int start_all_tasks()
210 status = rtems_task_start( Task_id[8], wfrm_task, 1 );
234 status = rtems_task_start( Task_id[8], wfrm_task, 1 );
211 if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_WFRM\n")
235 if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_WFRM\n")
212
236
237 /*status = rtems_task_start( Task_id[9], dumb_task, 1 );
238 if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_DUMB\n")*/
239
213 return 0;
240 return 0;
214 }
241 }
215
242
@@ -61,9 +61,11 rtems_task stat_task(rtems_task_argument
61 while(1){
61 while(1){
62 rtems_task_wake_after(1000);
62 rtems_task_wake_after(1000);
63 PRINTF1("%d\n", i)
63 PRINTF1("%d\n", i)
64 if (i == 2) {
64 if (i == 2) {
65 #ifdef PRINT_TASK_STATISTICS
65 rtems_cpu_usage_report();
66 rtems_cpu_usage_report();
66 rtems_cpu_usage_reset();
67 rtems_cpu_usage_reset();
68 #endif
67 i = 0;
69 i = 0;
68 }
70 }
69 else i++;
71 else i++;
@@ -1,34 +1,47
1 #include <fsw_processing.h>
1 #include <fsw_processing.h>
2 #include <math.h>
2 #include <math.h>
3 #include <stdio.h>
3 #include <stdio.h>
4 #include <stdlib.h>
4 #include <leon.h>
5 #include <leon.h>
6
7 // TOTAL = 32 coefficients * 4 = 128 octets * 3 * 12 = 4608 octets
8 // SX 12 coefficients
9 float k14_sx_re = 1;
10 float k14_sx_im = 1;
11 float k15_sx_re = 1;
12 float k15_sx_im = 1;
13 float k24_sx_re = 1;
14 float k24_sx_im = 1;
15 float k25_sx_re = 1;
16 float k25_sx_im = 1;
17 float k34_sx_re = 1;
18 float k34_sx_im = 1;
19 float k35_sx_re = 1;
20 float k35_sx_im = 1;
21 // NY 8 coefficients
22 float k24_ny_re = 1;
23 float k24_ny_im = 1;
24 float k25_ny_re = 1;
25 float k25_ny_im = 1;
26 float k34_ny_re = 1;
27 float k34_ny_im = 1;
28 float k35_ny_re = 1;
29 float k35_ny_im = 1;
30 // NZ 8 coefficients
31 float k24_nz_re = 1;
32 float k24_nz_im = 1;
33 float k25_nz_re = 1;
34 float k25_nz_im = 1;
35 float k34_nz_re = 1;
36 float k34_nz_im = 1;
37 float k35_nz_re = 1;
38 float k35_nz_im = 1;
39 // PE 4 coefficients
40 float k44_pe = 1;
41 float k55_pe = 1;
42 float k45_pe_re = 1;
43 float k45_pe_im = 1;
5
44
6 float k14_re = 1;
7 float k14_im = 1;
8 float k14_bis_re = 1;
9 float k14_bis_im = 1;
10 float k14_tris_re = 1;
11 float k14_tris_im = 1;
12 float k15_re = 1;
13 float k15_im = 1;
14 float k15_bis_re = 1;
15 float k15_bis_im = 1;
16 float k24_re = 1;
17 float k24_im = 1;
18 float k24_bis_re = 1;
19 float k24_bis_im = 1;
20 float k24_tris_re = 1;
21 float k24_tris_im = 1;
22 float k25_re = 1;
23 float k25_im = 1;
24 float k25_bis_re = 1;
25 float k25_bis_im = 1;
26 float k34_re = 1;
27 float k34_im = 1;
28 float k44 = 1;
29 float k55 = 1;
30 float k45_re = 1;
31 float k45_im = 1;
32 float alpha_M = M_PI/4;
45 float alpha_M = M_PI/4;
33
46
34 extern volatile int spec_mat_f0_a[ ];
47 extern volatile int spec_mat_f0_a[ ];
@@ -175,32 +188,32 void BP1_set(float * compressed_spec_mat
175 unsigned char * pt_char;
188 unsigned char * pt_char;
176 float PSDB, PSDE;
189 float PSDB, PSDE;
177 float NVEC_V0, NVEC_V1, NVEC_V2;
190 float NVEC_V0, NVEC_V1, NVEC_V2;
178 float significand;
191 //float significand;
179 int exponent;
192 //int exponent;
180 float aux, tr_SB_SB, tmp;
193 float aux, tr_SB_SB, tmp;
181 float e_cross_b_re, e_cross_b_im;
194 float sx_re, sx_im;
182 float n_cross_e_scal_b_re = 0, n_cross_e_scal_b_im = 0;
195 float nebx_re = 0, nebx_im = 0;
183 float nx = 0, ny = 0;
196 float ny = 0, nz = 0;
184 float bz_bz_star = 0;
197 float bx_bx_star = 0;
185 for(i=0; i<nb_bins_compressed_spec_mat; i++){
198 for(i=0; i<nb_bins_compressed_spec_mat; i++){
186 //==============================================
199 //==============================================
187 // BP1 PSD == B PAR_LFR_SC_BP1_PE_FL0 == 16 bits
200 // BP1 PSD == B PAR_LFR_SC_BP1_PE_FL0 == 16 bits
188 PSDB = compressed_spec_mat[i*30] // S11
201 PSDB = compressed_spec_mat[i*30] // S11
189 + compressed_spec_mat[i*30+10] // S22
202 + compressed_spec_mat[i*30+10] // S22
190 + compressed_spec_mat[i*30+18]; // S33
203 + compressed_spec_mat[i*30+18]; // S33
191 significand = frexp(PSDB, &exponent);
204 //significand = frexp(PSDB, &exponent);
192 pt_char = (unsigned char*) &PSDB;
205 pt_char = (unsigned char*) &PSDB;
193 LFR_BP1[i*9+8] = pt_char[0]; // bits 31 downto 24 of the float
206 LFR_BP1[i*9+2] = pt_char[0]; // bits 31 downto 24 of the float
194 LFR_BP1[i*9+7] = pt_char[1]; // bits 23 downto 16 of the float
207 LFR_BP1[i*9+3] = pt_char[1]; // bits 23 downto 16 of the float
195 //==============================================
208 //==============================================
196 // BP1 PSD == E PAR_LFR_SC_BP1_PB_FL0 == 16 bits
209 // BP1 PSD == E PAR_LFR_SC_BP1_PB_FL0 == 16 bits
197 PSDE = compressed_spec_mat[i*30+24] * k44 // S44
210 PSDE = compressed_spec_mat[i*30+24] * k44_pe // S44
198 + compressed_spec_mat[i*30+28] * k55 // S55
211 + compressed_spec_mat[i*30+28] * k55_pe // S55
199 + compressed_spec_mat[i*30+26] * k45_re // S45
212 + compressed_spec_mat[i*30+26] * k45_pe_re // S45
200 - compressed_spec_mat[i*30+27] * k45_im; // S45
213 - compressed_spec_mat[i*30+27] * k45_pe_im; // S45
201 pt_char = (unsigned char*) &PSDE;
214 pt_char = (unsigned char*) &PSDE;
202 LFR_BP1[i*9+6] = pt_char[0]; // bits 31 downto 24 of the float
215 LFR_BP1[i*9+0] = pt_char[0]; // bits 31 downto 24 of the float
203 LFR_BP1[i*9+5] = pt_char[1]; // bits 23 downto 16 of the float
216 LFR_BP1[i*9+1] = pt_char[1]; // bits 23 downto 16 of the float
204 //==============================================================================
217 //==============================================================================
205 // BP1 normal wave vector == PAR_LFR_SC_BP1_NVEC_V0_F0 == 8 bits
218 // BP1 normal wave vector == PAR_LFR_SC_BP1_NVEC_V0_F0 == 8 bits
206 // == PAR_LFR_SC_BP1_NVEC_V1_F0 == 8 bits
219 // == PAR_LFR_SC_BP1_NVEC_V1_F0 == 8 bits
@@ -208,78 +221,85 void BP1_set(float * compressed_spec_mat
208 tmp = sqrt(
221 tmp = sqrt(
209 compressed_spec_mat[i*30+3]*compressed_spec_mat[i*30+3] //Im S12
222 compressed_spec_mat[i*30+3]*compressed_spec_mat[i*30+3] //Im S12
210 +compressed_spec_mat[i*30+5]*compressed_spec_mat[i*30+5] //Im S13
223 +compressed_spec_mat[i*30+5]*compressed_spec_mat[i*30+5] //Im S13
211 +compressed_spec_mat[i*30+5]*compressed_spec_mat[i*30+13] //Im S23
224 +compressed_spec_mat[i*30+13]*compressed_spec_mat[i*30+13] //Im S23
212 );
225 );
213 NVEC_V0 = compressed_spec_mat[i*30+13] / tmp; // Im S23
226 NVEC_V0 = compressed_spec_mat[i*30+13] / tmp; // Im S23
214 NVEC_V1 = -compressed_spec_mat[i*30+5] / tmp; // Im S13
227 NVEC_V1 = -compressed_spec_mat[i*30+5] / tmp; // Im S13
215 NVEC_V2 = compressed_spec_mat[i*30+1] / tmp; // Im S12
228 NVEC_V2 = compressed_spec_mat[i*30+3] / tmp; // Im S12
216 LFR_BP1[i*9+4] = (char) (NVEC_V0*256);
229 LFR_BP1[i*9+4] = (char) (NVEC_V0*127);
217 LFR_BP1[i*9+3] = (char) (NVEC_V1*256);
230 LFR_BP1[i*9+5] = (char) (NVEC_V1*127);
218 pt_char = (unsigned char*) &NVEC_V2;
231 pt_char = (unsigned char*) &NVEC_V2;
219 LFR_BP1[i*9+2] = pt_char[0] & 0x80; // extract the sign of NVEC_V2
232 LFR_BP1[i*9+6] = pt_char[0] & 0x80; // extract the sign of NVEC_V2
220 //=======================================================
233 //=======================================================
221 // BP1 ellipticity == PAR_LFR_SC_BP1_ELLIP_F0 == 4 bits
234 // BP1 ellipticity == PAR_LFR_SC_BP1_ELLIP_F0 == 4 bits
222 aux = 2*tmp / PSDB; // compute the ellipticity
235 aux = 2*tmp / PSDB; // compute the ellipticity
223 tmp_u_char = (unsigned char) (aux*(16-1)); // convert the ellipticity
236 tmp_u_char = (unsigned char) (aux*(16-1)); // convert the ellipticity
224 LFR_BP1[i*9+2] = LFR_BP1[i*9+2] | ((tmp_u_char&0x0f)<<3); // keeps 4 bits of the resulting unsigned char
237 LFR_BP1[i*9+6] = LFR_BP1[i*9+6] | ((tmp_u_char&0x0f)<<3); // keeps 4 bits of the resulting unsigned char
225 //==============================================================
238 //==============================================================
226 // BP1 degree of polarization == PAR_LFR_SC_BP1_DOP_F0 == 3 bits
239 // BP1 degree of polarization == PAR_LFR_SC_BP1_DOP_F0 == 3 bits
227 for(j = 0; j<NB_VALUES_PER_spec_mat;j++){
240 for(j = 0; j<NB_VALUES_PER_spec_mat;j++){
228 tr_SB_SB = compressed_spec_mat[i*30]*compressed_spec_mat[i*30]
241 tr_SB_SB = compressed_spec_mat[i*30] * compressed_spec_mat[i*30]
229 + compressed_spec_mat[i*30+10]*compressed_spec_mat[i*30+10]
242 + compressed_spec_mat[i*30+10] * compressed_spec_mat[i*30+10]
230 + compressed_spec_mat[i*30+18]*compressed_spec_mat[i*30+18]
243 + compressed_spec_mat[i*30+18] * compressed_spec_mat[i*30+18]
231 + 2 * compressed_spec_mat[i*30+2]*compressed_spec_mat[i*30+2]
244 + 2 * compressed_spec_mat[i*30+2] * compressed_spec_mat[i*30+2]
232 + 2 * compressed_spec_mat[i*30+3]*compressed_spec_mat[i*30+3]
245 + 2 * compressed_spec_mat[i*30+3] * compressed_spec_mat[i*30+3]
233 + 2 * compressed_spec_mat[i*30+4]*compressed_spec_mat[i*30+4]
246 + 2 * compressed_spec_mat[i*30+4] * compressed_spec_mat[i*30+4]
234 + 2 * compressed_spec_mat[i*30+5]*compressed_spec_mat[i*30+5]
247 + 2 * compressed_spec_mat[i*30+5] * compressed_spec_mat[i*30+5]
235 + 2 * compressed_spec_mat[i*30+12]*compressed_spec_mat[i*30+12]
248 + 2 * compressed_spec_mat[i*30+12] * compressed_spec_mat[i*30+12]
236 + 2 * compressed_spec_mat[i*30+13]*compressed_spec_mat[i*30+13];
249 + 2 * compressed_spec_mat[i*30+13] * compressed_spec_mat[i*30+13];
237 }
250 }
238 aux = PSDB*PSDB;
251 aux = PSDB*PSDB;
239 tmp = ( 3*tr_SB_SB - aux ) / ( 2 * aux );
252 tmp = sqrt( abs( ( 3*tr_SB_SB - aux ) / ( 2 * aux ) ) );
240 tmp_u_char = (unsigned char) (NVEC_V0*(8-1));
253 tmp_u_char = (unsigned char) (NVEC_V0*(8-1));
241 LFR_BP1[i*9+2] = LFR_BP1[i*9+2] | ((tmp_u_char&0x07)); // keeps 3 bits of the resulting unsigned char
254 LFR_BP1[i*9+6] = LFR_BP1[i*9+6] | (tmp_u_char & 0x07); // keeps 3 bits of the resulting unsigned char
242 //=======================================================================================
255 //=======================================================================================
243 // BP1 z-component of the normalized Poynting flux == PAR_LFR_SC_BP1_SZ_F0 == 8 bits (7+1)
256 // BP1 x-component of the normalized Poynting flux == PAR_LFR_SC_BP1_SZ_F0 == 8 bits (7+1)
244 e_cross_b_re = compressed_spec_mat[i*30+20]*k34_re
257 sx_re = compressed_spec_mat[i*30+20] * k34_sx_re
245 + compressed_spec_mat[i*30+6]*k14_re
258 + compressed_spec_mat[i*30+6] * k14_sx_re
246 + compressed_spec_mat[i*30+8]*k15_re
259 + compressed_spec_mat[i*30+8] * k15_sx_re
247 + compressed_spec_mat[i*30+14]*k24_re
260 + compressed_spec_mat[i*30+14] * k24_sx_re
248 + compressed_spec_mat[i*30+16]*k25_re;
261 + compressed_spec_mat[i*30+16] * k25_sx_re
249 e_cross_b_im = compressed_spec_mat[i*30+21]*k34_im
262 + compressed_spec_mat[i*30+22] * k35_sx_re;
250 + compressed_spec_mat[i*30+7]*k14_im
263 sx_im = compressed_spec_mat[i*30+21] * k34_sx_im
251 + compressed_spec_mat[i*30+9]*k15_im
264 + compressed_spec_mat[i*30+7] * k14_sx_im
252 + compressed_spec_mat[i*30+15]*k24_im
265 + compressed_spec_mat[i*30+9] * k15_sx_im
253 + compressed_spec_mat[i*30+17]*k25_im;
266 + compressed_spec_mat[i*30+15] * k24_sx_im
254 tmp = e_cross_b_re / PSDE; // compute ReaSz
267 + compressed_spec_mat[i*30+17] * k25_sx_im
255 LFR_BP1[i*9+1] = ((unsigned char) (tmp * 128)) & 0x7f; // is it always positive?
268 + compressed_spec_mat[i*30+23] * k35_sx_im;
256 tmp = e_cross_b_re * e_cross_b_im;
269 LFR_BP1[i*9+7] = ((unsigned char) (sx_re * 128)) & 0x7f; // cf DOC for the compression
257 pt_char = (unsigned char*) &tmp;
270 if ( abs(sx_re) > abs(sx_im) )
258 LFR_BP1[i*9+1] = LFR_BP1[i*9+1] | (pt_char[0] & 0x80); // extract the sign of ArgSz
271 LFR_BP1[i*9+7] = LFR_BP1[i*9+1] | (0x80); // extract the sector of sx
272 else
273 LFR_BP1[i*9+7] = LFR_BP1[i*9+1] & (0x7f); // extract the sector of sx
259 //======================================================================
274 //======================================================================
260 // BP1 phase velocity estimator == PAR_LFR_SC_BP1_VPHI_F0 == 8 bits (7+1)
275 // BP1 phase velocity estimator == PAR_LFR_SC_BP1_VPHI_F0 == 8 bits (7+1)
261 nx = -sin(alpha_M)*NVEC_V0 - cos(alpha_M)*NVEC_V1;
276 ny = sin(alpha_M)*NVEC_V1 + cos(alpha_M)*NVEC_V2;
262 ny = NVEC_V2;
277 nz = NVEC_V0;
263 bz_bz_star = cos(alpha_M) * cos(alpha_M) * compressed_spec_mat[i*30] // re S11
278 bx_bx_star = cos(alpha_M) * cos(alpha_M) * compressed_spec_mat[i*30+10] // re S22
264 + sin(alpha_M) * sin(alpha_M) * compressed_spec_mat[i*30+10] // re S22
279 + sin(alpha_M) * sin(alpha_M) * compressed_spec_mat[i*30+18] // re S33
265 - 2 * sin(alpha_M) * cos(alpha_M) * compressed_spec_mat[i*30+2]; // re S12
280 - 2 * sin(alpha_M) * cos(alpha_M) * compressed_spec_mat[i*30+12]; // re S23
266 n_cross_e_scal_b_re = nx * (compressed_spec_mat[i*30+8]*k15_bis_re
281 nebx_re = ny * (compressed_spec_mat[i*30+14] * k24_ny_re
267 +compressed_spec_mat[i*30+6]*k14_bis_re
282 +compressed_spec_mat[i*30+16] * k25_ny_re
268 +compressed_spec_mat[i*30+16]*k25_bis_re
283 +compressed_spec_mat[i*30+20] * k34_ny_re
269 +compressed_spec_mat[i*30+14]*k24_bis_re)
284 +compressed_spec_mat[i*30+22] * k35_ny_re)
270 + ny * (compressed_spec_mat[i*30+6]*k14_tris_re
285 + nz * (compressed_spec_mat[i*30+14] * k24_nz_re
271 +compressed_spec_mat[i*30+14]*k24_tris_re);
286 +compressed_spec_mat[i*30+16] * k25_nz_re
272 n_cross_e_scal_b_im = nx * (compressed_spec_mat[i*30+8]*k15_bis_im
287 +compressed_spec_mat[i*30+20] * k34_nz_re
273 +compressed_spec_mat[i*30+6]*k14_bis_im
288 +compressed_spec_mat[i*30+22] * k35_nz_re);
274 +compressed_spec_mat[i*30+16]*k25_bis_im
289 nebx_im = ny * (compressed_spec_mat[i*30+15]*k24_ny_re
275 +compressed_spec_mat[i*30+14]*k24_bis_im)
290 +compressed_spec_mat[i*30+17] * k25_ny_re
276 + ny * (compressed_spec_mat[i*30+6]*k14_tris_im
291 +compressed_spec_mat[i*30+21] * k34_ny_re
277 +compressed_spec_mat[i*30+14]*k24_tris_im);
292 +compressed_spec_mat[i*30+23] * k35_ny_re)
278 tmp = n_cross_e_scal_b_re / bz_bz_star;
293 + nz * (compressed_spec_mat[i*30+15] * k24_nz_im
279 LFR_BP1[i*9+0] = ((unsigned char) (tmp * 128)) & 0x7f; // is it always positive?
294 +compressed_spec_mat[i*30+17] * k25_nz_im
280 tmp = n_cross_e_scal_b_re * n_cross_e_scal_b_im;
295 +compressed_spec_mat[i*30+21] * k34_nz_im
281 pt_char = (unsigned char*) &tmp;
296 +compressed_spec_mat[i*30+23] * k35_nz_im);
282 LFR_BP1[i*9+1] = LFR_BP1[i*9+0] | (pt_char[0] & 0x80); // extract the sign of ArgV
297 tmp = nebx_re / bx_bx_star;
298 LFR_BP1[i*9+8] = ((unsigned char) (tmp * 128)) & 0x7f; // cf DOC for the compression
299 if ( abs(nebx_re) > abs(nebx_im) )
300 LFR_BP1[i*9+8] = LFR_BP1[i*9+8] | (0x80); // extract the sector of nebx
301 else
302 LFR_BP1[i*9+8] = LFR_BP1[i*9+8] & (0x7f); // extract the sector of nebx
283 }
303 }
284
304
285 }
305 }
@@ -289,7 +289,7 rtems_task recv_task( rtems_task_argumen
289 PRINTF("In RECV *** Error: rtems_task_suspend(RTEMS_SELF)\n")
289 PRINTF("In RECV *** Error: rtems_task_suspend(RTEMS_SELF)\n")
290 }
290 }
291 else {
291 else {
292 //PRINTF1("In RECV *** Got Message of length %d\n", len)
292 PRINTF1("In RECV *** Got Message of length %d\n", len)
293 currentTC_LEN_RCV[0] = 0x00;
293 currentTC_LEN_RCV[0] = 0x00;
294 currentTC_LEN_RCV[1] = (unsigned char) len - CCSDS_TC_TM_PACKET_OFFSET - 3; // build the corresponding packet size field
294 currentTC_LEN_RCV[1] = (unsigned char) len - CCSDS_TC_TM_PACKET_OFFSET - 3; // build the corresponding packet size field
295 currentTC_LEN_RCV_AsUnsignedInt = (unsigned int) len - CCSDS_TC_TM_PACKET_OFFSET - 3; // => -3 is for Prot ID, Reserved and User App bytes
295 currentTC_LEN_RCV_AsUnsignedInt = (unsigned int) len - CCSDS_TC_TM_PACKET_OFFSET - 3; // => -3 is for Prot ID, Reserved and User App bytes
@@ -456,7 +456,7 int action_updt_time(ccsdsTelecommandPac
456 + (TC->dataAndCRC[1] << 16)
456 + (TC->dataAndCRC[1] << 16)
457 + (TC->dataAndCRC[2] << 8)
457 + (TC->dataAndCRC[2] << 8)
458 + TC->dataAndCRC[3];
458 + TC->dataAndCRC[3];
459 time_management_regs->ctrl = time_management_regs->ctrl | 1;
459 //time_management_regs->ctrl = time_management_regs->ctrl | 1;
460 return 0;
460 return 0;
461 }
461 }
462
462
@@ -486,4 +486,35 int send_tm_lfr_tc_exe_success(ccsdsTele
486 return 0;
486 return 0;
487 }
487 }
488
488
489 //***************************
490 // Interrupt Service Routines
491 rtems_isr commutation_isr1( rtems_vector_number vector )
492 {
493 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
494 printf("In spectral_matrices_isr *** Error sending event to DUMB\n");
495 }
489
496
497 rtems_isr commutation_isr2( rtems_vector_number vector )
498 {
499 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
500 printf("In spectral_matrices_isr *** Error sending event to DUMB\n");
501 }
502
503 rtems_task dumb_task( rtems_task_argument unused )
504 {
505 unsigned int coarse_time;
506 unsigned int fine_time;
507 rtems_event_set event_out;
508
509 PRINTF("In DUMB *** \n")
510
511 while(1){
512 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
513 coarse_time = time_management_regs->coarse_time;
514 fine_time = time_management_regs->fine_time;
515 printf("commutation_isr*, coarse time = %x, fine time = %x\n", coarse_time, fine_time);
516 }
517 }
518
519
520
@@ -30,12 +30,6 rtems_task wfrm_task(rtems_task_argument
30 header.dataFieldHeader[1] = 0x15; // service type
30 header.dataFieldHeader[1] = 0x15; // service type
31 header.dataFieldHeader[2] = 0x03; // service subtype
31 header.dataFieldHeader[2] = 0x03; // service subtype
32 header.dataFieldHeader[3] = CCSDS_DESTINATION_ID;
32 header.dataFieldHeader[3] = CCSDS_DESTINATION_ID;
33 header.dataFieldHeader[4] = 0xaa;
34 header.dataFieldHeader[5] = 0xbb;
35 header.dataFieldHeader[6] = 0xcc;
36 header.dataFieldHeader[7] = 0xdd;
37 header.dataFieldHeader[8] = 0xee;
38 header.dataFieldHeader[9] = 0xff;
39
33
40 header.auxiliaryHeader[0] = 0x00;
34 header.auxiliaryHeader[0] = 0x00;
41 header.auxiliaryHeader[1] = 0x1f;
35 header.auxiliaryHeader[1] = 0x1f;
@@ -52,6 +46,12 rtems_task wfrm_task(rtems_task_argument
52
46
53 while(1){
47 while(1){
54 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
48 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
49 header.dataFieldHeader[4] = (unsigned char) (time_management_regs->coarse_time>>24);
50 header.dataFieldHeader[5] = (unsigned char) (time_management_regs->coarse_time>>16);
51 header.dataFieldHeader[6] = (unsigned char) (time_management_regs->coarse_time>>8);
52 header.dataFieldHeader[7] = (unsigned char) (time_management_regs->coarse_time);
53 header.dataFieldHeader[8] = (unsigned char) (time_management_regs->fine_time>>8);
54 header.dataFieldHeader[9] = (unsigned char) (time_management_regs->fine_time);
55 for (i=0; i<7; i++) // send F0
55 for (i=0; i<7; i++) // send F0
56 {
56 {
57 // BUILD THE DATA
57 // BUILD THE DATA
General Comments 0
You need to be logged in to leave comments. Login now