@@ -1,46 +1,46 | |||
|
1 | 1 | <?xml version="1.0" encoding="UTF-8" standalone="yes" ?> |
|
2 | 2 | <CodeBlocks_layout_file> |
|
3 | 3 | <ActiveTarget name="Debug" /> |
|
4 |
<File name="../header/ccsds_types.h" open=" |
|
|
4 | <File name="../header/ccsds_types.h" open="0" top="0" tabpos="3"> | |
|
5 | 5 | <Cursor position="3403" topLine="101" /> |
|
6 | 6 | </File> |
|
7 | 7 | <File name="../header/fsw_init.h" open="0" top="0" tabpos="3"> |
|
8 |
<Cursor position="8 |
|
|
8 | <Cursor position="378" topLine="0" /> | |
|
9 | 9 | </File> |
|
10 | 10 | <File name="../header/fsw_misc.h" open="0" top="0" tabpos="0"> |
|
11 | 11 | <Cursor position="114" topLine="0" /> |
|
12 | 12 | </File> |
|
13 |
<File name="../header/fsw_params.h" open="0" top="0" tabpos=" |
|
|
14 |
<Cursor position=" |
|
|
13 | <File name="../header/fsw_params.h" open="0" top="0" tabpos="2"> | |
|
14 | <Cursor position="1358" topLine="31" /> | |
|
15 | 15 | </File> |
|
16 | 16 | <File name="../header/fsw_processing.h" open="0" top="0" tabpos="0"> |
|
17 |
<Cursor position=" |
|
|
17 | <Cursor position="628" topLine="7" /> | |
|
18 | 18 | </File> |
|
19 |
<File name="../header/grlib_regs.h" open="0" top="0" tabpos=" |
|
|
20 |
<Cursor position="1 |
|
|
19 | <File name="../header/grlib_regs.h" open="0" top="0" tabpos="4"> | |
|
20 | <Cursor position="1153" topLine="11" /> | |
|
21 | 21 | </File> |
|
22 | 22 | <File name="../header/tc_handler.h" open="0" top="0" tabpos="4"> |
|
23 |
<Cursor position="4 |
|
|
23 | <Cursor position="624" topLine="2" /> | |
|
24 | 24 | </File> |
|
25 | 25 | <File name="../header/wf_handler.h" open="0" top="0" tabpos="0"> |
|
26 |
<Cursor position=" |
|
|
26 | <Cursor position="318" topLine="0" /> | |
|
27 | 27 | </File> |
|
28 |
<File name="../src/fsw_globals.c" open="0" top="0" tabpos=" |
|
|
29 |
<Cursor position=" |
|
|
28 | <File name="../src/fsw_globals.c" open="0" top="0" tabpos="3"> | |
|
29 | <Cursor position="378" topLine="1" /> | |
|
30 | 30 | </File> |
|
31 | 31 | <File name="../src/fsw_init.c" open="1" top="0" tabpos="1"> |
|
32 |
<Cursor position=" |
|
|
32 | <Cursor position="2303" topLine="54" /> | |
|
33 | 33 | </File> |
|
34 |
<File name="../src/fsw_misc.c" open="0" top="0" tabpos=" |
|
|
35 |
<Cursor position=" |
|
|
34 | <File name="../src/fsw_misc.c" open="0" top="0" tabpos="2"> | |
|
35 | <Cursor position="475" topLine="0" /> | |
|
36 | 36 | </File> |
|
37 |
<File name="../src/fsw_processing.c" open="0" top="0" tabpos=" |
|
|
38 |
<Cursor position="1 |
|
|
37 | <File name="../src/fsw_processing.c" open="0" top="0" tabpos="0"> | |
|
38 | <Cursor position="1696" topLine="59" /> | |
|
39 | 39 | </File> |
|
40 | 40 | <File name="../src/tc_handler.c" open="1" top="1" tabpos="2"> |
|
41 |
<Cursor position="1 |
|
|
41 | <Cursor position="17607" topLine="36" /> | |
|
42 | 42 | </File> |
|
43 |
<File name="../src/wf_handler.c" open="0" top="0" tabpos=" |
|
|
44 |
<Cursor position=" |
|
|
43 | <File name="../src/wf_handler.c" open="0" top="0" tabpos="4"> | |
|
44 | <Cursor position="1660" topLine="24" /> | |
|
45 | 45 | </File> |
|
46 | 46 | </CodeBlocks_layout_file> |
|
1 | NO CONTENT: modified file, binary diff hidden |
@@ -22,10 +22,16 | |||
|
22 | 22 | |
|
23 | 23 | #define APBUART_CTRL_REG_MASK_DB 0xfffff7ff |
|
24 | 24 | |
|
25 | //********** | |
|
26 | // IRQ LINES | |
|
25 | 27 | #define IRQ_SM 9 |
|
26 | 28 | #define IRQ_SPARC_SM 0x19 // see sparcv8.pdf p.76 for interrupt levels |
|
27 | 29 | #define IRQ_WF 10 |
|
28 | 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 | 36 | #define CLKDIV_SM_SIMULATOR 9999 |
|
31 | 37 | #define CLKDIV_WF_SIMULATOR 9999999 |
@@ -42,11 +48,13 | |||
|
42 | 48 | #define TASKID_AVF0 6 |
|
43 | 49 | #define TASKID_BPF0 7 |
|
44 | 50 | #define TASKID_WFRM 8 |
|
51 | #define TASKID_DUMB 9 | |
|
45 | 52 | |
|
46 | 53 | #define ACTION_MSG_QUEUE_COUNT 10 |
|
47 | 54 | |
|
48 | 55 | //******* |
|
49 | 56 | // MACROS |
|
57 | //#define PRINT_TASK_STATISTICS | |
|
50 | 58 | #define PRINT_MESSAGES_ON_CONSOLE // enable or disable the printf instructions |
|
51 | 59 | #ifdef PRINT_MESSAGES_ON_CONSOLE |
|
52 | 60 | #define PRINTF(x) printf(x); |
@@ -27,6 +27,11 unsigned int currentTC_LEN_RCV_AsUnsigne | |||
|
27 | 27 | unsigned int currentTM_length; |
|
28 | 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 | 36 | // GENERAL USE FUNCTIONS |
|
32 | 37 | unsigned int lookUpTableForCRC[256]; |
@@ -46,6 +51,7 int TC_checker(ccsdsTelecommandPacket_t | |||
|
46 | 51 | // RTEMS TASK |
|
47 | 52 | rtems_task recv_task( rtems_task_argument unused ); |
|
48 | 53 | rtems_task actn_task( rtems_task_argument unused ); |
|
54 | rtems_task dumb_task( rtems_task_argument unused ); | |
|
49 | 55 | int create_message_queue(); |
|
50 | 56 | |
|
51 | 57 | //*********** |
@@ -57,6 +57,7 char *lstates[6] = {"Error-reset", | |||
|
57 | 57 | rtems_task Init( rtems_task_argument ignored ) |
|
58 | 58 | { |
|
59 | 59 | rtems_status_code status; |
|
60 | rtems_isr_entry old_isr_handler; | |
|
60 | 61 | |
|
61 | 62 | //send_console_outputs_on_serial_port(); |
|
62 | 63 | |
@@ -72,6 +73,22 rtems_task Init( rtems_task_argument ign | |||
|
72 | 73 | configure_timer((gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_WF_SIMULATOR, CLKDIV_WF_SIMULATOR, |
|
73 | 74 | IRQ_SPARC_WF, waveforms_isr ); |
|
74 | 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 );*/ | |
|
91 | ||
|
75 | 92 | status = rtems_task_delete(RTEMS_SELF); |
|
76 | 93 | } |
|
77 | 94 | |
@@ -129,6 +146,7 int create_all_tasks() | |||
|
129 | 146 | Task_name[6] = rtems_build_name( 'A', 'V', 'F', '0' ); |
|
130 | 147 | Task_name[7] = rtems_build_name( 'B', 'P', 'F', '0' ); |
|
131 | 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 | 151 | // RECV |
|
134 | 152 | status = rtems_task_create( |
@@ -156,7 +174,7 int create_all_tasks() | |||
|
156 | 174 | ); |
|
157 | 175 | // STAT |
|
158 | 176 | status = rtems_task_create( |
|
159 |
Task_name[5], |
|
|
177 | Task_name[5], 150, RTEMS_MINIMUM_STACK_SIZE * 2, | |
|
160 | 178 | RTEMS_DEFAULT_MODES, |
|
161 | 179 | RTEMS_DEFAULT_ATTRIBUTES, &Task_id[5] |
|
162 | 180 | ); |
@@ -178,6 +196,12 int create_all_tasks() | |||
|
178 | 196 | RTEMS_DEFAULT_MODES, |
|
179 | 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 | 206 | return 0; |
|
183 | 207 | } |
@@ -210,6 +234,9 int start_all_tasks() | |||
|
210 | 234 | status = rtems_task_start( Task_id[8], wfrm_task, 1 ); |
|
211 | 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 | 240 | return 0; |
|
214 | 241 | } |
|
215 | 242 |
@@ -62,8 +62,10 rtems_task stat_task(rtems_task_argument | |||
|
62 | 62 | rtems_task_wake_after(1000); |
|
63 | 63 | PRINTF1("%d\n", i) |
|
64 | 64 | if (i == 2) { |
|
65 | #ifdef PRINT_TASK_STATISTICS | |
|
65 | 66 | rtems_cpu_usage_report(); |
|
66 | 67 | rtems_cpu_usage_reset(); |
|
68 | #endif | |
|
67 | 69 | i = 0; |
|
68 | 70 | } |
|
69 | 71 | else i++; |
@@ -1,34 +1,47 | |||
|
1 | 1 | #include <fsw_processing.h> |
|
2 | 2 | #include <math.h> |
|
3 | 3 | #include <stdio.h> |
|
4 | #include <stdlib.h> | |
|
4 | 5 | #include <leon.h> |
|
5 | 6 | |
|
6 | float k14_re = 1; | |
|
7 | float k14_im = 1; | |
|
8 |
float k14_ |
|
|
9 |
float k14_ |
|
|
10 |
float k1 |
|
|
11 |
float k1 |
|
|
12 |
float k |
|
|
13 |
float k |
|
|
14 |
float k |
|
|
15 |
float k |
|
|
16 |
float k |
|
|
17 |
float k |
|
|
18 |
float k |
|
|
19 |
float k |
|
|
20 | float k24_tris_re = 1; | |
|
21 |
float k24_ |
|
|
22 |
float k2 |
|
|
23 |
float k25_ |
|
|
24 |
float k25_ |
|
|
25 |
float k |
|
|
26 |
float k34_ |
|
|
27 |
float k3 |
|
|
28 |
float k |
|
|
29 | float k55 = 1; | |
|
30 |
float k |
|
|
31 |
float k |
|
|
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; | |
|
44 | ||
|
32 | 45 | float alpha_M = M_PI/4; |
|
33 | 46 | |
|
34 | 47 | extern volatile int spec_mat_f0_a[ ]; |
@@ -175,32 +188,32 void BP1_set(float * compressed_spec_mat | |||
|
175 | 188 | unsigned char * pt_char; |
|
176 | 189 | float PSDB, PSDE; |
|
177 | 190 | float NVEC_V0, NVEC_V1, NVEC_V2; |
|
178 | float significand; | |
|
179 | int exponent; | |
|
191 | //float significand; | |
|
192 | //int exponent; | |
|
180 | 193 | float aux, tr_SB_SB, tmp; |
|
181 |
float |
|
|
182 |
float n |
|
|
183 |
float n |
|
|
184 |
float b |
|
|
194 | float sx_re, sx_im; | |
|
195 | float nebx_re = 0, nebx_im = 0; | |
|
196 | float ny = 0, nz = 0; | |
|
197 | float bx_bx_star = 0; | |
|
185 | 198 | for(i=0; i<nb_bins_compressed_spec_mat; i++){ |
|
186 | 199 | //============================================== |
|
187 | 200 | // BP1 PSD == B PAR_LFR_SC_BP1_PE_FL0 == 16 bits |
|
188 | 201 | PSDB = compressed_spec_mat[i*30] // S11 |
|
189 | 202 | + compressed_spec_mat[i*30+10] // S22 |
|
190 | 203 | + compressed_spec_mat[i*30+18]; // S33 |
|
191 | significand = frexp(PSDB, &exponent); | |
|
204 | //significand = frexp(PSDB, &exponent); | |
|
192 | 205 | pt_char = (unsigned char*) &PSDB; |
|
193 |
LFR_BP1[i*9+ |
|
|
194 |
LFR_BP1[i*9+ |
|
|
206 | LFR_BP1[i*9+2] = pt_char[0]; // bits 31 downto 24 of the float | |
|
207 | LFR_BP1[i*9+3] = pt_char[1]; // bits 23 downto 16 of the float | |
|
195 | 208 | //============================================== |
|
196 | 209 | // BP1 PSD == E PAR_LFR_SC_BP1_PB_FL0 == 16 bits |
|
197 | PSDE = compressed_spec_mat[i*30+24] * k44 // S44 | |
|
198 | + compressed_spec_mat[i*30+28] * k55 // S55 | |
|
199 | + compressed_spec_mat[i*30+26] * k45_re // S45 | |
|
200 | - compressed_spec_mat[i*30+27] * k45_im; // S45 | |
|
210 | PSDE = compressed_spec_mat[i*30+24] * k44_pe // S44 | |
|
211 | + compressed_spec_mat[i*30+28] * k55_pe // S55 | |
|
212 | + compressed_spec_mat[i*30+26] * k45_pe_re // S45 | |
|
213 | - compressed_spec_mat[i*30+27] * k45_pe_im; // S45 | |
|
201 | 214 | pt_char = (unsigned char*) &PSDE; |
|
202 |
LFR_BP1[i*9+ |
|
|
203 |
LFR_BP1[i*9+ |
|
|
215 | LFR_BP1[i*9+0] = pt_char[0]; // bits 31 downto 24 of the float | |
|
216 | LFR_BP1[i*9+1] = pt_char[1]; // bits 23 downto 16 of the float | |
|
204 | 217 | //============================================================================== |
|
205 | 218 | // BP1 normal wave vector == PAR_LFR_SC_BP1_NVEC_V0_F0 == 8 bits |
|
206 | 219 | // == PAR_LFR_SC_BP1_NVEC_V1_F0 == 8 bits |
@@ -208,20 +221,20 void BP1_set(float * compressed_spec_mat | |||
|
208 | 221 | tmp = sqrt( |
|
209 | 222 | compressed_spec_mat[i*30+3]*compressed_spec_mat[i*30+3] //Im S12 |
|
210 | 223 | +compressed_spec_mat[i*30+5]*compressed_spec_mat[i*30+5] //Im S13 |
|
211 |
+compressed_spec_mat[i*30+ |
|
|
224 | +compressed_spec_mat[i*30+13]*compressed_spec_mat[i*30+13] //Im S23 | |
|
212 | 225 | ); |
|
213 | 226 | NVEC_V0 = compressed_spec_mat[i*30+13] / tmp; // Im S23 |
|
214 | 227 | NVEC_V1 = -compressed_spec_mat[i*30+5] / tmp; // Im S13 |
|
215 |
NVEC_V2 = compressed_spec_mat[i*30+ |
|
|
216 |
LFR_BP1[i*9+4] = (char) (NVEC_V0* |
|
|
217 |
LFR_BP1[i*9+ |
|
|
228 | NVEC_V2 = compressed_spec_mat[i*30+3] / tmp; // Im S12 | |
|
229 | LFR_BP1[i*9+4] = (char) (NVEC_V0*127); | |
|
230 | LFR_BP1[i*9+5] = (char) (NVEC_V1*127); | |
|
218 | 231 | pt_char = (unsigned char*) &NVEC_V2; |
|
219 |
LFR_BP1[i*9+ |
|
|
232 | LFR_BP1[i*9+6] = pt_char[0] & 0x80; // extract the sign of NVEC_V2 | |
|
220 | 233 | //======================================================= |
|
221 | 234 | // BP1 ellipticity == PAR_LFR_SC_BP1_ELLIP_F0 == 4 bits |
|
222 | 235 | aux = 2*tmp / PSDB; // compute the ellipticity |
|
223 | 236 | tmp_u_char = (unsigned char) (aux*(16-1)); // convert the ellipticity |
|
224 |
LFR_BP1[i*9+ |
|
|
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 | 239 | // BP1 degree of polarization == PAR_LFR_SC_BP1_DOP_F0 == 3 bits |
|
227 | 240 | for(j = 0; j<NB_VALUES_PER_spec_mat;j++){ |
@@ -236,50 +249,57 void BP1_set(float * compressed_spec_mat | |||
|
236 | 249 | + 2 * compressed_spec_mat[i*30+13]*compressed_spec_mat[i*30+13]; |
|
237 | 250 | } |
|
238 | 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 | 253 | tmp_u_char = (unsigned char) (NVEC_V0*(8-1)); |
|
241 |
LFR_BP1[i*9+ |
|
|
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 |
|
|
244 |
|
|
|
245 | + compressed_spec_mat[i*30+6]*k14_re | |
|
246 | + compressed_spec_mat[i*30+8]*k15_re | |
|
247 | + compressed_spec_mat[i*30+14]*k24_re | |
|
248 |
+ compressed_spec_mat[i*30+16]*k25_re |
|
|
249 |
|
|
|
250 |
|
|
|
251 |
+ compressed_spec_mat[i*30+ |
|
|
252 |
+ compressed_spec_mat[i*30+ |
|
|
253 |
+ compressed_spec_mat[i*30+1 |
|
|
254 | tmp = e_cross_b_re / PSDE; // compute ReaSz | |
|
255 | LFR_BP1[i*9+1] = ((unsigned char) (tmp * 128)) & 0x7f; // is it always positive? | |
|
256 | tmp = e_cross_b_re * e_cross_b_im; | |
|
257 | pt_char = (unsigned char*) &tmp; | |
|
258 |
LFR_BP1[i*9+ |
|
|
256 | // BP1 x-component of the normalized Poynting flux == PAR_LFR_SC_BP1_SZ_F0 == 8 bits (7+1) | |
|
257 | sx_re = compressed_spec_mat[i*30+20] * k34_sx_re | |
|
258 | + compressed_spec_mat[i*30+6] * k14_sx_re | |
|
259 | + compressed_spec_mat[i*30+8] * k15_sx_re | |
|
260 | + compressed_spec_mat[i*30+14] * k24_sx_re | |
|
261 | + compressed_spec_mat[i*30+16] * k25_sx_re | |
|
262 | + compressed_spec_mat[i*30+22] * k35_sx_re; | |
|
263 | sx_im = compressed_spec_mat[i*30+21] * k34_sx_im | |
|
264 | + compressed_spec_mat[i*30+7] * k14_sx_im | |
|
265 | + compressed_spec_mat[i*30+9] * k15_sx_im | |
|
266 | + compressed_spec_mat[i*30+15] * k24_sx_im | |
|
267 | + compressed_spec_mat[i*30+17] * k25_sx_im | |
|
268 | + compressed_spec_mat[i*30+23] * k35_sx_im; | |
|
269 | LFR_BP1[i*9+7] = ((unsigned char) (sx_re * 128)) & 0x7f; // cf DOC for the compression | |
|
270 | if ( abs(sx_re) > abs(sx_im) ) | |
|
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 | 275 | // BP1 phase velocity estimator == PAR_LFR_SC_BP1_VPHI_F0 == 8 bits (7+1) |
|
261 |
n |
|
|
262 |
n |
|
|
263 |
b |
|
|
264 |
+ sin(alpha_M) * sin(alpha_M) * compressed_spec_mat[i*30+1 |
|
|
265 |
- 2 * sin(alpha_M) * cos(alpha_M) * compressed_spec_mat[i*30+2]; // re S |
|
|
266 |
n |
|
|
267 |
+compressed_spec_mat[i*30+6]* |
|
|
268 |
+compressed_spec_mat[i*30+ |
|
|
269 |
+compressed_spec_mat[i*30+ |
|
|
270 |
+ n |
|
|
271 |
+compressed_spec_mat[i*30+1 |
|
|
272 | n_cross_e_scal_b_im = nx * (compressed_spec_mat[i*30+8]*k15_bis_im | |
|
273 |
+compressed_spec_mat[i*30+ |
|
|
274 |
|
|
|
275 |
+compressed_spec_mat[i*30+1 |
|
|
276 |
+ |
|
|
277 |
+compressed_spec_mat[i*30+ |
|
|
278 | tmp = n_cross_e_scal_b_re / bz_bz_star; | |
|
279 | LFR_BP1[i*9+0] = ((unsigned char) (tmp * 128)) & 0x7f; // is it always positive? | |
|
280 | tmp = n_cross_e_scal_b_re * n_cross_e_scal_b_im; | |
|
281 | pt_char = (unsigned char*) &tmp; | |
|
282 | LFR_BP1[i*9+1] = LFR_BP1[i*9+0] | (pt_char[0] & 0x80); // extract the sign of ArgV | |
|
276 | ny = sin(alpha_M)*NVEC_V1 + cos(alpha_M)*NVEC_V2; | |
|
277 | nz = NVEC_V0; | |
|
278 | bx_bx_star = cos(alpha_M) * cos(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 | |
|
280 | - 2 * sin(alpha_M) * cos(alpha_M) * compressed_spec_mat[i*30+12]; // re S23 | |
|
281 | nebx_re = ny * (compressed_spec_mat[i*30+14] * k24_ny_re | |
|
282 | +compressed_spec_mat[i*30+16] * k25_ny_re | |
|
283 | +compressed_spec_mat[i*30+20] * k34_ny_re | |
|
284 | +compressed_spec_mat[i*30+22] * k35_ny_re) | |
|
285 | + nz * (compressed_spec_mat[i*30+14] * k24_nz_re | |
|
286 | +compressed_spec_mat[i*30+16] * k25_nz_re | |
|
287 | +compressed_spec_mat[i*30+20] * k34_nz_re | |
|
288 | +compressed_spec_mat[i*30+22] * k35_nz_re); | |
|
289 | nebx_im = ny * (compressed_spec_mat[i*30+15]*k24_ny_re | |
|
290 | +compressed_spec_mat[i*30+17] * k25_ny_re | |
|
291 | +compressed_spec_mat[i*30+21] * k34_ny_re | |
|
292 | +compressed_spec_mat[i*30+23] * k35_ny_re) | |
|
293 | + nz * (compressed_spec_mat[i*30+15] * k24_nz_im | |
|
294 | +compressed_spec_mat[i*30+17] * k25_nz_im | |
|
295 | +compressed_spec_mat[i*30+21] * k34_nz_im | |
|
296 | +compressed_spec_mat[i*30+23] * k35_nz_im); | |
|
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 | 289 | PRINTF("In RECV *** Error: rtems_task_suspend(RTEMS_SELF)\n") |
|
290 | 290 | } |
|
291 | 291 | else { |
|
292 |
|
|
|
292 | PRINTF1("In RECV *** Got Message of length %d\n", len) | |
|
293 | 293 | currentTC_LEN_RCV[0] = 0x00; |
|
294 | 294 | currentTC_LEN_RCV[1] = (unsigned char) len - CCSDS_TC_TM_PACKET_OFFSET - 3; // build the corresponding packet size field |
|
295 | 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 | 456 | + (TC->dataAndCRC[1] << 16) |
|
457 | 457 | + (TC->dataAndCRC[2] << 8) |
|
458 | 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 | 460 | return 0; |
|
461 | 461 | } |
|
462 | 462 | |
@@ -486,4 +486,35 int send_tm_lfr_tc_exe_success(ccsdsTele | |||
|
486 | 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 | 30 | header.dataFieldHeader[1] = 0x15; // service type |
|
31 | 31 | header.dataFieldHeader[2] = 0x03; // service subtype |
|
32 | 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 | 34 | header.auxiliaryHeader[0] = 0x00; |
|
41 | 35 | header.auxiliaryHeader[1] = 0x1f; |
@@ -52,6 +46,12 rtems_task wfrm_task(rtems_task_argument | |||
|
52 | 46 | |
|
53 | 47 | while(1){ |
|
54 | 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 | 55 | for (i=0; i<7; i++) // send F0 |
|
56 | 56 | { |
|
57 | 57 | // BUILD THE DATA |
General Comments 0
You need to be logged in to leave comments.
Login now