@@ -1,54 +1,60 | |||||
1 | <?xml version="1.0" encoding="UTF-8" standalone="yes" ?> |
|
1 | <?xml version="1.0" encoding="UTF-8" standalone="yes" ?> | |
2 | <CodeBlocks_project_file> |
|
2 | <CodeBlocks_project_file> | |
3 | <FileVersion major="1" minor="6" /> |
|
3 | <FileVersion major="1" minor="6" /> | |
4 | <Project> |
|
4 | <Project> | |
5 | <Option title="FSW-rtems" /> |
|
5 | <Option title="FSW-rtems" /> | |
6 | <Option pch_mode="2" /> |
|
6 | <Option pch_mode="2" /> | |
7 | <Option compiler="sparc_rtems_gcc" /> |
|
7 | <Option compiler="sparc_rtems_gcc" /> | |
8 | <Build> |
|
8 | <Build> | |
9 | <Target title="Debug"> |
|
9 | <Target title="Debug"> | |
10 | <Option output="bin/Debug/fsw" prefix_auto="1" extension_auto="1" /> |
|
10 | <Option output="bin/Debug/fsw" prefix_auto="1" extension_auto="1" /> | |
11 | <Option object_output="obj/Debug/" /> |
|
11 | <Option object_output="obj/Debug/" /> | |
12 | <Option type="1" /> |
|
12 | <Option type="1" /> | |
13 | <Option compiler="sparc_rtems_gcc_compiler" /> |
|
13 | <Option compiler="sparc_rtems_gcc_compiler" /> | |
14 | <Compiler> |
|
14 | <Compiler> | |
|
15 | <Add option="-fexpensive-optimizations" /> | |||
15 | <Add option="-O3" /> |
|
16 | <Add option="-O3" /> | |
16 | <Add option="-g" /> |
|
17 | <Add option="-g" /> | |
17 | <Add directory="../header" /> |
|
18 | <Add directory="../header" /> | |
18 | <Add directory="../src" /> |
|
19 | <Add directory="../src" /> | |
19 | </Compiler> |
|
20 | </Compiler> | |
20 | </Target> |
|
21 | </Target> | |
21 | <Target title="Release"> |
|
22 | <Target title="Release"> | |
22 | <Option output="bin/Release/FSW-rtems" prefix_auto="1" extension_auto="1" /> |
|
23 | <Option output="bin/Release/FSW-rtems" prefix_auto="1" extension_auto="1" /> | |
23 | <Option object_output="obj/Release/" /> |
|
24 | <Option object_output="obj/Release/" /> | |
24 | <Option type="1" /> |
|
25 | <Option type="1" /> | |
25 | <Option compiler="sparc_rtems_gcc" /> |
|
26 | <Option compiler="sparc_rtems_gcc" /> | |
26 | <Compiler> |
|
27 | <Compiler> | |
27 | <Add option="-O2" /> |
|
28 | <Add option="-O2" /> | |
28 | </Compiler> |
|
29 | </Compiler> | |
29 | <Linker> |
|
30 | <Linker> | |
30 | <Add option="-s" /> |
|
31 | <Add option="-s" /> | |
31 | </Linker> |
|
32 | </Linker> | |
32 | </Target> |
|
33 | </Target> | |
33 | </Build> |
|
34 | </Build> | |
34 | <Compiler> |
|
35 | <Compiler> | |
35 | <Add option="-Wall" /> |
|
36 | <Add option="-Wall" /> | |
36 | </Compiler> |
|
37 | </Compiler> | |
|
38 | <Unit filename="../header/FSW-config.h" /> | |||
37 | <Unit filename="../header/FSW-rtems-processing.h" /> |
|
39 | <Unit filename="../header/FSW-rtems-processing.h" /> | |
38 | <Unit filename="../header/TC_handler.h" /> |
|
40 | <Unit filename="../header/TC_handler.h" /> | |
39 | <Unit filename="../header/ccsds_types.h" /> |
|
41 | <Unit filename="../header/ccsds_types.h" /> | |
|
42 | <Unit filename="../header/grlibregs.h" /> | |||
|
43 | <Unit filename="../src/FSW-config.c"> | |||
|
44 | <Option compilerVar="CC" /> | |||
|
45 | </Unit> | |||
40 | <Unit filename="../src/FSW-rtems-processing.c"> |
|
46 | <Unit filename="../src/FSW-rtems-processing.c"> | |
41 | <Option compilerVar="CC" /> |
|
47 | <Option compilerVar="CC" /> | |
42 | </Unit> |
|
48 | </Unit> | |
43 | <Unit filename="../src/FSW-rtems.c"> |
|
49 | <Unit filename="../src/FSW-rtems.c"> | |
44 | <Option compilerVar="CC" /> |
|
50 | <Option compilerVar="CC" /> | |
45 | </Unit> |
|
51 | </Unit> | |
46 | <Unit filename="../src/TC_handler.c"> |
|
52 | <Unit filename="../src/TC_handler.c"> | |
47 | <Option compilerVar="CC" /> |
|
53 | <Option compilerVar="CC" /> | |
48 | </Unit> |
|
54 | </Unit> | |
49 | <Extensions> |
|
55 | <Extensions> | |
50 | <code_completion /> |
|
56 | <code_completion /> | |
51 | <debugger /> |
|
57 | <debugger /> | |
52 | </Extensions> |
|
58 | </Extensions> | |
53 | </Project> |
|
59 | </Project> | |
54 | </CodeBlocks_project_file> |
|
60 | </CodeBlocks_project_file> |
@@ -1,79 +1,91 | |||||
1 | # depslib dependency file v1.0 |
|
1 | # depslib dependency file v1.0 | |
2 | 1355231335 source:e:\missions\solo\dev_ple\src\fsw-rtems.c |
|
2 | 1355231335 source:e:\missions\solo\dev_ple\src\fsw-rtems.c | |
3 | <rtems.h> |
|
3 | <rtems.h> | |
4 | <bsp.h> |
|
4 | <bsp.h> | |
5 | <rtems/confdefs.h> |
|
5 | <rtems/confdefs.h> | |
6 | <drvmgr/drvmgr_confdefs.h> |
|
6 | <drvmgr/drvmgr_confdefs.h> | |
7 | <rtems.h> |
|
7 | <rtems.h> | |
8 | <errno.h> |
|
8 | <errno.h> | |
9 | <fcntl.h> |
|
9 | <fcntl.h> | |
10 | <stdio.h> |
|
10 | <stdio.h> | |
11 | <stdlib.h> |
|
11 | <stdlib.h> | |
12 | <FSW-config.c> |
|
12 | <FSW-config.c> | |
13 | <grspw.h> |
|
13 | <grspw.h> | |
14 | <apbuart.h> |
|
14 | <apbuart.h> | |
15 | <FSW-rtems-processing.h> |
|
15 | <FSW-rtems-processing.h> | |
16 |
|
16 | |||
17 | 1355231335 -processing.h> |
|
17 | 1355231335 -processing.h> | |
18 |
|
18 | |||
19 | 1350983928 source:e:\missions\solo\dev_ple\src\grspw.c |
|
19 | 1350983928 source:e:\missions\solo\dev_ple\src\grspw.c | |
20 | <..\header\grspw.h> |
|
20 | <..\header\grspw.h> | |
21 | <stdio.h> |
|
21 | <stdio.h> | |
22 |
|
22 | |||
23 | 1354190601 |
|
23 | 1354190601 | |
24 |
|
24 | |||
25 | 1350983928 e:\missions\solo\dev_ple\header\grspw.h |
|
25 | 1350983928 e:\missions\solo\dev_ple\header\grspw.h | |
26 |
|
26 | |||
27 | 1351173364 source:e:\missions\solo\dev_ple\src\fsw-config.c |
|
27 | 1351173364 source:e:\missions\solo\dev_ple\src\fsw-config.c | |
28 | <drvmgr/ambapp_bus.h> |
|
28 | <drvmgr/ambapp_bus.h> | |
29 |
|
29 | |||
30 | 1354190602 bapp_bus.h> |
|
30 | 1354190602 bapp_bus.h> | |
31 |
|
31 | |||
32 | 1354190602 e:\missions\solo\dev_ple\src\fsw-config.c |
|
32 | 1354190602 e:\missions\solo\dev_ple\src\fsw-config.c | |
33 | <drvmgr/ambapp_bus.h> |
|
33 | <drvmgr/ambapp_bus.h> | |
34 |
|
34 | |||
35 | 1354190602 source:e:\missions\solo\dev_ple\src\fsw-rtems-processing.c |
|
35 | 1354190602 source:e:\missions\solo\dev_ple\src\fsw-rtems-processing.c | |
36 | <FSW-rtems-processing.h> |
|
36 | <FSW-rtems-processing.h> | |
37 | <math.h> |
|
37 | <math.h> | |
38 | <stdio.h> |
|
38 | <stdio.h> | |
39 | <leon.h> |
|
39 | <leon.h> | |
40 |
|
40 | |||
41 | 1354190601 e:\missions\solo\dev_ple\header\fsw-rtems-processing.h |
|
41 | 1354190601 e:\missions\solo\dev_ple\header\fsw-rtems-processing.h | |
42 | <rtems.h> |
|
42 | <rtems.h> | |
43 |
|
43 | |||
44 |
135 |
|
44 | 1359358276 source:/opt/DEV_PLE/src/FSW-rtems-processing.c | |
45 | <FSW-rtems-processing.h> |
|
45 | <FSW-rtems-processing.h> | |
46 | <math.h> |
|
46 | <math.h> | |
47 | <stdio.h> |
|
47 | <stdio.h> | |
48 | <leon.h> |
|
48 | <leon.h> | |
49 |
|
49 | |||
50 |
135 |
|
50 | 1359358276 /opt/DEV_PLE/header/FSW-rtems-processing.h | |
51 | <rtems.h> |
|
51 | <rtems.h> | |
|
52 | <grlibregs.h> | |||
|
53 | <FSW-config.h> | |||
52 |
|
54 | |||
53 |
135 |
|
55 | 1359382800 source:/opt/DEV_PLE/src/FSW-rtems.c | |
54 | <rtems.h> |
|
56 | <rtems.h> | |
55 | <bsp.h> |
|
57 | <bsp.h> | |
56 | <rtems/confdefs.h> |
|
58 | <rtems/confdefs.h> | |
57 | <drvmgr/drvmgr_confdefs.h> |
|
59 | <drvmgr/drvmgr_confdefs.h> | |
58 | <rtems.h> |
|
60 | <rtems.h> | |
59 | <errno.h> |
|
61 | <errno.h> | |
60 | <fcntl.h> |
|
62 | <fcntl.h> | |
61 | <stdio.h> |
|
63 | <stdio.h> | |
62 | <stdlib.h> |
|
64 | <stdlib.h> | |
63 | <FSW-config.c> |
|
|||
64 | <grspw.h> |
|
65 | <grspw.h> | |
65 | <apbuart.h> |
|
66 | <apbuart.h> | |
|
67 | <FSW-config.h> | |||
66 | <TC_handler.h> |
|
68 | <TC_handler.h> | |
67 | <FSW-rtems-processing.h> |
|
69 | <FSW-rtems-processing.h> | |
|
70 | <grlibregs.h> | |||
68 |
|
71 | |||
69 |
1358 |
|
72 | 1358847387 /opt/DEV_PLE/src/FSW-config.c | |
70 | <drvmgr/ambapp_bus.h> |
|
73 | <drvmgr/ambapp_bus.h> | |
71 |
|
74 | |||
72 |
135 |
|
75 | 1359379052 source:/opt/DEV_PLE/src/TC_handler.c | |
73 | <TC_handler.h> |
|
76 | <TC_handler.h> | |
|
77 | <FSW-config.h> | |||
74 |
|
78 | |||
75 |
135 |
|
79 | 1359377846 /opt/DEV_PLE/header/TC_handler.h | |
|
80 | <stdio.h> | |||
76 | <ccsds_types.h> |
|
81 | <ccsds_types.h> | |
77 |
|
82 | |||
78 |
135 |
|
83 | 1359372295 /opt/DEV_PLE/header/ccsds_types.h | |
|
84 | ||||
|
85 | 1359365823 source:/opt/DEV_PLE/src/FSW-config.c | |||
|
86 | <drvmgr/ambapp_bus.h> | |||
79 |
|
87 | |||
|
88 | 1359371402 /opt/DEV_PLE/header/FSW-config.h | |||
|
89 | ||||
|
90 | 1358858576 /opt/DEV_PLE/header/grlibregs.h | |||
|
91 |
@@ -1,22 +1,31 | |||||
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/FSW- |
|
4 | <File name="../header/FSW-config.h" open="0" top="0" tabpos="0"> | |
5 |
<Cursor position="8 |
|
5 | <Cursor position="408" topLine="0" /> | |
|
6 | </File> | |||
|
7 | <File name="../header/FSW-rtems-processing.h" open="0" top="0" tabpos="4"> | |||
|
8 | <Cursor position="230" topLine="0" /> | |||
|
9 | </File> | |||
|
10 | <File name="../header/TC_handler.h" open="1" top="0" tabpos="5"> | |||
|
11 | <Cursor position="107" topLine="0" /> | |||
6 | </File> |
|
12 | </File> | |
7 |
<File name="../header/ |
|
13 | <File name="../header/ccsds_types.h" open="1" top="0" tabpos="4"> | |
8 |
<Cursor position=" |
|
14 | <Cursor position="1571" topLine="40" /> | |
9 | </File> |
|
15 | </File> | |
10 |
<File name="../header/ |
|
16 | <File name="../header/grlibregs.h" open="0" top="0" tabpos="6"> | |
11 |
<Cursor position=" |
|
17 | <Cursor position="262" topLine="1" /> | |
|
18 | </File> | |||
|
19 | <File name="../src/FSW-config.c" open="1" top="0" tabpos="3"> | |||
|
20 | <Cursor position="274" topLine="0" /> | |||
12 | </File> |
|
21 | </File> | |
13 | <File name="../src/FSW-rtems-processing.c" open="0" top="0" tabpos="2"> |
|
22 | <File name="../src/FSW-rtems-processing.c" open="0" top="0" tabpos="2"> | |
14 | <Cursor position="18" topLine="0" /> |
|
23 | <Cursor position="18000" topLine="340" /> | |
15 | </File> |
|
24 | </File> | |
16 |
<File name="../src/FSW-rtems.c" open="1" top=" |
|
25 | <File name="../src/FSW-rtems.c" open="1" top="0" tabpos="1"> | |
17 |
<Cursor position=" |
|
26 | <Cursor position="8119" topLine="99" /> | |
18 | </File> |
|
27 | </File> | |
19 |
<File name="../src/TC_handler.c" open="1" top=" |
|
28 | <File name="../src/TC_handler.c" open="1" top="1" tabpos="2"> | |
20 |
<Cursor position="7 |
|
29 | <Cursor position="7593" topLine="53" /> | |
21 | </File> |
|
30 | </File> | |
22 | </CodeBlocks_layout_file> |
|
31 | </CodeBlocks_layout_file> |
@@ -1,32 +1,45 | |||||
1 | #ifndef FSW_RTEMS_PROCESSING_H_INCLUDED |
|
1 | #ifndef FSW_RTEMS_PROCESSING_H_INCLUDED | |
2 | #define FSW_RTEMS_PROCESSING_H_INCLUDED |
|
2 | #define FSW_RTEMS_PROCESSING_H_INCLUDED | |
3 |
|
3 | |||
4 | #include <rtems.h> |
|
4 | #include <rtems.h> | |
5 |
|
5 | #include <grlibregs.h> | ||
6 | #define ADDRESS_APB_SPECTRAL_MATRICES 0x80000700 |
|
6 | #include <FSW-config.h> | |
7 | #define LPP_SPECTRAL_MATRIX_CTRL 0x80000700 |
|
|||
8 | #define LPP_SPECTRAL_MATRIX_1 0x80000704 |
|
|||
9 | #define LPP_SPECTRAL_MATRIX_2 0x80000708 |
|
|||
10 | #define IRQ_SPECTRAL_MATRICES 12 |
|
|||
11 |
|
7 | |||
12 | #define NB_BINS_SPECTRAL_MATRIX 128 |
|
8 | #define NB_BINS_SPECTRAL_MATRIX 128 | |
13 | #define NB_VALUES_PER_SPECTRAL_MATRIX 25 |
|
9 | #define NB_VALUES_PER_SPECTRAL_MATRIX 25 | |
14 | #define TOTAL_SIZE_SPECTRAL_MATRIX NB_BINS_SPECTRAL_MATRIX * NB_VALUES_PER_SPECTRAL_MATRIX |
|
10 | #define TOTAL_SIZE_SPECTRAL_MATRIX NB_BINS_SPECTRAL_MATRIX * NB_VALUES_PER_SPECTRAL_MATRIX | |
15 | #define NB_BINS_COMPRESSED_MATRIX_f0 11 |
|
11 | #define NB_BINS_COMPRESSED_MATRIX_f0 11 | |
16 | #define SIZE_COMPRESSED_SPECTRAL_MATRIX_f1 13 |
|
12 | #define SIZE_COMPRESSED_SPECTRAL_MATRIX_f1 13 | |
17 | #define SIZE_COMPRESSED_SPECTRAL_MATRIX_f2 12 |
|
13 | #define SIZE_COMPRESSED_SPECTRAL_MATRIX_f2 12 | |
18 | #define TOTAL_SIZE_COMPRESSED_MATRIX_f0 NB_BINS_COMPRESSED_MATRIX_f0 * NB_VALUES_PER_SPECTRAL_MATRIX |
|
14 | #define TOTAL_SIZE_COMPRESSED_MATRIX_f0 NB_BINS_COMPRESSED_MATRIX_f0 * NB_VALUES_PER_SPECTRAL_MATRIX | |
19 | #define NB_AVERAGE_NORMAL_f0 96*4 |
|
15 | #define NB_AVERAGE_NORMAL_f0 96*4 | |
20 |
|
16 | #define NB_SM_TO_RECEIVE_BEFORE_AVF0 8 | ||
21 | rtems_isr spectral_matrices_isr( rtems_vector_number vector ); |
|
17 | ||
|
18 | ||||
|
19 | struct BP1_str{ | |||
|
20 | volatile unsigned char PE[2]; | |||
|
21 | volatile unsigned char PB[2]; | |||
|
22 | volatile unsigned char V0; | |||
|
23 | volatile unsigned char V1; | |||
|
24 | volatile unsigned char V2_ELLIP_DOP; | |||
|
25 | volatile unsigned char SZ; | |||
|
26 | volatile unsigned char VPHI; | |||
|
27 | }; | |||
|
28 | typedef struct BP1_str BP1_t; | |||
|
29 | ||||
|
30 | // ISR | |||
|
31 | rtems_isr spectral_matrices_isr( rtems_vector_number vector ); | |||
|
32 | // RTEMS TASKS | |||
22 | rtems_task spw_bppr_task(rtems_task_argument argument); |
|
33 | rtems_task spw_bppr_task(rtems_task_argument argument); | |
23 | rtems_task spw_avf0_task(rtems_task_argument argument); |
|
34 | rtems_task spw_avf0_task(rtems_task_argument argument); | |
24 | rtems_task spw_bpf0_task(rtems_task_argument argument); |
|
35 | rtems_task spw_bpf0_task(rtems_task_argument argument); | |
|
36 | rtems_task spw_smiq_task(rtems_task_argument argument); // added to test the spectral matrix simulator | |||
|
37 | // | |||
25 | rtems_task spw_bppr_task_rate_monotonic(rtems_task_argument argument); |
|
38 | rtems_task spw_bppr_task_rate_monotonic(rtems_task_argument argument); | |
26 | void matrix_average(volatile int *spectral_matrix, float *averaged_spectral_matrix); |
|
39 | void matrix_average(volatile int *spectral_matrix, float *averaged_spectral_matrix); | |
27 | void matrix_compression(float *averaged_spectral_matrix, unsigned char fChannel, float *compressed_spectral_matrix); |
|
40 | void matrix_compression(float *averaged_spectral_matrix, unsigned char fChannel, float *compressed_spectral_matrix); | |
28 | void matrix_reset(float *averaged_spectral_matrix); |
|
41 | void matrix_reset(float *averaged_spectral_matrix); | |
29 | void BP1_set(float * compressed_spectral_matrix, unsigned char nb_bins_compressed_spectral_matrix, unsigned char * LFR_BP1); |
|
42 | void BP1_set(float * compressed_spectral_matrix, unsigned char nb_bins_compressed_spectral_matrix, unsigned char * LFR_BP1); | |
30 | void BP2_set(float * compressed_spectral_matrix, unsigned char nb_bins_compressed_spectral_matrix); |
|
43 | void BP2_set(float * compressed_spectral_matrix, unsigned char nb_bins_compressed_spectral_matrix); | |
31 |
|
44 | |||
32 | #endif // FSW_RTEMS_PROCESSING_H_INCLUDED |
|
45 | #endif // FSW_RTEMS_PROCESSING_H_INCLUDED |
@@ -1,22 +1,25 | |||||
1 | #ifndef TC_HANDLER_H_INCLUDED |
|
1 | #ifndef TC_HANDLER_H_INCLUDED | |
2 | #define TC_HANDLER_H_INCLUDED |
|
2 | #define TC_HANDLER_H_INCLUDED | |
3 |
|
3 | |||
4 |
#include < |
|
4 | #include <stdio.h> | |
|
5 | #include <ccsds_types.h> | |||
5 |
|
6 | |||
6 | unsigned char currentTC_LEN_RCV[2]; // SHALL be equal to the current TC packet estimated packet length field |
|
7 | unsigned char currentTC_LEN_RCV[2]; // SHALL be equal to the current TC packet estimated packet length field | |
7 | unsigned char currentTC_COMPUTED_CRC[2]; |
|
8 | unsigned char currentTC_COMPUTED_CRC[2]; | |
8 | unsigned int currentTC_LEN_RCV_AsUnsignedInt; |
|
9 | unsigned int currentTC_LEN_RCV_AsUnsignedInt; | |
9 | unsigned int currentTM_length; |
|
10 | unsigned int currentTM_length; | |
10 | ccsdsTelemetryPacket_t currentTM; |
|
|||
11 | ccsdsTelecommandPacket_t currentTC; |
|
|||
12 | unsigned char currentTC_processedFlag; |
|
11 | unsigned char currentTC_processedFlag; | |
13 |
|
12 | |||
14 | unsigned int lookUpTableForCRC[256]; |
|
13 | unsigned int lookUpTableForCRC[256]; | |
15 | void InitLookUpTableForCRC(); |
|
14 | void InitLookUpTableForCRC(); | |
16 | void GetCRCAsTwoBytes(unsigned char* data, unsigned char* crcAsTwoBytes, unsigned int sizeOfData); |
|
15 | void GetCRCAsTwoBytes(unsigned char* data, unsigned char* crcAsTwoBytes, unsigned int sizeOfData); | |
17 |
|
16 | |||
18 |
unsigned char |
|
17 | unsigned char acceptTM(ccsdsTelecommandPacket_t * TMPacket, unsigned int TC_LEN_RCV); | |
19 | unsigned char TM_acceptance_generator(ccsdsTelecommandPacket_t * TCPacket, unsigned int code, ccsdsTelemetryPacket_t * TMPacket); |
|
18 | ||
20 | unsigned char TM_not_implemented_generator(ccsdsTelecommandPacket_t * TCPacket, ccsdsTelemetryPacket_t * TMPacket); |
|
19 | unsigned char TM_build_header( enum TM_TYPE tm_type, unsigned int SID, unsigned int packetLength, | |
|
20 | unsigned int coarseTime, unsigned int fineTime, TMHeader_t *TMHeader); | |||
|
21 | unsigned char TM_build_data(ccsdsTelecommandPacket_t *TC, char* data, unsigned int SID, unsigned char *computed_CRC); | |||
|
22 | unsigned int TC_checker(ccsdsTelecommandPacket_t *TC, unsigned int TC_LEN_RCV, | |||
|
23 | TMHeader_t *TM_Header, unsigned int *hlen, char *data); | |||
21 |
|
24 | |||
22 | #endif // TC_HANDLER_H_INCLUDED |
|
25 | #endif // TC_HANDLER_H_INCLUDED |
@@ -1,63 +1,118 | |||||
1 | #ifndef CCSDS_H_INCLUDED |
|
1 | #ifndef CCSDS_H_INCLUDED | |
2 | #define CCSDS_H_INCLUDED |
|
2 | #define CCSDS_H_INCLUDED | |
3 |
|
3 | |||
4 | #define CCSDS_TELEMETRY_HEADER_LENGTH 16+4 |
|
4 | #define CCSDS_TELEMETRY_HEADER_LENGTH 16+4 | |
5 | #define CCSDS_TM_PKT_MAX_SIZE 4412 |
|
5 | #define CCSDS_TM_PKT_MAX_SIZE 4412 | |
6 | #define CCSDS_TELECOMMAND_HEADER_LENGTH 10+4 |
|
6 | #define CCSDS_TELECOMMAND_HEADER_LENGTH 10+4 | |
7 | #define CCSDS_TC_PKT_MAX_SIZE 248 |
|
7 | #define CCSDS_TC_PKT_MAX_SIZE 248 | |
8 | #define CCSDS_TC_TM_PACKET_OFFSET 7 |
|
8 | #define CCSDS_TC_TM_PACKET_OFFSET 7 | |
9 | #define CCSDS_PROCESS_ID 76 |
|
9 | #define CCSDS_PROCESS_ID 76 | |
10 | #define CCSDS_PACKET_CATEGORY 12 |
|
10 | #define CCSDS_PACKET_CATEGORY 12 | |
11 | #define CCSDS_DESTINATION_ID 0x21 |
|
11 | #define CCSDS_DESTINATION_ID 0x21 | |
|
12 | #define CCSDS_PROTOCOLE_ID 0x02 | |||
|
13 | #define CCSDS_USER_APP 0x00 | |||
12 |
|
14 | |||
13 | #define SIZE_TM_LFR_TC_EXE_NOT_IMPLEMENTED 24 |
|
15 | #define SIZE_TM_LFR_TC_EXE_NOT_IMPLEMENTED 24 | |
14 | #define SIZE_TM_LFR_TC_EXE_CORRUPTED 32 |
|
16 | #define SIZE_TM_LFR_TC_EXE_CORRUPTED 32 | |
15 |
|
17 | |||
16 | #define ILLEGAL_APID 0 |
|
18 | #define ILLEGAL_APID 0 | |
17 | #define WRONG_LEN_PACKET 1 |
|
19 | #define WRONG_LEN_PACKET 1 | |
18 | #define INCOR_CHECKSUM 2 |
|
20 | #define INCOR_CHECKSUM 2 | |
19 | #define ILL_TYPE 3 |
|
21 | #define ILL_TYPE 3 | |
20 | #define ILL_SUBTYPE 4 |
|
22 | #define ILL_SUBTYPE 4 | |
21 | #define WRONG_APP_DATA 5 |
|
23 | #define WRONG_APP_DATA 5 | |
22 | #define WRONG_CMD_CODE 6 |
|
24 | #define WRONG_CMD_CODE 6 | |
23 | #define CCSDS_TM_VALID 7 |
|
25 | #define CCSDS_TM_VALID 7 | |
24 | // |
|
26 | // | |
25 | #define TC_NOT_EXE 40000 |
|
27 | #define TC_NOT_EXE 40000 | |
26 | #define WRONG_SRC_ID 40001 |
|
28 | #define WRONG_SRC_ID 40001 | |
27 | #define FUNCT_NOT_IMPL 40002 |
|
29 | #define FUNCT_NOT_IMPL 40002 | |
28 | #define FAIL_DETECTED 40003 |
|
30 | #define FAIL_DETECTED 40003 | |
29 | #define NOT_ALLOWED 40004 |
|
31 | #define NOT_ALLOWED 40004 | |
30 | #define CORRUPTED 40005 |
|
32 | #define CORRUPTED 40005 | |
31 | // |
|
33 | // | |
32 | #define CCSDS_ERR_SRC 8 |
|
34 | #define SID_TC_RESET 3 | |
33 | #define CCSDS_ERR_CAT 9 |
|
35 | #define SID_TC_LOAD_COMM 20 | |
34 |
|
36 | #define SID_TC_LOAD_NORM 21 | ||
35 | struct ccsdsTelemetryPacket_str |
|
37 | #define SID_TC_LOAD_BURST 24 | |
|
38 | #define SID_TC_LOAD_SBM1 27 | |||
|
39 | #define SID_TC_LOAD_SBM2 28 | |||
|
40 | #define SID_TC_DUMP 30 | |||
|
41 | #define SID_TC_ENTER 40 | |||
|
42 | #define SID_TC_UPDT_INFO 50 | |||
|
43 | #define SID_TC_EN_CAL 60 | |||
|
44 | #define SID_TC_DIS_CAL 61 | |||
|
45 | #define SID_TC_UPDT_TIME 129 | |||
|
46 | ||||
|
47 | #define SID_DEFAULT 0 | |||
|
48 | #define SID_EXE_INC 5 | |||
|
49 | #define SID_NOT_EXE 40000 | |||
|
50 | #define SID_NOT_IMP 40002 | |||
|
51 | #define SID_EXE_ERR 40003 | |||
|
52 | #define SID_EXE_CORR 40005 | |||
|
53 | ||||
|
54 | #define SID_NORM_SWF_F0 3 | |||
|
55 | #define SID_NORM_SWF_F1 4 | |||
|
56 | #define SID_NORM_SWF_F2 5 | |||
|
57 | #define SID_NORM_CWF_F3 1 | |||
|
58 | ||||
|
59 | #define TM_LEN_EXE 20 - CCSDS_TC_TM_PACKET_OFFSET | |||
|
60 | #define TM_LEN_NOT_EXE 26 - CCSDS_TC_TM_PACKET_OFFSET | |||
|
61 | #define TM_LEN_NOT_IMP 24 - CCSDS_TC_TM_PACKET_OFFSET | |||
|
62 | #define TM_LEN_EXE_ERR 24 - CCSDS_TC_TM_PACKET_OFFSET | |||
|
63 | #define TM_LEN_EXE_CORR 32 - CCSDS_TC_TM_PACKET_OFFSET | |||
|
64 | #define TM_HEADER_LEN 16 | |||
|
65 | #define TM_LEN_SCI_NORM_SWF_340 340 * 12 + 6 + 10 - 1 | |||
|
66 | #define TM_LEN_SCI_NORM_SWF_8 8 * 12 + 6 + 10 - 1 | |||
|
67 | ||||
|
68 | enum TM_TYPE{ | |||
|
69 | TM_LFR_TC_EXE_OK, | |||
|
70 | TM_LFR_TC_EXE_ERR, | |||
|
71 | TM_LFR_HK, | |||
|
72 | TM_LFR_SCI, | |||
|
73 | TM_LFR_SCI_SBM, | |||
|
74 | TM_LFR_PAR_DUMP | |||
|
75 | }; | |||
|
76 | ||||
|
77 | struct TMHeader_str | |||
36 | { |
|
78 | { | |
37 | volatile unsigned char targetLogicalAddress; |
|
79 | volatile unsigned char targetLogicalAddress; | |
38 | volatile unsigned char protocolIdentifier; |
|
80 | volatile unsigned char protocolIdentifier; | |
39 | volatile unsigned char reserved; |
|
81 | volatile unsigned char reserved; | |
40 | volatile unsigned char userApplication; |
|
82 | volatile unsigned char userApplication; | |
41 | volatile unsigned char packetID[2]; |
|
83 | volatile unsigned char packetID[2]; | |
42 | volatile unsigned char packetSequenceControl[2]; |
|
84 | volatile unsigned char packetSequenceControl[2]; | |
43 | volatile unsigned char packetLength[2]; |
|
85 | volatile unsigned char packetLength[2]; | |
|
86 | volatile unsigned char dataFieldHeader[10]; | |||
|
87 | }; | |||
|
88 | typedef struct TMHeader_str TMHeader_t; | |||
|
89 | ||||
|
90 | struct ExtendedTMHeader_str | |||
|
91 | { | |||
|
92 | volatile unsigned char targetLogicalAddress; | |||
|
93 | volatile unsigned char protocolIdentifier; | |||
|
94 | volatile unsigned char reserved; | |||
|
95 | volatile unsigned char userApplication; | |||
|
96 | volatile unsigned char packetID[2]; | |||
|
97 | volatile unsigned char packetSequenceControl[2]; | |||
|
98 | volatile unsigned char packetLength[2]; | |||
44 | volatile unsigned char dataFieldHeader[10]; |
|
99 | volatile unsigned char dataFieldHeader[10]; | |
45 |
volatile unsigned char |
|
100 | volatile unsigned char auxiliaryHeader[6]; | |
46 | }; |
|
101 | }; | |
47 | typedef struct ccsdsTelemetryPacket_str ccsdsTelemetryPacket_t; |
|
102 | typedef struct ExtendedTMHeader_str ExtendedTMHeader_t; | |
48 |
|
103 | |||
49 | struct ccsdsTelecommandPacket_str |
|
104 | struct ccsdsTelecommandPacket_str | |
50 | { |
|
105 | { | |
51 | //unsigned char targetLogicalAddress; |
|
106 | //unsigned char targetLogicalAddress; // removed by the grspw module | |
52 | volatile unsigned char protocolIdentifier; |
|
107 | volatile unsigned char protocolIdentifier; | |
53 | volatile unsigned char reserved; |
|
108 | volatile unsigned char reserved; | |
54 | volatile unsigned char userApplication; |
|
109 | volatile unsigned char userApplication; | |
55 | volatile unsigned char packetID[2]; |
|
110 | volatile unsigned char packetID[2]; | |
56 | volatile unsigned char packetSequenceControl[2]; |
|
111 | volatile unsigned char packetSequenceControl[2]; | |
57 | volatile unsigned char packetLength[2]; |
|
112 | volatile unsigned char packetLength[2]; | |
58 | volatile unsigned char dataFieldHeader[4]; |
|
113 | volatile unsigned char dataFieldHeader[4]; | |
59 | volatile unsigned char dataAndCRC[CCSDS_TC_PKT_MAX_SIZE-10]; |
|
114 | volatile unsigned char dataAndCRC[CCSDS_TC_PKT_MAX_SIZE-10]; | |
60 | }; |
|
115 | }; | |
61 | typedef struct ccsdsTelecommandPacket_str ccsdsTelecommandPacket_t; |
|
116 | typedef struct ccsdsTelecommandPacket_str ccsdsTelecommandPacket_t; | |
62 |
|
117 | |||
63 | #endif // CCSDS_H_INCLUDED |
|
118 | #endif // CCSDS_H_INCLUDED |
@@ -1,53 +1,53 | |||||
1 | #include <drvmgr/ambapp_bus.h> |
|
1 | #include <drvmgr/ambapp_bus.h> | |
2 |
|
2 | |||
3 | // GRSPW0 resources |
|
3 | // GRSPW0 resources | |
4 | struct drvmgr_key grlib_grspw_0n1_res[] = { |
|
4 | struct drvmgr_key grlib_grspw_0n1_res[] = { | |
5 | {"txBdCnt", KEY_TYPE_INT, {(unsigned int)10}}, |
|
5 | {"txBdCnt", KEY_TYPE_INT, {(unsigned int)10}}, | |
6 | {"rxBdCnt", KEY_TYPE_INT, {(unsigned int)10}}, |
|
6 | {"rxBdCnt", KEY_TYPE_INT, {(unsigned int)10}}, | |
7 |
{"txDataSize", KEY_TYPE_INT, {(unsigned int)4 |
|
7 | {"txDataSize", KEY_TYPE_INT, {(unsigned int)4096}}, | |
8 |
{"txHdrSize", KEY_TYPE_INT, {(unsigned int) |
|
8 | {"txHdrSize", KEY_TYPE_INT, {(unsigned int)20+6}}, // 6 is for the auxiliary header, when needed | |
9 | {"rxPktSize", KEY_TYPE_INT, {(unsigned int)248+4}}, |
|
9 | {"rxPktSize", KEY_TYPE_INT, {(unsigned int)248+4}}, | |
10 | KEY_EMPTY |
|
10 | KEY_EMPTY | |
11 | }; |
|
11 | }; | |
12 |
|
12 | |||
13 | #if 0 |
|
13 | #if 0 | |
14 | /* APBUART0 */ |
|
14 | /* APBUART0 */ | |
15 | struct drvmgr_key grlib_drv_res_apbuart0[] = |
|
15 | struct drvmgr_key grlib_drv_res_apbuart0[] = | |
16 | { |
|
16 | { | |
17 | {"mode", KEY_TYPE_INT, {(unsigned int)1}}, |
|
17 | {"mode", KEY_TYPE_INT, {(unsigned int)1}}, | |
18 | {"syscon", KEY_TYPE_INT, {(unsigned int)1}}, |
|
18 | {"syscon", KEY_TYPE_INT, {(unsigned int)1}}, | |
19 | KEY_EMPTY |
|
19 | KEY_EMPTY | |
20 | }; |
|
20 | }; | |
21 | /* APBUART1 */ |
|
21 | /* APBUART1 */ | |
22 | struct drvmgr_key grlib_drv_res_apbuart1[] = |
|
22 | struct drvmgr_key grlib_drv_res_apbuart1[] = | |
23 | { |
|
23 | { | |
24 | {"mode", KEY_TYPE_INT, {(unsigned int)1}}, |
|
24 | {"mode", KEY_TYPE_INT, {(unsigned int)1}}, | |
25 | {"syscon", KEY_TYPE_INT, {(unsigned int)0}}, |
|
25 | {"syscon", KEY_TYPE_INT, {(unsigned int)0}}, | |
26 | KEY_EMPTY |
|
26 | KEY_EMPTY | |
27 | }; |
|
27 | }; | |
28 | /* LEON3 System with driver configuration for 2 APBUARTs, the |
|
28 | /* LEON3 System with driver configuration for 2 APBUARTs, the | |
29 | * the rest of the AMBA device drivers use their defaults. |
|
29 | * the rest of the AMBA device drivers use their defaults. | |
30 | */ |
|
30 | */ | |
31 |
|
31 | |||
32 | /* Override default debug UART assignment. |
|
32 | /* Override default debug UART assignment. | |
33 | * 0 = Default APBUART. APBUART[0], but on MP system CPU0=APBUART0, |
|
33 | * 0 = Default APBUART. APBUART[0], but on MP system CPU0=APBUART0, | |
34 | * CPU1=APBUART1... |
|
34 | * CPU1=APBUART1... | |
35 | * 1 = APBUART[0] |
|
35 | * 1 = APBUART[0] | |
36 | * 2 = APBUART[1] |
|
36 | * 2 = APBUART[1] | |
37 | * 3 = APBUART[2] |
|
37 | * 3 = APBUART[2] | |
38 | * ... |
|
38 | * ... | |
39 | */ |
|
39 | */ | |
40 | //int debug_uart_index = 2; /* second UART -- APBUART[1] */ |
|
40 | //int debug_uart_index = 2; /* second UART -- APBUART[1] */ | |
41 | #endif |
|
41 | #endif | |
42 |
|
42 | |||
43 | // If RTEMS_DRVMGR_STARTUP is defined we override the "weak defaults" that is defined by the LEON3 BSP. |
|
43 | // If RTEMS_DRVMGR_STARTUP is defined we override the "weak defaults" that is defined by the LEON3 BSP. | |
44 |
|
44 | |||
45 | struct drvmgr_bus_res grlib_drv_resources = { |
|
45 | struct drvmgr_bus_res grlib_drv_resources = { | |
46 | .next = NULL, |
|
46 | .next = NULL, | |
47 | .resource = { |
|
47 | .resource = { | |
48 | {DRIVER_AMBAPP_GAISLER_GRSPW_ID, 0, &grlib_grspw_0n1_res[0]}, |
|
48 | {DRIVER_AMBAPP_GAISLER_GRSPW_ID, 0, &grlib_grspw_0n1_res[0]}, | |
49 | // {DRIVER_AMBAPP_GAISLER_APBUART_ID, 0, &grlib_drv_res_apbuart0[0]}, |
|
49 | // {DRIVER_AMBAPP_GAISLER_APBUART_ID, 0, &grlib_drv_res_apbuart0[0]}, | |
50 | // {DRIVER_AMBAPP_GAISLER_APBUART_ID, 1, &grlib_drv_res_apbuart1[0]}, |
|
50 | // {DRIVER_AMBAPP_GAISLER_APBUART_ID, 1, &grlib_drv_res_apbuart1[0]}, | |
51 | RES_EMPTY /* Mark end of device resource array */ |
|
51 | RES_EMPTY /* Mark end of device resource array */ | |
52 | } |
|
52 | } | |
53 | }; |
|
53 | }; |
@@ -1,397 +1,423 | |||||
1 | #include <FSW-rtems-processing.h> |
|
1 | #include <FSW-rtems-processing.h> | |
2 | #include<math.h> |
|
2 | #include<math.h> | |
3 | #include <stdio.h> |
|
3 | #include <stdio.h> | |
4 | #include <leon.h> |
|
4 | #include <leon.h> | |
5 |
|
5 | |||
6 | float k14_re = 1; |
|
6 | float k14_re = 1; | |
7 | float k14_im = 1; |
|
7 | float k14_im = 1; | |
8 | float k14_bis_re = 1; |
|
8 | float k14_bis_re = 1; | |
9 | float k14_bis_im = 1; |
|
9 | float k14_bis_im = 1; | |
10 | float k14_tris_re = 1; |
|
10 | float k14_tris_re = 1; | |
11 | float k14_tris_im = 1; |
|
11 | float k14_tris_im = 1; | |
12 | float k15_re = 1; |
|
12 | float k15_re = 1; | |
13 | float k15_im = 1; |
|
13 | float k15_im = 1; | |
14 | float k15_bis_re = 1; |
|
14 | float k15_bis_re = 1; | |
15 | float k15_bis_im = 1; |
|
15 | float k15_bis_im = 1; | |
16 | float k24_re = 1; |
|
16 | float k24_re = 1; | |
17 | float k24_im = 1; |
|
17 | float k24_im = 1; | |
18 | float k24_bis_re = 1; |
|
18 | float k24_bis_re = 1; | |
19 | float k24_bis_im = 1; |
|
19 | float k24_bis_im = 1; | |
20 | float k24_tris_re = 1; |
|
20 | float k24_tris_re = 1; | |
21 | float k24_tris_im = 1; |
|
21 | float k24_tris_im = 1; | |
22 | float k25_re = 1; |
|
22 | float k25_re = 1; | |
23 | float k25_im = 1; |
|
23 | float k25_im = 1; | |
24 | float k25_bis_re = 1; |
|
24 | float k25_bis_re = 1; | |
25 | float k25_bis_im = 1; |
|
25 | float k25_bis_im = 1; | |
26 | float k34_re = 1; |
|
26 | float k34_re = 1; | |
27 | float k34_im = 1; |
|
27 | float k34_im = 1; | |
28 | float k44 = 1; |
|
28 | float k44 = 1; | |
29 | float k55 = 1; |
|
29 | float k55 = 1; | |
30 | float k45_re = 1; |
|
30 | float k45_re = 1; | |
31 | float k45_im = 1; |
|
31 | float k45_im = 1; | |
32 | float alpha_M = M_PI/4; |
|
32 | float alpha_M = M_PI/4; | |
33 |
|
33 | |||
34 | volatile int spectral_matrix_f0_a[TOTAL_SIZE_SPECTRAL_MATRIX]; |
|
34 | volatile int spectral_matrix_f0_a[TOTAL_SIZE_SPECTRAL_MATRIX]; | |
35 | volatile int spectral_matrix_f0_b[TOTAL_SIZE_SPECTRAL_MATRIX]; |
|
35 | volatile int spectral_matrix_f0_b[TOTAL_SIZE_SPECTRAL_MATRIX]; | |
36 | volatile int spectral_matrix_f0_c[TOTAL_SIZE_SPECTRAL_MATRIX]; |
|
36 | volatile int spectral_matrix_f0_c[TOTAL_SIZE_SPECTRAL_MATRIX]; | |
37 | volatile int spectral_matrix_f0_d[TOTAL_SIZE_SPECTRAL_MATRIX]; |
|
37 | volatile int spectral_matrix_f0_d[TOTAL_SIZE_SPECTRAL_MATRIX]; | |
38 | volatile int spectral_matrix_f0_e[TOTAL_SIZE_SPECTRAL_MATRIX]; |
|
38 | volatile int spectral_matrix_f0_e[TOTAL_SIZE_SPECTRAL_MATRIX]; | |
39 | volatile int spectral_matrix_f0_f[TOTAL_SIZE_SPECTRAL_MATRIX]; |
|
39 | volatile int spectral_matrix_f0_f[TOTAL_SIZE_SPECTRAL_MATRIX]; | |
40 | volatile int spectral_matrix_f0_g[TOTAL_SIZE_SPECTRAL_MATRIX]; |
|
40 | volatile int spectral_matrix_f0_g[TOTAL_SIZE_SPECTRAL_MATRIX]; | |
41 | volatile int spectral_matrix_f0_h[TOTAL_SIZE_SPECTRAL_MATRIX]; |
|
41 | volatile int spectral_matrix_f0_h[TOTAL_SIZE_SPECTRAL_MATRIX]; | |
42 | float averaged_spectral_matrix_f0[TOTAL_SIZE_SPECTRAL_MATRIX]; |
|
42 | float averaged_spectral_matrix_f0[TOTAL_SIZE_SPECTRAL_MATRIX]; | |
43 | float compressed_spectral_matrix_f0[TOTAL_SIZE_COMPRESSED_MATRIX_f0]; |
|
43 | float compressed_spectral_matrix_f0[TOTAL_SIZE_COMPRESSED_MATRIX_f0]; | |
44 | unsigned char LFR_BP1_F0[NB_BINS_COMPRESSED_MATRIX_f0*9]; |
|
44 | unsigned char LFR_BP1_F0[NB_BINS_COMPRESSED_MATRIX_f0*9]; | |
|
45 | ||||
|
46 | BP1_t data_BP1[NB_BINS_COMPRESSED_MATRIX_f0]; | |||
45 |
|
47 | |||
46 | extern rtems_id Task_id[]; /* array of task ids */ |
|
48 | extern rtems_id Task_id[]; /* array of task ids */ | |
47 |
|
49 | |||
48 |
s |
|
50 | spectral_matrices_regs_t *spectral_matrices_regs; | |
49 | volatile int ctrl; |
|
|||
50 | volatile int address0; |
|
|||
51 | volatile int address1; |
|
|||
52 | }; |
|
|||
53 | struct spectral_matrices_regs_str *spectral_matrices_regs; |
|
|||
54 |
|
51 | |||
55 | // Interrupt Service Routine for spectral matrices processing |
|
52 | // Interrupt Service Routine for spectral matrices processing | |
56 | rtems_isr spectral_matrices_isr( rtems_vector_number vector ) |
|
53 | rtems_isr spectral_matrices_isr( rtems_vector_number vector ) | |
57 | { |
|
54 | { | |
58 | if (rtems_event_send( Task_id[4], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) |
|
55 | if (rtems_event_send( Task_id[4], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) | |
59 |
printf("In spectral_matrices_isr *** Error sending event to |
|
56 | printf("In spectral_matrices_isr *** Error sending event to AVF0\n"); | |
60 | } |
|
57 | } | |
|
58 | ||||
|
59 | rtems_task spw_smiq_task(rtems_task_argument argument) // process the Spectral Matrices IRQ | |||
|
60 | { | |||
|
61 | rtems_event_set event_out; | |||
|
62 | gptimer_regs_t *gptimer_regs; | |||
|
63 | gptimer_regs = (gptimer_regs_t *) REGS_ADDRESS_GPTIMER; | |||
|
64 | unsigned char nb_interrupt_f0 = 0; | |||
|
65 | ||||
|
66 | while(1){ | |||
|
67 | rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0 | |||
|
68 | nb_interrupt_f0 = nb_interrupt_f0 + 1; | |||
|
69 | if (nb_interrupt_f0 == (NB_SM_TO_RECEIVE_BEFORE_AVF0-1) ){ | |||
|
70 | if (rtems_event_send( Task_id[6], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) | |||
|
71 | printf("In spw_smiq_task *** Error sending event to AVF0\n"); | |||
|
72 | nb_interrupt_f0 = 0; | |||
|
73 | } | |||
|
74 | gptimer_regs->timer2_ctrl = gptimer_regs->timer2_ctrl | 0x00000010; | |||
|
75 | } | |||
|
76 | } | |||
61 |
|
77 | |||
62 | rtems_task spw_bppr_task(rtems_task_argument argument) |
|
78 | rtems_task spw_bppr_task(rtems_task_argument argument) | |
63 | { |
|
79 | { | |
64 | rtems_status_code status; |
|
80 | rtems_status_code status; | |
65 | rtems_event_set event_out; |
|
81 | rtems_event_set event_out; | |
66 | static int nb_average_f0 = 0; |
|
82 | static int nb_average_f0 = 0; | |
67 | //static int nb_average_f1 = 0; |
|
83 | //static int nb_average_f1 = 0; | |
68 | //static int nb_average_f2 = 0; |
|
84 | //static int nb_average_f2 = 0; | |
69 |
|
85 | |||
70 | while(1) |
|
86 | while(1) | |
71 |
|
87 | |||
72 |
spectral_matrices_regs = (struct spectral_matrices_regs_str *) ADDRESS |
|
88 | spectral_matrices_regs = (struct spectral_matrices_regs_str *) REGS_ADDRESS_SPECTRAL_MATRICES; | |
73 | spectral_matrices_regs->address0 = (volatile int) spectral_matrix_f0_a; |
|
89 | spectral_matrices_regs->address0 = (volatile int) spectral_matrix_f0_a; | |
74 | spectral_matrices_regs->address1 = (volatile int) spectral_matrix_f0_b; |
|
90 | spectral_matrices_regs->address1 = (volatile int) spectral_matrix_f0_b; | |
75 |
|
91 | |||
76 | printf("In BPPR ***\n"); |
|
92 | printf("In BPPR ***\n"); | |
77 |
|
93 | |||
78 | while(1){ // wait for an event to begin with the processing |
|
94 | while(1){ // wait for an event to begin with the processing | |
79 | status = rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); |
|
95 | status = rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); | |
80 | if (status == RTEMS_SUCCESSFUL){ |
|
96 | if (status == RTEMS_SUCCESSFUL){ | |
81 | if ((spectral_matrices_regs->ctrl & 0x00000001)==1){ |
|
97 | if ((spectral_matrices_regs->ctrl & 0x00000001)==1){ | |
82 | matrix_average(spectral_matrix_f0_a, averaged_spectral_matrix_f0); |
|
98 | matrix_average(spectral_matrix_f0_a, averaged_spectral_matrix_f0); | |
83 | spectral_matrices_regs->ctrl = spectral_matrices_regs->ctrl & 0xfffffffe; |
|
99 | spectral_matrices_regs->ctrl = spectral_matrices_regs->ctrl & 0xfffffffe; | |
84 | //printf("f0_a\n"); |
|
100 | //printf("f0_a\n"); | |
85 | nb_average_f0++; |
|
101 | nb_average_f0++; | |
86 | } |
|
102 | } | |
87 | if (((spectral_matrices_regs->ctrl>>1) & 0x00000001)==1){ |
|
103 | if (((spectral_matrices_regs->ctrl>>1) & 0x00000001)==1){ | |
88 | matrix_average(spectral_matrix_f0_b, compressed_spectral_matrix_f0); |
|
104 | matrix_average(spectral_matrix_f0_b, compressed_spectral_matrix_f0); | |
89 | spectral_matrices_regs->ctrl = spectral_matrices_regs->ctrl & 0xfffffffd; |
|
105 | spectral_matrices_regs->ctrl = spectral_matrices_regs->ctrl & 0xfffffffd; | |
90 | //printf("f0_b\n"); |
|
106 | //printf("f0_b\n"); | |
91 | nb_average_f0++; |
|
107 | nb_average_f0++; | |
92 | } |
|
108 | } | |
93 | if (nb_average_f0 == NB_AVERAGE_NORMAL_f0){ |
|
109 | if (nb_average_f0 == NB_AVERAGE_NORMAL_f0){ | |
94 | matrix_compression(averaged_spectral_matrix_f0, 0, compressed_spectral_matrix_f0); |
|
110 | matrix_compression(averaged_spectral_matrix_f0, 0, compressed_spectral_matrix_f0); | |
95 | //printf("f0 compressed\n"); |
|
111 | //printf("f0 compressed\n"); | |
96 | nb_average_f0 = 0; |
|
112 | nb_average_f0 = 0; | |
97 | matrix_reset(averaged_spectral_matrix_f0); |
|
113 | matrix_reset(averaged_spectral_matrix_f0); | |
98 | } |
|
114 | } | |
99 | } |
|
115 | } | |
100 | } |
|
116 | } | |
101 | } |
|
117 | } | |
102 |
|
118 | |||
103 | void matrix_average(volatile int *spectral_matrix, float *averaged_spectral_matrix) |
|
119 | void matrix_average(volatile int *spectral_matrix, float *averaged_spectral_matrix) | |
104 | { |
|
120 | { | |
105 | int i; |
|
121 | int i; | |
106 | for(i=0; i<TOTAL_SIZE_SPECTRAL_MATRIX; i++){ |
|
122 | for(i=0; i<TOTAL_SIZE_SPECTRAL_MATRIX; i++){ | |
107 | averaged_spectral_matrix[i] = averaged_spectral_matrix[i] + spectral_matrix_f0_a[i] |
|
123 | averaged_spectral_matrix[i] = averaged_spectral_matrix[i] + spectral_matrix_f0_a[i] | |
108 | + spectral_matrix_f0_b[i] |
|
124 | + spectral_matrix_f0_b[i] | |
109 | + spectral_matrix_f0_c[i] |
|
125 | + spectral_matrix_f0_c[i] | |
110 | + spectral_matrix_f0_d[i] |
|
126 | + spectral_matrix_f0_d[i] | |
111 | + spectral_matrix_f0_e[i] |
|
127 | + spectral_matrix_f0_e[i] | |
112 | + spectral_matrix_f0_f[i] |
|
128 | + spectral_matrix_f0_f[i] | |
113 | + spectral_matrix_f0_g[i] |
|
129 | + spectral_matrix_f0_g[i] | |
114 | + spectral_matrix_f0_h[i]; |
|
130 | + spectral_matrix_f0_h[i]; | |
115 | } |
|
131 | } | |
116 | } |
|
132 | } | |
117 |
|
133 | |||
118 | void matrix_reset(float *averaged_spectral_matrix) |
|
134 | void matrix_reset(float *averaged_spectral_matrix) | |
119 | { |
|
135 | { | |
120 | int i; |
|
136 | int i; | |
121 | for(i=0; i<TOTAL_SIZE_SPECTRAL_MATRIX; i++){ |
|
137 | for(i=0; i<TOTAL_SIZE_SPECTRAL_MATRIX; i++){ | |
122 | averaged_spectral_matrix_f0[i] = 0; |
|
138 | averaged_spectral_matrix_f0[i] = 0; | |
123 | } |
|
139 | } | |
124 | } |
|
140 | } | |
125 |
|
141 | |||
126 | void matrix_compression(float *averaged_spectral_matrix, unsigned char fChannel, float *compressed_spectral_matrix) |
|
142 | void matrix_compression(float *averaged_spectral_matrix, unsigned char fChannel, float *compressed_spectral_matrix) | |
127 | { |
|
143 | { | |
128 | int i, j; |
|
144 | int i, j; | |
129 | switch (fChannel){ |
|
145 | switch (fChannel){ | |
130 | case 0: |
|
146 | case 0: | |
131 | for(i=0;i<NB_BINS_COMPRESSED_MATRIX_f0;i++){ |
|
147 | for(i=0;i<NB_BINS_COMPRESSED_MATRIX_f0;i++){ | |
132 | j = 17 + i * 8; |
|
148 | j = 17 + i * 8; | |
133 | compressed_spectral_matrix[i] = (averaged_spectral_matrix[j] |
|
149 | compressed_spectral_matrix[i] = (averaged_spectral_matrix[j] | |
134 | + averaged_spectral_matrix[j+1] |
|
150 | + averaged_spectral_matrix[j+1] | |
135 | + averaged_spectral_matrix[j+2] |
|
151 | + averaged_spectral_matrix[j+2] | |
136 | + averaged_spectral_matrix[j+3] |
|
152 | + averaged_spectral_matrix[j+3] | |
137 | + averaged_spectral_matrix[j+4] |
|
153 | + averaged_spectral_matrix[j+4] | |
138 | + averaged_spectral_matrix[j+5] |
|
154 | + averaged_spectral_matrix[j+5] | |
139 | + averaged_spectral_matrix[j+6] |
|
155 | + averaged_spectral_matrix[j+6] | |
140 | + averaged_spectral_matrix[j+7])/(8*NB_AVERAGE_NORMAL_f0); |
|
156 | + averaged_spectral_matrix[j+7])/(8*NB_AVERAGE_NORMAL_f0); | |
141 | } |
|
157 | } | |
142 | break; |
|
158 | break; | |
143 | case 1: |
|
159 | case 1: | |
144 | // case fChannel = f1 tp be completed later |
|
160 | // case fChannel = f1 tp be completed later | |
145 | break; |
|
161 | break; | |
146 | case 2: |
|
162 | case 2: | |
147 | // case fChannel = f1 tp be completed later |
|
163 | // case fChannel = f1 tp be completed later | |
148 | break; |
|
164 | break; | |
149 | default: |
|
165 | default: | |
150 | break; |
|
166 | break; | |
151 | } |
|
167 | } | |
152 | } |
|
168 | } | |
153 |
|
169 | |||
154 | void BP1_set(float * compressed_spectral_matrix, unsigned char nb_bins_compressed_spectral_matrix, unsigned char * LFR_BP1){ |
|
170 | void BP1_set(float * compressed_spectral_matrix, unsigned char nb_bins_compressed_spectral_matrix, unsigned char * LFR_BP1){ | |
155 | int i, j; |
|
171 | int i, j; | |
156 | unsigned char tmp_u_char; |
|
172 | unsigned char tmp_u_char; | |
157 | unsigned char * pt_char; |
|
173 | unsigned char * pt_char; | |
158 | float PSDB, PSDE; |
|
174 | float PSDB, PSDE; | |
159 | float NVEC_V0, NVEC_V1, NVEC_V2; |
|
175 | float NVEC_V0, NVEC_V1, NVEC_V2; | |
160 | float significand; |
|
176 | float significand; | |
161 | int exponent; |
|
177 | int exponent; | |
162 | float aux, tr_SB_SB, tmp; |
|
178 | float aux, tr_SB_SB, tmp; | |
163 | float e_cross_b_re, e_cross_b_im; |
|
179 | float e_cross_b_re, e_cross_b_im; | |
164 | float n_cross_e_scal_b_re = 0, n_cross_e_scal_b_im = 0; |
|
180 | float n_cross_e_scal_b_re = 0, n_cross_e_scal_b_im = 0; | |
165 | float nx = 0, ny = 0; |
|
181 | float nx = 0, ny = 0; | |
166 | float bz_bz_star = 0; |
|
182 | float bz_bz_star = 0; | |
167 | for(i=0; i<nb_bins_compressed_spectral_matrix; i++){ |
|
183 | for(i=0; i<nb_bins_compressed_spectral_matrix; i++){ | |
168 | //============================================== |
|
184 | //============================================== | |
169 | // BP1 PSD == B PAR_LFR_SC_BP1_PE_FL0 == 16 bits |
|
185 | // BP1 PSD == B PAR_LFR_SC_BP1_PE_FL0 == 16 bits | |
170 | PSDB = compressed_spectral_matrix[i*30] // S11 |
|
186 | PSDB = compressed_spectral_matrix[i*30] // S11 | |
171 | + compressed_spectral_matrix[i*30+10] // S22 |
|
187 | + compressed_spectral_matrix[i*30+10] // S22 | |
172 | + compressed_spectral_matrix[i*30+18]; // S33 |
|
188 | + compressed_spectral_matrix[i*30+18]; // S33 | |
173 | significand = frexp(PSDB, &exponent); |
|
189 | significand = frexp(PSDB, &exponent); | |
174 | pt_char = (unsigned char*) &PSDB; |
|
190 | pt_char = (unsigned char*) &PSDB; | |
175 | LFR_BP1[i*9+8] = pt_char[0]; // bits 31 downto 24 of the float |
|
191 | LFR_BP1[i*9+8] = pt_char[0]; // bits 31 downto 24 of the float | |
176 | LFR_BP1[i*9+7] = pt_char[1]; // bits 23 downto 16 of the float |
|
192 | LFR_BP1[i*9+7] = pt_char[1]; // bits 23 downto 16 of the float | |
177 | //============================================== |
|
193 | //============================================== | |
178 | // BP1 PSD == E PAR_LFR_SC_BP1_PB_FL0 == 16 bits |
|
194 | // BP1 PSD == E PAR_LFR_SC_BP1_PB_FL0 == 16 bits | |
179 | PSDE = compressed_spectral_matrix[i*30+24] * k44 // S44 |
|
195 | PSDE = compressed_spectral_matrix[i*30+24] * k44 // S44 | |
180 | + compressed_spectral_matrix[i*30+28] * k55 // S55 |
|
196 | + compressed_spectral_matrix[i*30+28] * k55 // S55 | |
181 | + compressed_spectral_matrix[i*30+26] * k45_re // S45 |
|
197 | + compressed_spectral_matrix[i*30+26] * k45_re // S45 | |
182 | - compressed_spectral_matrix[i*30+27] * k45_im; // S45 |
|
198 | - compressed_spectral_matrix[i*30+27] * k45_im; // S45 | |
183 | pt_char = (unsigned char*) &PSDE; |
|
199 | pt_char = (unsigned char*) &PSDE; | |
184 | LFR_BP1[i*9+6] = pt_char[0]; // bits 31 downto 24 of the float |
|
200 | LFR_BP1[i*9+6] = pt_char[0]; // bits 31 downto 24 of the float | |
185 | LFR_BP1[i*9+5] = pt_char[1]; // bits 23 downto 16 of the float |
|
201 | LFR_BP1[i*9+5] = pt_char[1]; // bits 23 downto 16 of the float | |
186 | //============================================================================== |
|
202 | //============================================================================== | |
187 | // BP1 normal wave vector == PAR_LFR_SC_BP1_NVEC_V0_F0 == 8 bits |
|
203 | // BP1 normal wave vector == PAR_LFR_SC_BP1_NVEC_V0_F0 == 8 bits | |
188 | // == PAR_LFR_SC_BP1_NVEC_V1_F0 == 8 bits |
|
204 | // == PAR_LFR_SC_BP1_NVEC_V1_F0 == 8 bits | |
189 | // == PAR_LFR_SC_BP1_NVEC_V2_F0 == 1 bits |
|
205 | // == PAR_LFR_SC_BP1_NVEC_V2_F0 == 1 bits | |
190 | tmp = sqrt( |
|
206 | tmp = sqrt( | |
191 | compressed_spectral_matrix[i*30+3]*compressed_spectral_matrix[i*30+3] //Im S12 |
|
207 | compressed_spectral_matrix[i*30+3]*compressed_spectral_matrix[i*30+3] //Im S12 | |
192 | +compressed_spectral_matrix[i*30+5]*compressed_spectral_matrix[i*30+5] //Im S13 |
|
208 | +compressed_spectral_matrix[i*30+5]*compressed_spectral_matrix[i*30+5] //Im S13 | |
193 | +compressed_spectral_matrix[i*30+5]*compressed_spectral_matrix[i*30+13] //Im S23 |
|
209 | +compressed_spectral_matrix[i*30+5]*compressed_spectral_matrix[i*30+13] //Im S23 | |
194 | ); |
|
210 | ); | |
195 | NVEC_V0 = compressed_spectral_matrix[i*30+13] / tmp; // Im S23 |
|
211 | NVEC_V0 = compressed_spectral_matrix[i*30+13] / tmp; // Im S23 | |
196 | NVEC_V1 = -compressed_spectral_matrix[i*30+5] / tmp; // Im S13 |
|
212 | NVEC_V1 = -compressed_spectral_matrix[i*30+5] / tmp; // Im S13 | |
197 | NVEC_V2 = compressed_spectral_matrix[i*30+1] / tmp; // Im S12 |
|
213 | NVEC_V2 = compressed_spectral_matrix[i*30+1] / tmp; // Im S12 | |
198 | LFR_BP1[i*9+4] = (char) (NVEC_V0*256); |
|
214 | LFR_BP1[i*9+4] = (char) (NVEC_V0*256); | |
199 | LFR_BP1[i*9+3] = (char) (NVEC_V1*256); |
|
215 | LFR_BP1[i*9+3] = (char) (NVEC_V1*256); | |
200 | pt_char = (unsigned char*) &NVEC_V2; |
|
216 | pt_char = (unsigned char*) &NVEC_V2; | |
201 | LFR_BP1[i*9+2] = pt_char[0] & 0x80; // extract the sign of NVEC_V2 |
|
217 | LFR_BP1[i*9+2] = pt_char[0] & 0x80; // extract the sign of NVEC_V2 | |
202 | //======================================================= |
|
218 | //======================================================= | |
203 | // BP1 ellipticity == PAR_LFR_SC_BP1_ELLIP_F0 == 4 bits |
|
219 | // BP1 ellipticity == PAR_LFR_SC_BP1_ELLIP_F0 == 4 bits | |
204 | aux = 2*tmp / PSDB; // compute the ellipticity |
|
220 | aux = 2*tmp / PSDB; // compute the ellipticity | |
205 | tmp_u_char = (unsigned char) (aux*(16-1)); // convert the ellipticity |
|
221 | tmp_u_char = (unsigned char) (aux*(16-1)); // convert the ellipticity | |
206 | LFR_BP1[i*9+2] = LFR_BP1[i*9+2] | ((tmp_u_char&0x0f)<<3); // keeps 4 bits of the resulting unsigned char |
|
222 | LFR_BP1[i*9+2] = LFR_BP1[i*9+2] | ((tmp_u_char&0x0f)<<3); // keeps 4 bits of the resulting unsigned char | |
207 | //============================================================== |
|
223 | //============================================================== | |
208 | // BP1 degree of polarization == PAR_LFR_SC_BP1_DOP_F0 == 3 bits |
|
224 | // BP1 degree of polarization == PAR_LFR_SC_BP1_DOP_F0 == 3 bits | |
209 | for(j = 0; j<NB_VALUES_PER_SPECTRAL_MATRIX;j++){ |
|
225 | for(j = 0; j<NB_VALUES_PER_SPECTRAL_MATRIX;j++){ | |
210 | tr_SB_SB = compressed_spectral_matrix[i*30]*compressed_spectral_matrix[i*30] |
|
226 | tr_SB_SB = compressed_spectral_matrix[i*30]*compressed_spectral_matrix[i*30] | |
211 | + compressed_spectral_matrix[i*30+10]*compressed_spectral_matrix[i*30+10] |
|
227 | + compressed_spectral_matrix[i*30+10]*compressed_spectral_matrix[i*30+10] | |
212 | + compressed_spectral_matrix[i*30+18]*compressed_spectral_matrix[i*30+18] |
|
228 | + compressed_spectral_matrix[i*30+18]*compressed_spectral_matrix[i*30+18] | |
213 | + 2 * compressed_spectral_matrix[i*30+2]*compressed_spectral_matrix[i*30+2] |
|
229 | + 2 * compressed_spectral_matrix[i*30+2]*compressed_spectral_matrix[i*30+2] | |
214 | + 2 * compressed_spectral_matrix[i*30+3]*compressed_spectral_matrix[i*30+3] |
|
230 | + 2 * compressed_spectral_matrix[i*30+3]*compressed_spectral_matrix[i*30+3] | |
215 | + 2 * compressed_spectral_matrix[i*30+4]*compressed_spectral_matrix[i*30+4] |
|
231 | + 2 * compressed_spectral_matrix[i*30+4]*compressed_spectral_matrix[i*30+4] | |
216 | + 2 * compressed_spectral_matrix[i*30+5]*compressed_spectral_matrix[i*30+5] |
|
232 | + 2 * compressed_spectral_matrix[i*30+5]*compressed_spectral_matrix[i*30+5] | |
217 | + 2 * compressed_spectral_matrix[i*30+12]*compressed_spectral_matrix[i*30+12] |
|
233 | + 2 * compressed_spectral_matrix[i*30+12]*compressed_spectral_matrix[i*30+12] | |
218 | + 2 * compressed_spectral_matrix[i*30+13]*compressed_spectral_matrix[i*30+13]; |
|
234 | + 2 * compressed_spectral_matrix[i*30+13]*compressed_spectral_matrix[i*30+13]; | |
219 | } |
|
235 | } | |
220 | aux = PSDB*PSDB; |
|
236 | aux = PSDB*PSDB; | |
221 | tmp = ( 3*tr_SB_SB - aux ) / ( 2 * aux ); |
|
237 | tmp = ( 3*tr_SB_SB - aux ) / ( 2 * aux ); | |
222 | tmp_u_char = (unsigned char) (NVEC_V0*(8-1)); |
|
238 | tmp_u_char = (unsigned char) (NVEC_V0*(8-1)); | |
223 | LFR_BP1[i*9+2] = LFR_BP1[i*9+2] | ((tmp_u_char&0x07)); // keeps 3 bits of the resulting unsigned char |
|
239 | LFR_BP1[i*9+2] = LFR_BP1[i*9+2] | ((tmp_u_char&0x07)); // keeps 3 bits of the resulting unsigned char | |
224 | //======================================================================================= |
|
240 | //======================================================================================= | |
225 | // BP1 z-component of the normalized Poynting flux == PAR_LFR_SC_BP1_SZ_F0 == 8 bits (7+1) |
|
241 | // BP1 z-component of the normalized Poynting flux == PAR_LFR_SC_BP1_SZ_F0 == 8 bits (7+1) | |
226 | e_cross_b_re = compressed_spectral_matrix[i*30+20]*k34_re |
|
242 | e_cross_b_re = compressed_spectral_matrix[i*30+20]*k34_re | |
227 | + compressed_spectral_matrix[i*30+6]*k14_re |
|
243 | + compressed_spectral_matrix[i*30+6]*k14_re | |
228 | + compressed_spectral_matrix[i*30+8]*k15_re |
|
244 | + compressed_spectral_matrix[i*30+8]*k15_re | |
229 | + compressed_spectral_matrix[i*30+14]*k24_re |
|
245 | + compressed_spectral_matrix[i*30+14]*k24_re | |
230 | + compressed_spectral_matrix[i*30+16]*k25_re; |
|
246 | + compressed_spectral_matrix[i*30+16]*k25_re; | |
231 | e_cross_b_im = compressed_spectral_matrix[i*30+21]*k34_im |
|
247 | e_cross_b_im = compressed_spectral_matrix[i*30+21]*k34_im | |
232 | + compressed_spectral_matrix[i*30+7]*k14_im |
|
248 | + compressed_spectral_matrix[i*30+7]*k14_im | |
233 | + compressed_spectral_matrix[i*30+9]*k15_im |
|
249 | + compressed_spectral_matrix[i*30+9]*k15_im | |
234 | + compressed_spectral_matrix[i*30+15]*k24_im |
|
250 | + compressed_spectral_matrix[i*30+15]*k24_im | |
235 | + compressed_spectral_matrix[i*30+17]*k25_im; |
|
251 | + compressed_spectral_matrix[i*30+17]*k25_im; | |
236 | tmp = e_cross_b_re / PSDE; // compute ReaSz |
|
252 | tmp = e_cross_b_re / PSDE; // compute ReaSz | |
237 | LFR_BP1[i*9+1] = ((unsigned char) (tmp * 128)) & 0x7f; // is it always positive? |
|
253 | LFR_BP1[i*9+1] = ((unsigned char) (tmp * 128)) & 0x7f; // is it always positive? | |
238 | tmp = e_cross_b_re * e_cross_b_im; |
|
254 | tmp = e_cross_b_re * e_cross_b_im; | |
239 | pt_char = (unsigned char*) &tmp; |
|
255 | pt_char = (unsigned char*) &tmp; | |
240 | LFR_BP1[i*9+1] = LFR_BP1[i*9+1] | (pt_char[0] & 0x80); // extract the sign of ArgSz |
|
256 | LFR_BP1[i*9+1] = LFR_BP1[i*9+1] | (pt_char[0] & 0x80); // extract the sign of ArgSz | |
241 | //====================================================================== |
|
257 | //====================================================================== | |
242 | // BP1 phase velocity estimator == PAR_LFR_SC_BP1_VPHI_F0 == 8 bits (7+1) |
|
258 | // BP1 phase velocity estimator == PAR_LFR_SC_BP1_VPHI_F0 == 8 bits (7+1) | |
243 | nx = -sin(alpha_M)*NVEC_V0 - cos(alpha_M)*NVEC_V1; |
|
259 | nx = -sin(alpha_M)*NVEC_V0 - cos(alpha_M)*NVEC_V1; | |
244 | ny = NVEC_V2; |
|
260 | ny = NVEC_V2; | |
245 | bz_bz_star = cos(alpha_M) * cos(alpha_M) * compressed_spectral_matrix[i*30] // re S11 |
|
261 | bz_bz_star = cos(alpha_M) * cos(alpha_M) * compressed_spectral_matrix[i*30] // re S11 | |
246 | + sin(alpha_M) * sin(alpha_M) * compressed_spectral_matrix[i*30+10] // re S22 |
|
262 | + sin(alpha_M) * sin(alpha_M) * compressed_spectral_matrix[i*30+10] // re S22 | |
247 | - 2 * sin(alpha_M) * cos(alpha_M) * compressed_spectral_matrix[i*30+2]; // re S12 |
|
263 | - 2 * sin(alpha_M) * cos(alpha_M) * compressed_spectral_matrix[i*30+2]; // re S12 | |
248 | n_cross_e_scal_b_re = nx * (compressed_spectral_matrix[i*30+8]*k15_bis_re |
|
264 | n_cross_e_scal_b_re = nx * (compressed_spectral_matrix[i*30+8]*k15_bis_re | |
249 | +compressed_spectral_matrix[i*30+6]*k14_bis_re |
|
265 | +compressed_spectral_matrix[i*30+6]*k14_bis_re | |
250 | +compressed_spectral_matrix[i*30+16]*k25_bis_re |
|
266 | +compressed_spectral_matrix[i*30+16]*k25_bis_re | |
251 | +compressed_spectral_matrix[i*30+14]*k24_bis_re) |
|
267 | +compressed_spectral_matrix[i*30+14]*k24_bis_re) | |
252 | + ny * (compressed_spectral_matrix[i*30+6]*k14_tris_re |
|
268 | + ny * (compressed_spectral_matrix[i*30+6]*k14_tris_re | |
253 | +compressed_spectral_matrix[i*30+14]*k24_tris_re); |
|
269 | +compressed_spectral_matrix[i*30+14]*k24_tris_re); | |
254 | n_cross_e_scal_b_im = nx * (compressed_spectral_matrix[i*30+8]*k15_bis_im |
|
270 | n_cross_e_scal_b_im = nx * (compressed_spectral_matrix[i*30+8]*k15_bis_im | |
255 | +compressed_spectral_matrix[i*30+6]*k14_bis_im |
|
271 | +compressed_spectral_matrix[i*30+6]*k14_bis_im | |
256 | +compressed_spectral_matrix[i*30+16]*k25_bis_im |
|
272 | +compressed_spectral_matrix[i*30+16]*k25_bis_im | |
257 | +compressed_spectral_matrix[i*30+14]*k24_bis_im) |
|
273 | +compressed_spectral_matrix[i*30+14]*k24_bis_im) | |
258 | + ny * (compressed_spectral_matrix[i*30+6]*k14_tris_im |
|
274 | + ny * (compressed_spectral_matrix[i*30+6]*k14_tris_im | |
259 | +compressed_spectral_matrix[i*30+14]*k24_tris_im); |
|
275 | +compressed_spectral_matrix[i*30+14]*k24_tris_im); | |
260 | tmp = n_cross_e_scal_b_re / bz_bz_star; |
|
276 | tmp = n_cross_e_scal_b_re / bz_bz_star; | |
261 | LFR_BP1[i*9+0] = ((unsigned char) (tmp * 128)) & 0x7f; // is it always positive? |
|
277 | LFR_BP1[i*9+0] = ((unsigned char) (tmp * 128)) & 0x7f; // is it always positive? | |
262 | tmp = n_cross_e_scal_b_re * n_cross_e_scal_b_im; |
|
278 | tmp = n_cross_e_scal_b_re * n_cross_e_scal_b_im; | |
263 | pt_char = (unsigned char*) &tmp; |
|
279 | pt_char = (unsigned char*) &tmp; | |
264 | LFR_BP1[i*9+1] = LFR_BP1[i*9+0] | (pt_char[0] & 0x80); // extract the sign of ArgV |
|
280 | LFR_BP1[i*9+1] = LFR_BP1[i*9+0] | (pt_char[0] & 0x80); // extract the sign of ArgV | |
265 | } |
|
281 | } | |
266 |
|
282 | |||
267 | } |
|
283 | } | |
268 |
|
284 | |||
269 | void BP2_set(float * compressed_spectral_matrix, unsigned char nb_bins_compressed_spectral_matrix){ |
|
285 | void BP2_set(float * compressed_spectral_matrix, unsigned char nb_bins_compressed_spectral_matrix){ | |
270 | // BP2 autocorrelation |
|
286 | // BP2 autocorrelation | |
271 | int i, aux = 0; |
|
287 | int i, aux = 0; | |
272 | for(i = 0; i<nb_bins_compressed_spectral_matrix; i++){ |
|
288 | for(i = 0; i<nb_bins_compressed_spectral_matrix; i++){ | |
273 | // S12 |
|
289 | // S12 | |
274 | aux = sqrt(compressed_spectral_matrix[i*30]*compressed_spectral_matrix[i*30+10]); |
|
290 | aux = sqrt(compressed_spectral_matrix[i*30]*compressed_spectral_matrix[i*30+10]); | |
275 | compressed_spectral_matrix[i*30+2] = compressed_spectral_matrix[i*30+2] / aux; |
|
291 | compressed_spectral_matrix[i*30+2] = compressed_spectral_matrix[i*30+2] / aux; | |
276 | compressed_spectral_matrix[i*30+3] = compressed_spectral_matrix[i*30+3] / aux; |
|
292 | compressed_spectral_matrix[i*30+3] = compressed_spectral_matrix[i*30+3] / aux; | |
277 | // S13 |
|
293 | // S13 | |
278 | aux = sqrt(compressed_spectral_matrix[i*30]*compressed_spectral_matrix[i*30+18]); |
|
294 | aux = sqrt(compressed_spectral_matrix[i*30]*compressed_spectral_matrix[i*30+18]); | |
279 | compressed_spectral_matrix[i*30+4] = compressed_spectral_matrix[i*30+4] / aux; |
|
295 | compressed_spectral_matrix[i*30+4] = compressed_spectral_matrix[i*30+4] / aux; | |
280 | compressed_spectral_matrix[i*30+5] = compressed_spectral_matrix[i*30+5] / aux; |
|
296 | compressed_spectral_matrix[i*30+5] = compressed_spectral_matrix[i*30+5] / aux; | |
281 | // S23 |
|
297 | // S23 | |
282 | aux = sqrt(compressed_spectral_matrix[i*30+12]*compressed_spectral_matrix[i*30+18]); |
|
298 | aux = sqrt(compressed_spectral_matrix[i*30+12]*compressed_spectral_matrix[i*30+18]); | |
283 | compressed_spectral_matrix[i*30+12] = compressed_spectral_matrix[i*30+12] / aux; |
|
299 | compressed_spectral_matrix[i*30+12] = compressed_spectral_matrix[i*30+12] / aux; | |
284 | compressed_spectral_matrix[i*30+13] = compressed_spectral_matrix[i*30+13] / aux; |
|
300 | compressed_spectral_matrix[i*30+13] = compressed_spectral_matrix[i*30+13] / aux; | |
285 | // S45 |
|
301 | // S45 | |
286 | aux = sqrt(compressed_spectral_matrix[i*30+24]*compressed_spectral_matrix[i*30+28]); |
|
302 | aux = sqrt(compressed_spectral_matrix[i*30+24]*compressed_spectral_matrix[i*30+28]); | |
287 | compressed_spectral_matrix[i*30+26] = compressed_spectral_matrix[i*30+26] / aux; |
|
303 | compressed_spectral_matrix[i*30+26] = compressed_spectral_matrix[i*30+26] / aux; | |
288 | compressed_spectral_matrix[i*30+27] = compressed_spectral_matrix[i*30+27] / aux; |
|
304 | compressed_spectral_matrix[i*30+27] = compressed_spectral_matrix[i*30+27] / aux; | |
289 | // S14 |
|
305 | // S14 | |
290 | aux = sqrt(compressed_spectral_matrix[i*30]*compressed_spectral_matrix[i*30+24]); |
|
306 | aux = sqrt(compressed_spectral_matrix[i*30]*compressed_spectral_matrix[i*30+24]); | |
291 | compressed_spectral_matrix[i*30+6] = compressed_spectral_matrix[i*30+6] / aux; |
|
307 | compressed_spectral_matrix[i*30+6] = compressed_spectral_matrix[i*30+6] / aux; | |
292 | compressed_spectral_matrix[i*30+7] = compressed_spectral_matrix[i*30+7] / aux; |
|
308 | compressed_spectral_matrix[i*30+7] = compressed_spectral_matrix[i*30+7] / aux; | |
293 | // S15 |
|
309 | // S15 | |
294 | aux = sqrt(compressed_spectral_matrix[i*30]*compressed_spectral_matrix[i*30+28]); |
|
310 | aux = sqrt(compressed_spectral_matrix[i*30]*compressed_spectral_matrix[i*30+28]); | |
295 | compressed_spectral_matrix[i*30+8] = compressed_spectral_matrix[i*30+8] / aux; |
|
311 | compressed_spectral_matrix[i*30+8] = compressed_spectral_matrix[i*30+8] / aux; | |
296 | compressed_spectral_matrix[i*30+9] = compressed_spectral_matrix[i*30+9] / aux; |
|
312 | compressed_spectral_matrix[i*30+9] = compressed_spectral_matrix[i*30+9] / aux; | |
297 | // S24 |
|
313 | // S24 | |
298 | aux = sqrt(compressed_spectral_matrix[i*10]*compressed_spectral_matrix[i*30+24]); |
|
314 | aux = sqrt(compressed_spectral_matrix[i*10]*compressed_spectral_matrix[i*30+24]); | |
299 | compressed_spectral_matrix[i*30+14] = compressed_spectral_matrix[i*30+14] / aux; |
|
315 | compressed_spectral_matrix[i*30+14] = compressed_spectral_matrix[i*30+14] / aux; | |
300 | compressed_spectral_matrix[i*30+15] = compressed_spectral_matrix[i*30+15] / aux; |
|
316 | compressed_spectral_matrix[i*30+15] = compressed_spectral_matrix[i*30+15] / aux; | |
301 | // S25 |
|
317 | // S25 | |
302 | aux = sqrt(compressed_spectral_matrix[i*10]*compressed_spectral_matrix[i*30+28]); |
|
318 | aux = sqrt(compressed_spectral_matrix[i*10]*compressed_spectral_matrix[i*30+28]); | |
303 | compressed_spectral_matrix[i*30+16] = compressed_spectral_matrix[i*30+16] / aux; |
|
319 | compressed_spectral_matrix[i*30+16] = compressed_spectral_matrix[i*30+16] / aux; | |
304 | compressed_spectral_matrix[i*30+17] = compressed_spectral_matrix[i*30+17] / aux; |
|
320 | compressed_spectral_matrix[i*30+17] = compressed_spectral_matrix[i*30+17] / aux; | |
305 | // S34 |
|
321 | // S34 | |
306 | aux = sqrt(compressed_spectral_matrix[i*18]*compressed_spectral_matrix[i*30+24]); |
|
322 | aux = sqrt(compressed_spectral_matrix[i*18]*compressed_spectral_matrix[i*30+24]); | |
307 | compressed_spectral_matrix[i*30+20] = compressed_spectral_matrix[i*30+20] / aux; |
|
323 | compressed_spectral_matrix[i*30+20] = compressed_spectral_matrix[i*30+20] / aux; | |
308 | compressed_spectral_matrix[i*30+21] = compressed_spectral_matrix[i*30+21] / aux; |
|
324 | compressed_spectral_matrix[i*30+21] = compressed_spectral_matrix[i*30+21] / aux; | |
309 | // S35 |
|
325 | // S35 | |
310 | aux = sqrt(compressed_spectral_matrix[i*18]*compressed_spectral_matrix[i*30+28]); |
|
326 | aux = sqrt(compressed_spectral_matrix[i*18]*compressed_spectral_matrix[i*30+28]); | |
311 | compressed_spectral_matrix[i*30+22] = compressed_spectral_matrix[i*30+22] / aux; |
|
327 | compressed_spectral_matrix[i*30+22] = compressed_spectral_matrix[i*30+22] / aux; | |
312 | compressed_spectral_matrix[i*30+23] = compressed_spectral_matrix[i*30+23] / aux; |
|
328 | compressed_spectral_matrix[i*30+23] = compressed_spectral_matrix[i*30+23] / aux; | |
313 | } |
|
329 | } | |
314 | } |
|
330 | } | |
315 |
|
331 | |||
|
332 | rtems_task spw_avf0_task(rtems_task_argument argument){ | |||
|
333 | int i; | |||
|
334 | static int nb_average; | |||
|
335 | rtems_event_set event_out; | |||
|
336 | rtems_status_code status; | |||
|
337 | ||||
|
338 | spectral_matrices_regs = (struct spectral_matrices_regs_str *) REGS_ADDRESS_SPECTRAL_MATRICES; | |||
|
339 | spectral_matrices_regs->address0 = (volatile int) spectral_matrix_f0_a; | |||
|
340 | spectral_matrices_regs->address1 = (volatile int) spectral_matrix_f0_b; | |||
|
341 | ||||
|
342 | nb_average = 0; | |||
|
343 | ||||
|
344 | PRINTF("In AVFO *** \n") | |||
|
345 | ||||
|
346 | while(1){ | |||
|
347 | rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0 | |||
|
348 | for(i=0; i<TOTAL_SIZE_SPECTRAL_MATRIX; i++){ | |||
|
349 | averaged_spectral_matrix_f0[i] = averaged_spectral_matrix_f0[i] + spectral_matrix_f0_a[i] | |||
|
350 | + spectral_matrix_f0_b[i] | |||
|
351 | + spectral_matrix_f0_c[i] | |||
|
352 | + spectral_matrix_f0_d[i] | |||
|
353 | + spectral_matrix_f0_e[i] | |||
|
354 | + spectral_matrix_f0_f[i] | |||
|
355 | + spectral_matrix_f0_g[i] | |||
|
356 | + spectral_matrix_f0_h[i]; | |||
|
357 | } | |||
|
358 | spectral_matrices_regs->ctrl = spectral_matrices_regs->ctrl & 0xfffffffe; // reset the appropriate bit in the register | |||
|
359 | nb_average = nb_average + NB_SM_TO_RECEIVE_BEFORE_AVF0; | |||
|
360 | if (nb_average == NB_AVERAGE_NORMAL_f0) { | |||
|
361 | nb_average = 0; | |||
|
362 | status = rtems_event_send( Task_id[7], RTEMS_EVENT_0 ); // sending an event to the task 7, BPF0 | |||
|
363 | if (status != RTEMS_SUCCESSFUL) printf("IN TASK AVF0 *** Error sending RTEMS_EVENT_0, code %d\n", status); | |||
|
364 | } | |||
|
365 | } | |||
|
366 | } | |||
|
367 | ||||
|
368 | rtems_task spw_bpf0_task(rtems_task_argument argument){ | |||
|
369 | rtems_event_set event_out; | |||
|
370 | ||||
|
371 | while(1){ | |||
|
372 | rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0 | |||
|
373 | matrix_compression(averaged_spectral_matrix_f0, 0, compressed_spectral_matrix_f0); | |||
|
374 | BP1_set(compressed_spectral_matrix_f0, NB_BINS_COMPRESSED_MATRIX_f0, LFR_BP1_F0); | |||
|
375 | //PRINTF("IN TASK BPF0 *** Matrix compressed, parameters calculated\n") | |||
|
376 | } | |||
|
377 | } | |||
|
378 | ||||
|
379 | //******* | |||
|
380 | // UNUSED | |||
316 | rtems_task spw_bppr_task_rate_monotonic(rtems_task_argument argument) |
|
381 | rtems_task spw_bppr_task_rate_monotonic(rtems_task_argument argument) | |
317 | { |
|
382 | {/* | |
318 | rtems_status_code status; |
|
383 | rtems_status_code status; | |
319 | //static int nb_average_f1 = 0; |
|
384 | //static int nb_average_f1 = 0; | |
320 | //static int nb_average_f2 = 0; |
|
385 | //static int nb_average_f2 = 0; | |
321 |
|
|
386 | ||
322 | rtems_name name; |
|
387 | rtems_name name; | |
323 | rtems_id period; |
|
388 | rtems_id period; | |
324 | name = rtems_build_name( 'P', 'E', 'R', 'D' ); |
|
389 | name = rtems_build_name( 'P', 'E', 'R', 'D' ); | |
325 | status = rtems_rate_monotonic_create( name, &period ); |
|
390 | status = rtems_rate_monotonic_create( name, &period ); | |
326 | if( status != RTEMS_SUCCESSFUL ) { |
|
391 | if( status != RTEMS_SUCCESSFUL ) { | |
327 | printf( "rtems_rate_monotonic_create failed with status of %d\n", status ); |
|
392 | printf( "rtems_rate_monotonic_create failed with status of %d\n", status ); | |
328 | //exit( 1 ); |
|
393 | //exit( 1 ); | |
329 | } |
|
394 | } | |
330 |
|
|
395 | ||
331 |
spectral_matrices_regs = (struct spectral_matrices_regs_str *) ADDRESS |
|
396 | spectral_matrices_regs = (struct spectral_matrices_regs_str *) REGS_ADDRESS_SPECTRAL_MATRICES; | |
332 | spectral_matrices_regs->address0 = (volatile int) spectral_matrix_f0_a; |
|
397 | spectral_matrices_regs->address0 = (volatile int) spectral_matrix_f0_a; | |
333 | spectral_matrices_regs->address1 = (volatile int) spectral_matrix_f0_b; |
|
398 | spectral_matrices_regs->address1 = (volatile int) spectral_matrix_f0_b; | |
334 |
|
|
399 | ||
335 | printf("In BPPR BIS ***\n"); |
|
400 | printf("In BPPR BIS ***\n"); | |
336 |
|
|
401 | ||
337 | while(1){ // launch the rate monotonic task |
|
402 | while(1){ // launch the rate monotonic task | |
338 | if ( rtems_rate_monotonic_period( period, 8 ) == RTEMS_TIMEOUT ){ |
|
403 | if ( rtems_rate_monotonic_period( period, 8 ) == RTEMS_TIMEOUT ){ | |
339 | printf("TIMEOUT\n"); |
|
404 | printf("TIMEOUT\n"); | |
340 | //break; |
|
405 | //break; | |
341 | } |
|
406 | } | |
342 | status = rtems_event_send( Task_id[6], RTEMS_EVENT_0 ); // sending an event to the task 6, AVF0 |
|
407 | status = rtems_event_send( Task_id[6], RTEMS_EVENT_0 ); // sending an event to the task 6, AVF0 | |
343 | if (status != RTEMS_SUCCESSFUL) printf("IN TASK BPPR BIS *** Error sending RTEMS_EVENT_0 to AVF0, code %d\n", status); |
|
408 | if (status != RTEMS_SUCCESSFUL) printf("IN TASK BPPR BIS *** Error sending RTEMS_EVENT_0 to AVF0, code %d\n", status); | |
344 | } |
|
409 | } | |
345 |
|
|
410 | ||
346 | status = rtems_rate_monotonic_delete( period ); |
|
411 | status = rtems_rate_monotonic_delete( period ); | |
347 | if ( status != RTEMS_SUCCESSFUL ) { |
|
412 | if ( status != RTEMS_SUCCESSFUL ) { | |
348 | printf( "rtems_rate_monotonic_delete failed with status of %d.\n", status ); |
|
413 | printf( "rtems_rate_monotonic_delete failed with status of %d.\n", status ); | |
349 | //exit( 1 ); |
|
414 | //exit( 1 ); | |
350 | } |
|
415 | } | |
351 |
status = rtems_task_delete( RTEMS_SELF ); / |
|
416 | status = rtems_task_delete( RTEMS_SELF ); // should not return | |
352 | printf( "rtems_task_delete returned with status of %d.\n", status ); |
|
417 | printf( "rtems_task_delete returned with status of %d.\n", status ); | |
353 |
|
|
418 | //exit( 1 );*/ | |
354 | } |
|
419 | } | |
355 |
|
420 | |||
356 | rtems_task spw_avf0_task(rtems_task_argument argument){ |
|
421 | ||
357 | int i; |
|
422 | ||
358 | static int nb_average; |
|
|||
359 | rtems_event_set event_out; |
|
|||
360 | rtems_status_code status; |
|
|||
361 |
|
||||
362 | nb_average = 0; |
|
|||
363 |
|
423 | |||
364 | while(1){ |
|
|||
365 | rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0 |
|
|||
366 | for(i=0; i<TOTAL_SIZE_SPECTRAL_MATRIX; i++){ |
|
|||
367 | averaged_spectral_matrix_f0[i] = averaged_spectral_matrix_f0[i] + spectral_matrix_f0_a[i] |
|
|||
368 | + spectral_matrix_f0_b[i] |
|
|||
369 | + spectral_matrix_f0_c[i] |
|
|||
370 | + spectral_matrix_f0_d[i] |
|
|||
371 | + spectral_matrix_f0_e[i] |
|
|||
372 | + spectral_matrix_f0_f[i] |
|
|||
373 | + spectral_matrix_f0_g[i] |
|
|||
374 | + spectral_matrix_f0_h[i]; |
|
|||
375 | } |
|
|||
376 | spectral_matrices_regs->ctrl = spectral_matrices_regs->ctrl & 0xfffffffe; // reset the appropriate bit in the register |
|
|||
377 | nb_average = nb_average + 8; |
|
|||
378 | if (nb_average == NB_AVERAGE_NORMAL_f0) { |
|
|||
379 | nb_average = 0; |
|
|||
380 | status = rtems_event_send( Task_id[7], RTEMS_EVENT_0 ); // sending an event to the task 7, BPF0 |
|
|||
381 | if (status != RTEMS_SUCCESSFUL) printf("IN TASK AVF0 *** Error sending RTEMS_EVENT_0, code %d\n", status); |
|
|||
382 | } |
|
|||
383 | } |
|
|||
384 | } |
|
|||
385 |
|
||||
386 | rtems_task spw_bpf0_task(rtems_task_argument argument){ |
|
|||
387 | rtems_event_set event_out; |
|
|||
388 |
|
||||
389 | while(1){ |
|
|||
390 | rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0 |
|
|||
391 | matrix_compression(averaged_spectral_matrix_f0, 0, compressed_spectral_matrix_f0); |
|
|||
392 | BP1_set(compressed_spectral_matrix_f0, NB_BINS_COMPRESSED_MATRIX_f0, LFR_BP1_F0); |
|
|||
393 | printf("IN TASK BPF0 *** Matrix compressed, parameters calculated\n"); |
|
|||
394 | } |
|
|||
395 | } |
|
|||
396 |
|
||||
397 |
|
This diff has been collapsed as it changes many lines, (579 lines changed) Show them Hide them | |||||
@@ -1,460 +1,535 | |||||
1 | //************************* |
|
1 | //************************* | |
2 | // GPL reminder to be added |
|
2 | // GPL reminder to be added | |
3 | //************************* |
|
3 | //************************* | |
4 |
|
4 | |||
5 | //#define PRINT_MESSAGES_ON_CONSOLE // enable or disable the printf instructions |
|
|||
6 | #ifdef PRINT_MESSAGES_ON_CONSOLE |
|
|||
7 | #define PRINTF(x) printf(x); |
|
|||
8 | #define PRINTF1(x,y) printf(x,y); |
|
|||
9 | #define PRINTF2(x,y,z) printf(x,y,z); |
|
|||
10 | #else |
|
|||
11 | #define PRINTF(x) ; |
|
|||
12 | #define PRINTF1(x,y) ; |
|
|||
13 | #define PRINTF2(x,y,z) ; |
|
|||
14 | #endif |
|
|||
15 |
|
||||
16 | #define OFFSET_COARSE_TIME 4+10-1 // -1 => the receiver suppresses the target logical address |
|
5 | #define OFFSET_COARSE_TIME 4+10-1 // -1 => the receiver suppresses the target logical address | |
17 | #define OFFSET_FINE_TIME OFFSET_COARSE_TIME+4 |
|
6 | #define OFFSET_FINE_TIME OFFSET_COARSE_TIME+4 | |
18 | #define TC_FIFO_SIZE 5 |
|
7 | #define TC_FIFO_SIZE 5 | |
19 |
|
8 | |||
20 | #define ADDRESS_APBUART_REGISTERS 0x80000100 |
|
9 | #define ADDRESS_APBUART_REGISTERS 0x80000100 | |
21 | #define ADDRESS_GRSPW_REGISTERS 0x80000500 |
|
10 | #define ADDRESS_GRSPW_REGISTERS 0x80000500 | |
22 | #define APBUART_CTRL_REG_MASK_DB 0xfffff7ff |
|
11 | #define APBUART_CTRL_REG_MASK_DB 0xfffff7ff | |
23 |
|
12 | |||
24 | #define GRSPW_DEVICE_NAME "/dev/grspw0" |
|
13 | #define GRSPW_DEVICE_NAME "/dev/grspw0" | |
25 | #define UART_DEVICE_NAME "/dev/console" |
|
14 | #define UART_DEVICE_NAME "/dev/console" | |
26 |
|
15 | |||
27 | #include <rtems.h> |
|
16 | #include <rtems.h> | |
28 |
|
17 | |||
29 | /* configuration information */ |
|
18 | /* configuration information */ | |
30 |
|
19 | |||
31 | #define CONFIGURE_INIT |
|
20 | #define CONFIGURE_INIT | |
32 | #include <bsp.h> /* for device driver prototypes */ |
|
21 | #include <bsp.h> /* for device driver prototypes */ | |
33 |
|
22 | |||
34 | /* configuration information */ |
|
23 | /* configuration information */ | |
35 |
|
24 | |||
36 | #define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER |
|
25 | #define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER | |
37 | #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER |
|
26 | #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER | |
38 |
|
27 | |||
39 | #define CONFIGURE_MAXIMUM_TASKS 10 |
|
28 | #define CONFIGURE_MAXIMUM_TASKS 10 | |
40 | #define CONFIGURE_RTEMS_INIT_TASKS_TABLE |
|
29 | #define CONFIGURE_RTEMS_INIT_TASKS_TABLE | |
41 | #define CONFIGURE_EXTRA_TASK_STACKS (3 * RTEMS_MINIMUM_STACK_SIZE) |
|
30 | #define CONFIGURE_EXTRA_TASK_STACKS (3 * RTEMS_MINIMUM_STACK_SIZE) | |
42 | #define CONFIGURE_LIBIO_MAXIMUM_FILE_DESCRIPTORS 32 |
|
31 | #define CONFIGURE_LIBIO_MAXIMUM_FILE_DESCRIPTORS 32 | |
43 | #define CONFIGURE_INIT_TASK_PRIORITY 100 |
|
32 | #define CONFIGURE_INIT_TASK_PRIORITY 100 | |
44 | #define CONFIGURE_MAXIMUM_DRIVERS 16 |
|
33 | #define CONFIGURE_MAXIMUM_DRIVERS 16 | |
45 | #define CONFIGURE_MAXIMUM_PERIODS 1 |
|
34 | #define CONFIGURE_MAXIMUM_PERIODS 1 | |
46 |
|
35 | |||
47 | #include <rtems/confdefs.h> |
|
36 | #include <rtems/confdefs.h> | |
48 |
|
37 | |||
49 | /* If --drvmgr was enabled during the configuration of the RTEMS kernel */ |
|
38 | /* If --drvmgr was enabled during the configuration of the RTEMS kernel */ | |
50 | #ifdef RTEMS_DRVMGR_STARTUP |
|
39 | #ifdef RTEMS_DRVMGR_STARTUP | |
51 | #ifdef LEON3 |
|
40 | #ifdef LEON3 | |
52 | /* Add Timer and UART Driver */ |
|
41 | /* Add Timer and UART Driver */ | |
53 | #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER |
|
42 | #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER | |
54 | #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER |
|
43 | #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER | |
55 | #endif |
|
44 | #endif | |
56 | #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER |
|
45 | #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER | |
57 | #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART |
|
46 | #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART | |
58 | #endif |
|
47 | #endif | |
59 | #endif |
|
48 | #endif | |
60 | #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */ |
|
49 | #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */ | |
61 | #include <drvmgr/drvmgr_confdefs.h> |
|
50 | #include <drvmgr/drvmgr_confdefs.h> | |
62 | #endif |
|
51 | #endif | |
63 |
|
52 | |||
64 | #include <rtems.h> |
|
53 | #include <rtems.h> | |
65 | #include <errno.h> |
|
54 | #include <errno.h> | |
66 | #include <fcntl.h> |
|
55 | #include <fcntl.h> | |
67 | #include <stdio.h> |
|
56 | #include <stdio.h> | |
68 | #include <stdlib.h> |
|
57 | #include <stdlib.h> | |
69 |
|
58 | |||
70 | #include <FSW-config.c> |
|
59 | //#include <FSW-config.c> | |
71 | #include <grspw.h> |
|
60 | #include <grspw.h> | |
72 | #include <apbuart.h> |
|
61 | #include <apbuart.h> | |
|
62 | #include <FSW-config.h> | |||
73 | #include <TC_handler.h> |
|
63 | #include <TC_handler.h> | |
74 | #include <FSW-rtems-processing.h> |
|
64 | #include <FSW-rtems-processing.h> | |
|
65 | #include <grlibregs.h> | |||
75 |
|
66 | |||
76 |
char * |
|
67 | /*char *tmGeneratorMsg[2] = { "NOTHING_TO_DO", | |
77 |
" |
|
68 | "TM_GENERATED" | |
78 | "INCOR_CHECKSUM 2", |
|
69 | };*/ | |
79 | "ILL_TYPE 3", |
|
|||
80 | "ILL_SUBTYPE 4", |
|
|||
81 | "WRONG_APP_DATA 5", |
|
|||
82 | "WRONG_CMD_CODE 6", |
|
|||
83 | "CCSDS_TM_VALID 7" |
|
|||
84 | }; |
|
|||
85 |
|
70 | |||
86 | char *tmGeneratorMsg[2] = { "NOTHING_TO_DO", |
|
71 | // RTEMS TASKS | |
87 | "TM_GENERATED" |
|
|||
88 | }; |
|
|||
89 |
|
||||
90 | rtems_task Init( rtems_task_argument argument); /* forward declaration needed */ |
|
72 | rtems_task Init( rtems_task_argument argument); /* forward declaration needed */ | |
91 | rtems_task spw_recv_task(rtems_task_argument argument); |
|
73 | rtems_task spw_recv_task(rtems_task_argument argument); | |
92 | rtems_task spw_tcck_task(rtems_task_argument argument); |
|
|||
93 | rtems_task spw_spiq_task(rtems_task_argument argument); |
|
74 | rtems_task spw_spiq_task(rtems_task_argument argument); | |
94 | rtems_task spw_stat_task(rtems_task_argument argument); |
|
75 | rtems_task spw_stat_task(rtems_task_argument argument); | |
95 | int configure_spw_link(); |
|
76 | rtems_task spw_wfrm_task(rtems_task_argument argument); | |
|
77 | // ISR | |||
|
78 | rtems_isr waveforms_isr( rtems_vector_number vector ); | |||
|
79 | ||||
|
80 | int send_console_outputs_on_serial_port(); | |||
|
81 | int create_all_tasks(); | |||
|
82 | int start_all_tasks(); | |||
|
83 | int configure_spw_link(); | |||
|
84 | int configure_timer_for_sm_simulation(); | |||
|
85 | int configure_timer_for_wf_simulation(); | |||
96 | extern int rtems_cpu_usage_report(); |
|
86 | extern int rtems_cpu_usage_report(); | |
97 | extern int rtems_cpu_usage_reset(); |
|
87 | extern int rtems_cpu_usage_reset(); | |
98 |
|
88 | |||
99 | char *link_status(int status); |
|
89 | char *link_status(int status); | |
100 | void print_statistics(spw_stats *); |
|
90 | void print_statistics(spw_stats *); | |
101 | extern int sched_yield(); |
|
91 | extern int sched_yield(); | |
102 | extern int errno; |
|
92 | extern int errno; | |
103 |
|
93 | |||
104 | char *lstates[6] = {"Error-reset", |
|
94 | char *lstates[6] = {"Error-reset", | |
105 | "Error-wait", |
|
95 | "Error-wait", | |
106 | "Ready", |
|
96 | "Ready", | |
107 | "Started", |
|
97 | "Started", | |
108 | "Connecting", |
|
98 | "Connecting", | |
109 | "Run" |
|
99 | "Run" | |
110 | }; |
|
100 | }; | |
111 |
|
101 | |||
112 | struct tc_fifo_str{ |
|
|||
113 | struct tc_fifo_str *next; |
|
|||
114 | unsigned char *tc_pkt; |
|
|||
115 | unsigned int tc_pktlength; |
|
|||
116 | unsigned char ready; |
|
|||
117 | }; |
|
|||
118 | struct tc_fifo_str tc_fifo[TC_FIFO_SIZE]; |
|
|||
119 | struct tc_fifo_str *tc_being_processed; |
|
|||
120 | struct tc_fifo_str *tc_pending_reception; |
|
|||
121 |
|
||||
122 | struct apbuart_regs_str{ |
|
|||
123 | volatile unsigned int data; |
|
|||
124 | volatile unsigned int status; |
|
|||
125 | volatile unsigned int ctrl; |
|
|||
126 | volatile unsigned int scaler; |
|
|||
127 | volatile unsigned int fifoDebug; |
|
|||
128 | }; |
|
|||
129 |
|
||||
130 | int fdSPW; |
|
102 | int fdSPW; | |
131 | int fdUART; |
|
103 | int fdUART; | |
132 |
|
104 | |||
133 | rtems_id Task_id[10]; /* array of task ids */ |
|
105 | rtems_id Task_id[10]; /* array of task ids */ | |
134 | rtems_name Task_name[10]; /* array of task names */ |
|
106 | rtems_name Task_name[10]; /* array of task names */ | |
135 | rtems_name sem_tc_fifo_name; |
|
|||
136 | rtems_id sem_tc_fifo_id; |
|
|||
137 |
|
107 | |||
138 | rtems_task Init( rtems_task_argument ignored ) { |
|
108 | rtems_task Init( rtems_task_argument ignored ) { | |
139 | rtems_status_code status; |
|
109 | rtems_status_code status; | |
140 | //struct apbuart_regs_str *apbuart_regs; |
|
110 | ||
141 | rtems_isr_entry old_isr_handler; |
|
111 | send_console_outputs_on_serial_port(); | |
|
112 | ||||
|
113 | create_all_tasks(); | |||
|
114 | start_all_tasks(); | |||
|
115 | ||||
|
116 | configure_spw_link(); | |||
|
117 | configure_timer_for_sm_simulation(); | |||
|
118 | configure_timer_for_wf_simulation(); | |||
|
119 | ||||
|
120 | InitLookUpTableForCRC(); // in TC_handler.h | |||
|
121 | ||||
|
122 | status = rtems_task_delete(RTEMS_SELF); | |||
|
123 | } | |||
|
124 | ||||
|
125 | rtems_task spw_recv_task( rtems_task_argument unused ) { | |||
|
126 | rtems_status_code status; | |||
|
127 | int len = 0; | |||
|
128 | unsigned int i = 0; | |||
|
129 | unsigned int data_length = 0; | |||
|
130 | ccsdsTelecommandPacket_t currentTC; | |||
|
131 | spw_ioctl_pkt_send spw_ioctl_send; | |||
|
132 | TMHeader_t TM_header; | |||
|
133 | char data[100]; | |||
|
134 | ||||
|
135 | for(i=0; i<100; i++) data[i] = 0; | |||
|
136 | ||||
|
137 | PRINTF("In RECV *** \n") | |||
|
138 | ||||
|
139 | while(1){ | |||
|
140 | len = read(fdSPW, (char*) ¤tTC, CCSDS_TC_PKT_MAX_SIZE); // the call to read is blocking | |||
|
141 | if (len == -1){ // error during the read call | |||
|
142 | PRINTF("In RECV *** last read call returned -1\n") | |||
|
143 | if (rtems_event_send( Task_id[3], SPW_LINKERR_EVENT ) != RTEMS_SUCCESSFUL) | |||
|
144 | PRINTF("IN RECV *** Error: rtems_event_send SPW_LINKERR_EVENT\n") | |||
|
145 | if (rtems_task_suspend(RTEMS_SELF) != RTEMS_SUCCESSFUL) | |||
|
146 | PRINTF("In RECV *** Error: rtems_task_suspend(RTEMS_SELF)\n") | |||
|
147 | } | |||
|
148 | else { | |||
|
149 | //PRINTF1("In RECV *** Got Message of length %d\n", len) | |||
|
150 | currentTC_LEN_RCV[0] = 0x00; | |||
|
151 | currentTC_LEN_RCV[1] = (unsigned char) len - CCSDS_TC_TM_PACKET_OFFSET - 3; // build the corresponding packet size field | |||
|
152 | currentTC_LEN_RCV_AsUnsignedInt = (unsigned int) len - CCSDS_TC_TM_PACKET_OFFSET - 3; // => -3 is for Prot ID, Reserved and User App bytes | |||
|
153 | // CHECK THE TC AND BUILD THE APPROPRIATE TM | |||
|
154 | data_length = TC_checker(¤tTC, currentTC_LEN_RCV_AsUnsignedInt, | |||
|
155 | &TM_header, &spw_ioctl_send.hlen, data); | |||
|
156 | spw_ioctl_send.hlen = TM_HEADER_LEN + 4; // + 4 is for the protocole extra header | |||
|
157 | spw_ioctl_send.hdr = (char*) &TM_header; | |||
|
158 | spw_ioctl_send.dlen = data_length; | |||
|
159 | spw_ioctl_send.data = data; | |||
|
160 | //printf("hlen %d, dlen %d\n", spw_ioctl_send.hlen, spw_ioctl_send.dlen); | |||
|
161 | // SEND PACKET | |||
|
162 | status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send ); | |||
|
163 | if (status!=RTEMS_SUCCESSFUL) printf("In TC_checker *** Error SPACEWIRE_IOCTRL_SEND\n"); | |||
|
164 | //PRINTF1("In TC_checker *** packet of size %d sent\n", spw_ioctl_send.sent) | |||
|
165 | } | |||
|
166 | } | |||
|
167 | ||||
|
168 | close(fdSPW); | |||
|
169 | } | |||
|
170 | ||||
|
171 | rtems_task spw_spiq_task(rtems_task_argument unused){ | |||
|
172 | rtems_event_set event_out; | |||
|
173 | struct grspw_regs_str *grspw_regs; | |||
|
174 | grspw_regs = (struct grspw_regs_str *) ADDRESS_GRSPW_REGISTERS; | |||
|
175 | ||||
|
176 | while(1){ | |||
|
177 | PRINTF("In SPIQ *** Waiting for SPW_LINKERR_EVENT\n") | |||
|
178 | rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT | |||
|
179 | ||||
|
180 | if (rtems_task_suspend(Task_id[1])!=RTEMS_SUCCESSFUL) // suspend RECV task | |||
|
181 | PRINTF("In SPIQ *** Error suspending RECV Task\n") | |||
|
182 | ||||
|
183 | configure_spw_link(); | |||
142 |
|
184 | |||
|
185 | if (rtems_task_restart(Task_id[1], 1)!=RTEMS_SUCCESSFUL) // restart RECV task | |||
|
186 | PRINTF("In SPIQ *** Error resume RECV Task\n") | |||
|
187 | } | |||
|
188 | } | |||
|
189 | ||||
|
190 | rtems_isr waveforms_isr( rtems_vector_number vector ) | |||
|
191 | { | |||
|
192 | if (rtems_event_send( Task_id[8], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) | |||
|
193 | printf("In spectral_matrices_isr *** Error sending event to WFRM\n"); | |||
|
194 | } | |||
|
195 | ||||
|
196 | rtems_task spw_wfrm_task(rtems_task_argument argument) | |||
|
197 | { | |||
|
198 | unsigned int length; | |||
|
199 | unsigned int i = 0; | |||
|
200 | rtems_status_code status; | |||
|
201 | spw_ioctl_pkt_send spw_ioctl_send; | |||
|
202 | rtems_event_set event_out; | |||
|
203 | gptimer_regs_t *gptimer_regs; | |||
|
204 | gptimer_regs = (gptimer_regs_t *) REGS_ADDRESS_GPTIMER; | |||
|
205 | ExtendedTMHeader_t header; | |||
|
206 | ||||
|
207 | header.targetLogicalAddress = CCSDS_DESTINATION_ID; | |||
|
208 | header.protocolIdentifier = CCSDS_PROTOCOLE_ID; | |||
|
209 | header.reserved = 0x00; | |||
|
210 | header.userApplication = CCSDS_USER_APP; | |||
|
211 | header.packetID[0] = 0x0c; | |||
|
212 | header.packetID[1] = 0xcc; | |||
|
213 | header.packetSequenceControl[0] = 0x00; | |||
|
214 | header.packetSequenceControl[1] = 0x00; | |||
|
215 | header.packetLength[0] = 0x00; | |||
|
216 | header.packetLength[1] = 0x00; | |||
|
217 | header.dataFieldHeader[0] = 0x10; | |||
|
218 | header.dataFieldHeader[1] = 0x15; // service type | |||
|
219 | header.dataFieldHeader[2] = 0x03; // service subtype | |||
|
220 | header.dataFieldHeader[3] = CCSDS_DESTINATION_ID; | |||
|
221 | header.dataFieldHeader[4] = 0xaa; | |||
|
222 | header.dataFieldHeader[5] = 0xbb; | |||
|
223 | header.dataFieldHeader[6] = 0xcc; | |||
|
224 | header.dataFieldHeader[7] = 0xdd; | |||
|
225 | header.dataFieldHeader[8] = 0xee; | |||
|
226 | header.dataFieldHeader[9] = 0xff; | |||
|
227 | ||||
|
228 | header.auxiliaryHeader[0] = 0x00; | |||
|
229 | header.auxiliaryHeader[1] = 0x1f; | |||
|
230 | header.auxiliaryHeader[2] = 0x07; // PKT_CNT | |||
|
231 | header.auxiliaryHeader[3] = 0x00; // PKT_NR | |||
|
232 | header.auxiliaryHeader[4] = 0x00; // BLK_NR MSB | |||
|
233 | header.auxiliaryHeader[5] = 0x00; // BLK_NR LSB | |||
|
234 | ||||
|
235 | // BUILD THE PACKET HEADER | |||
|
236 | spw_ioctl_send.hlen = TM_HEADER_LEN + 4 + 6; // + 4 is for the protocole extra header, + 6 is for the auxiliary header | |||
|
237 | spw_ioctl_send.hdr = (char*) &header; | |||
|
238 | ||||
|
239 | while(1){ | |||
|
240 | rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0 | |||
|
241 | for (i=0; i<7; i++) // send F0 | |||
|
242 | { | |||
|
243 | // BUILD THE DATA | |||
|
244 | if (i==6) { | |||
|
245 | spw_ioctl_send.dlen = 8 * NB_BYTES_SWF_BLK; | |||
|
246 | length = TM_LEN_SCI_NORM_SWF_340; | |||
|
247 | } | |||
|
248 | else { | |||
|
249 | spw_ioctl_send.dlen = 340 * NB_BYTES_SWF_BLK; | |||
|
250 | length = TM_LEN_SCI_NORM_SWF_8; | |||
|
251 | } | |||
|
252 | spw_ioctl_send.data = (char*) &waveform_snapshot_f0[i * 340 * NB_BYTES_SWF_BLK]; | |||
|
253 | // BUILD THE HEADER | |||
|
254 | header.packetLength[0] = (unsigned char) (length>>8); | |||
|
255 | header.packetLength[1] = (unsigned char) (length); | |||
|
256 | header.auxiliaryHeader[0] = SID_NORM_SWF_F0; // SID | |||
|
257 | // SEND PACKET | |||
|
258 | status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send ); | |||
|
259 | if (status!=RTEMS_SUCCESSFUL) printf("In WFRM *** Error SPACEWIRE_IOCTRL_SEND\n"); | |||
|
260 | //sched_yield(); | |||
|
261 | } | |||
|
262 | for (i=0; i<7; i++) // send F1 | |||
|
263 | { | |||
|
264 | // BUILD THE DATA | |||
|
265 | if (i==6) { | |||
|
266 | spw_ioctl_send.dlen = 8 * NB_BYTES_SWF_BLK; | |||
|
267 | length = TM_LEN_SCI_NORM_SWF_340; | |||
|
268 | } | |||
|
269 | else { | |||
|
270 | spw_ioctl_send.dlen = 340 * NB_BYTES_SWF_BLK; | |||
|
271 | length = TM_LEN_SCI_NORM_SWF_8; | |||
|
272 | } | |||
|
273 | spw_ioctl_send.data = (char*) &waveform_snapshot_f1[i * 340 * NB_BYTES_SWF_BLK]; | |||
|
274 | // BUILD THE HEADER | |||
|
275 | header.packetLength[0] = (unsigned char) (length>>8); | |||
|
276 | header.packetLength[1] = (unsigned char) (length); | |||
|
277 | header.auxiliaryHeader[0] = SID_NORM_SWF_F1; // SID | |||
|
278 | // SEND PACKET | |||
|
279 | status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send ); | |||
|
280 | if (status!=RTEMS_SUCCESSFUL) printf("In WFRM *** Error SPACEWIRE_IOCTRL_SEND\n"); | |||
|
281 | //sched_yield(); | |||
|
282 | } | |||
|
283 | for (i=0; i<7; i++) // send F0 | |||
|
284 | { | |||
|
285 | // BUILD THE DATA | |||
|
286 | if (i==6) { | |||
|
287 | spw_ioctl_send.dlen = 8 * NB_BYTES_SWF_BLK; | |||
|
288 | length = TM_LEN_SCI_NORM_SWF_340; | |||
|
289 | } | |||
|
290 | else { | |||
|
291 | spw_ioctl_send.dlen = 340 * NB_BYTES_SWF_BLK; | |||
|
292 | length = TM_LEN_SCI_NORM_SWF_8; | |||
|
293 | } | |||
|
294 | spw_ioctl_send.data = (char*) &waveform_snapshot_f2[i * 340 * NB_BYTES_SWF_BLK]; | |||
|
295 | // BUILD THE HEADER | |||
|
296 | header.packetLength[0] = (unsigned char) (length>>8); | |||
|
297 | header.packetLength[1] = (unsigned char) (length); | |||
|
298 | header.auxiliaryHeader[0] = SID_NORM_SWF_F2; // SID | |||
|
299 | // SEND PACKET | |||
|
300 | status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send ); | |||
|
301 | if (status!=RTEMS_SUCCESSFUL) printf("In WFRM *** Error SPACEWIRE_IOCTRL_SEND\n"); | |||
|
302 | //sched_yield(); | |||
|
303 | } | |||
|
304 | // irq processed, reset the related register of the timer unit | |||
|
305 | gptimer_regs->timer3_ctrl = gptimer_regs->timer3_ctrl | 0x00000010; | |||
|
306 | } | |||
|
307 | } | |||
|
308 | ||||
|
309 | int create_all_tasks() | |||
|
310 | { | |||
|
311 | rtems_status_code status; | |||
|
312 | ||||
143 | Task_name[1] = rtems_build_name( 'R', 'E', 'C', 'V' ); |
|
313 | Task_name[1] = rtems_build_name( 'R', 'E', 'C', 'V' ); | |
144 |
Task_name[ |
|
314 | Task_name[3] = rtems_build_name( 'S', 'P', 'I', 'Q' ); | |
145 |
Task_name[ |
|
315 | Task_name[4] = rtems_build_name( 'S', 'M', 'I', 'Q' ); | |
146 | Task_name[4] = rtems_build_name( 'B', 'P', 'P', 'R' ); |
|
|||
147 | Task_name[5] = rtems_build_name( 'S', 'T', 'A', 'T' ); |
|
316 | Task_name[5] = rtems_build_name( 'S', 'T', 'A', 'T' ); | |
148 | Task_name[6] = rtems_build_name( 'A', 'V', 'F', '0' ); |
|
317 | Task_name[6] = rtems_build_name( 'A', 'V', 'F', '0' ); | |
149 | Task_name[7] = rtems_build_name( 'B', 'P', 'F', '0' ); |
|
318 | Task_name[7] = rtems_build_name( 'B', 'P', 'F', '0' ); | |
150 |
|
|
319 | Task_name[8] = rtems_build_name( 'W', 'F', 'R', 'M' ); | |
151 |
|
320 | |||
152 | // RECV |
|
321 | // RECV | |
153 | status = rtems_task_create( |
|
322 | status = rtems_task_create( | |
154 | Task_name[1], 200, RTEMS_MINIMUM_STACK_SIZE * 2, |
|
323 | Task_name[1], 200, RTEMS_MINIMUM_STACK_SIZE * 2, | |
155 | RTEMS_DEFAULT_MODES, |
|
324 | RTEMS_DEFAULT_MODES, | |
156 | RTEMS_DEFAULT_ATTRIBUTES, &Task_id[1] |
|
325 | RTEMS_DEFAULT_ATTRIBUTES, &Task_id[1] | |
157 | ); |
|
326 | ); | |
158 | // TCCK |
|
|||
159 | status = rtems_task_create( |
|
|||
160 | Task_name[2], 200, RTEMS_MINIMUM_STACK_SIZE * 2, |
|
|||
161 | RTEMS_DEFAULT_MODES, |
|
|||
162 | RTEMS_DEFAULT_ATTRIBUTES, &Task_id[2] |
|
|||
163 | ); |
|
|||
164 | // SPIQ |
|
327 | // SPIQ | |
165 | status = rtems_task_create( |
|
328 | status = rtems_task_create( | |
166 | Task_name[3], 50, RTEMS_MINIMUM_STACK_SIZE * 2, |
|
329 | Task_name[3], 50, RTEMS_MINIMUM_STACK_SIZE * 2, | |
167 | RTEMS_DEFAULT_MODES, |
|
330 | RTEMS_DEFAULT_MODES, | |
168 | RTEMS_DEFAULT_ATTRIBUTES, &Task_id[3] |
|
331 | RTEMS_DEFAULT_ATTRIBUTES, &Task_id[3] | |
169 | ); |
|
332 | ); | |
170 |
// |
|
333 | // SMIQ | |
171 | status = rtems_task_create( |
|
334 | status = rtems_task_create( | |
172 | Task_name[4], 10, RTEMS_MINIMUM_STACK_SIZE * 2, |
|
335 | Task_name[4], 10, RTEMS_MINIMUM_STACK_SIZE * 2, | |
173 | RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT, |
|
336 | RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT, | |
174 | RTEMS_DEFAULT_ATTRIBUTES, &Task_id[4] |
|
337 | RTEMS_DEFAULT_ATTRIBUTES, &Task_id[4] | |
175 | ); |
|
338 | ); | |
176 | // STAT |
|
339 | // STAT | |
177 | status = rtems_task_create( |
|
340 | status = rtems_task_create( | |
178 | Task_name[5], 200, RTEMS_MINIMUM_STACK_SIZE * 2, |
|
341 | Task_name[5], 200, RTEMS_MINIMUM_STACK_SIZE * 2, | |
179 | RTEMS_DEFAULT_MODES, |
|
342 | RTEMS_DEFAULT_MODES, | |
180 | RTEMS_DEFAULT_ATTRIBUTES, &Task_id[5] |
|
343 | RTEMS_DEFAULT_ATTRIBUTES, &Task_id[5] | |
181 | ); |
|
344 | ); | |
182 | // AVF0 |
|
345 | // AVF0 | |
183 | status = rtems_task_create( |
|
346 | status = rtems_task_create( | |
184 | Task_name[6], 50, RTEMS_MINIMUM_STACK_SIZE * 2, |
|
347 | Task_name[6], 50, RTEMS_MINIMUM_STACK_SIZE * 2, | |
185 | RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT, |
|
348 | RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT, | |
186 | RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[6] |
|
349 | RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[6] | |
187 | ); |
|
350 | ); | |
188 | // BPF0 |
|
351 | // BPF0 | |
189 | status = rtems_task_create( |
|
352 | status = rtems_task_create( | |
190 | Task_name[7], 50, RTEMS_MINIMUM_STACK_SIZE * 2, |
|
353 | Task_name[7], 50, RTEMS_MINIMUM_STACK_SIZE * 2, | |
191 | RTEMS_DEFAULT_MODES, |
|
354 | RTEMS_DEFAULT_MODES, | |
192 | RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[7] |
|
355 | RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[7] | |
193 | ); |
|
356 | ); | |
194 |
|
357 | // WFRM | ||
195 |
status = rtems_ |
|
358 | status = rtems_task_create( | |
196 | sem_tc_fifo_name, |
|
359 | Task_name[8], 100, RTEMS_MINIMUM_STACK_SIZE * 2, | |
197 | 1, |
|
360 | RTEMS_DEFAULT_MODES, | |
198 | RTEMS_BINARY_SEMAPHORE, |
|
361 | RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[8] | |
199 | 1, |
|
362 | ); | |
200 | &sem_tc_fifo_id |
|
363 | ||
201 | ); |
|
364 | return 0; | |
202 |
|
365 | } | ||
203 | //******************************************** |
|
366 | ||
204 | // Send the console outputs on the serial port |
|
367 | int start_all_tasks() | |
205 | //apbuart_regs = (struct apbuart_regs_str *) ADDRESS_APBUART_REGISTERS; |
|
368 | { | |
206 | //apbuart_regs->ctrl = apbuart_regs->ctrl & APBUART_CTRL_REG_MASK_DB; |
|
369 | rtems_status_code status; | |
207 | //PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n") |
|
370 | ||
208 | // |
|
|||
209 | //******************************************** |
|
|||
210 |
|
||||
211 | configure_spw_link(); |
|
|||
212 |
|
||||
213 | status = rtems_interrupt_catch( spectral_matrices_isr, 0x1c, &old_isr_handler) ; // 0x1c comes from sparcv8.pdf p.76 |
|
|||
214 | if (status==RTEMS_SUCCESSFUL) PRINTF("In INIT *** rtems_interrupt_catch successfullly configured\n") |
|
|||
215 |
|
||||
216 | LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRICES ); |
|
|||
217 |
|
||||
218 | status = rtems_task_start( Task_id[2], spw_tcck_task, 1 ); |
|
|||
219 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_TCCK\n") |
|
|||
220 |
|
||||
221 | status = rtems_task_start( Task_id[3], spw_spiq_task, 1 ); |
|
371 | status = rtems_task_start( Task_id[3], spw_spiq_task, 1 ); | |
222 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_SPIQ\n") |
|
372 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_SPIQ\n") | |
223 |
|
373 | |||
224 | status = rtems_task_start( Task_id[1], spw_recv_task, 1 ); |
|
374 | status = rtems_task_start( Task_id[1], spw_recv_task, 1 ); | |
225 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_RECV\n") |
|
375 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_RECV\n") | |
226 |
|
376 | |||
227 | //status = rtems_task_start( Task_id[4], spw_bppr_task_rate_monotonic, 1 ); |
|
377 | //status = rtems_task_start( Task_id[4], spw_bppr_task_rate_monotonic, 1 ); | |
228 | //if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_BPPR\n") |
|
378 | status = rtems_task_start( Task_id[4], spw_smiq_task, 1 ); | |
|
379 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_BPPR\n") | |||
229 |
|
380 | |||
230 | status = rtems_task_start( Task_id[5], spw_stat_task, 1 ); |
|
381 | status = rtems_task_start( Task_id[5], spw_stat_task, 1 ); | |
231 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_STAT\n") |
|
382 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_STAT\n") | |
232 |
|
383 | |||
233 | status = rtems_task_start( Task_id[6], spw_avf0_task, 1 ); |
|
384 | status = rtems_task_start( Task_id[6], spw_avf0_task, 1 ); | |
234 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_AVF0\n") |
|
385 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_AVF0\n") | |
235 |
|
386 | |||
236 | status = rtems_task_start( Task_id[7], spw_bpf0_task, 1 ); |
|
387 | status = rtems_task_start( Task_id[7], spw_bpf0_task, 1 ); | |
237 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_BPF0\n") |
|
388 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_BPF0\n") | |
238 |
|
389 | |||
239 | InitLookUpTableForCRC(); // in TC_handler.h |
|
390 | status = rtems_task_start( Task_id[8], spw_wfrm_task, 1 ); | |
240 |
|
391 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_WFRM\n") | ||
241 | status = rtems_task_delete(RTEMS_SELF); |
|
|||
242 | } |
|
|||
243 |
|
392 | |||
244 | rtems_task spw_recv_task_old( rtems_task_argument unused ) { |
|
393 | return 0; | |
245 | rtems_status_code status; |
|
|||
246 | int len = 0; |
|
|||
247 | unsigned int i; |
|
|||
248 |
|
||||
249 | PRINTF("In RECV *** \n") |
|
|||
250 |
|
||||
251 | status = rtems_semaphore_obtain( sem_tc_fifo_id, RTEMS_WAIT, RTEMS_NO_TIMEOUT ); |
|
|||
252 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error: rtems_semaphore_obtain\n") |
|
|||
253 | for (i = 0; i<TC_FIFO_SIZE; i++){ // initialize the tc fifo ring |
|
|||
254 | tc_fifo[i].tc_pktlength = 0; |
|
|||
255 | tc_fifo[i].ready = 0; |
|
|||
256 | } |
|
|||
257 | status = rtems_semaphore_release( sem_tc_fifo_id ); |
|
|||
258 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error: rtems_semaphore_release\n") |
|
|||
259 |
|
||||
260 | tc_being_processed = tc_fifo; |
|
|||
261 | tc_pending_reception = tc_fifo; |
|
|||
262 |
|
||||
263 | while(1){ |
|
|||
264 | if (tc_pending_reception->ready == 0){ |
|
|||
265 | PRINTF("In RECV *** Just before read\n") |
|
|||
266 | len = read(fdSPW, tc_pending_reception->tc_pkt, CCSDS_TC_PKT_MAX_SIZE); // the call to read is blocking |
|
|||
267 | PRINTF("In RECV *** Just after read\n") |
|
|||
268 | if (len == -1) { // error during the read call |
|
|||
269 | PRINTF("In RECV *** last read call returned -1\n") |
|
|||
270 | if (rtems_event_send( Task_id[3], SPW_LINKERR_EVENT ) != RTEMS_SUCCESSFUL) |
|
|||
271 | PRINTF("IN RECV *** Error: rtems_event_send SPW_LINKERR_EVENT\n") |
|
|||
272 | if (rtems_task_suspend(RTEMS_SELF) != RTEMS_SUCCESSFUL) |
|
|||
273 | PRINTF("In RECV *** Error: rtems_task_suspend(RTEMS_SELF)\n") |
|
|||
274 | } |
|
|||
275 | else { |
|
|||
276 | tc_pending_reception->tc_pktlength = len; |
|
|||
277 | tc_pending_reception->ready = 1; |
|
|||
278 | PRINTF1("In RECV *** Got Message of length %d\n",tc_pending_reception->tc_pktlength) |
|
|||
279 | status = rtems_event_send( Task_id[2], RTEMS_EVENT_0 ); // sending an event to the task TCCK |
|
|||
280 | if (status == RTEMS_INVALID_ID) PRINTF("IN TASK RECV *** invalid task id when sending RTEMS_EVENT_0\n") |
|
|||
281 | tc_pending_reception = tc_pending_reception->next; |
|
|||
282 | sched_yield(); |
|
|||
283 | } |
|
|||
284 | } |
|
|||
285 | else { |
|
|||
286 | status = rtems_event_send( Task_id[2], RTEMS_EVENT_0 ); // sending an event to the task TCCK |
|
|||
287 | if (status == RTEMS_INVALID_ID) PRINTF("IN TASK RECV *** invalid task id when sending RTEMS_EVENT_0\n") |
|
|||
288 | sched_yield(); // if not executed, the system blocks when the packets arrive faster than the processing |
|
|||
289 | // this call gives back the hand to the scheduler and allows TCCK to process the TC packets |
|
|||
290 | } |
|
|||
291 | } |
|
|||
292 |
|
||||
293 | close(fdSPW); |
|
|||
294 | } |
|
394 | } | |
295 |
|
||||
296 | rtems_task spw_recv_task( rtems_task_argument unused ) { |
|
|||
297 | rtems_status_code status; |
|
|||
298 | int len = 0; |
|
|||
299 |
|
||||
300 | PRINTF("In RECV BIS *** \n") |
|
|||
301 |
|
395 | |||
302 | currentTC_processedFlag = 1; // reset the flag |
|
396 | int send_console_outputs_on_serial_port() // Send the console outputs on the serial port | |
|
397 | { | |||
|
398 | struct apbuart_regs_str *apbuart_regs; | |||
303 |
|
399 | |||
304 | while(1){ |
|
400 | apbuart_regs = (struct apbuart_regs_str *) ADDRESS_APBUART_REGISTERS; | |
305 | while(currentTC_processedFlag == 0) sched_yield(); |
|
401 | apbuart_regs->ctrl = apbuart_regs->ctrl & APBUART_CTRL_REG_MASK_DB; | |
306 | len = read(fdSPW, (char*) ¤tTC, CCSDS_TC_PKT_MAX_SIZE); // the call to read is blocking |
|
402 | PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n") | |
307 | if (len == -1){ // error during the read call |
|
|||
308 | PRINTF("In RECV *** last read call returned -1\n") |
|
|||
309 | if (rtems_event_send( Task_id[3], SPW_LINKERR_EVENT ) != RTEMS_SUCCESSFUL) |
|
|||
310 | PRINTF("IN RECV *** Error: rtems_event_send SPW_LINKERR_EVENT\n") |
|
|||
311 | if (rtems_task_suspend(RTEMS_SELF) != RTEMS_SUCCESSFUL) |
|
|||
312 | PRINTF("In RECV *** Error: rtems_task_suspend(RTEMS_SELF)\n") |
|
|||
313 | } |
|
|||
314 | else { |
|
|||
315 | currentTC_processedFlag = 0; |
|
|||
316 | PRINTF1("In RECV *** Got Message of length %d\n", len) |
|
|||
317 | currentTC_LEN_RCV[0] = 0x00; |
|
|||
318 | currentTC_LEN_RCV[1] = (unsigned char) len - CCSDS_TC_TM_PACKET_OFFSET - 3; // build the corresponding packet size field |
|
|||
319 | currentTC_LEN_RCV_AsUnsignedInt = (unsigned int) len - CCSDS_TC_TM_PACKET_OFFSET - 3; // => -3 is for Prot ID, Reserved and User App bytes |
|
|||
320 | status = rtems_event_send( Task_id[2], RTEMS_EVENT_0 ); // sending an event to the task TCCK |
|
|||
321 | if (status == RTEMS_INVALID_ID) PRINTF("IN TASK RECV *** invalid task id when sending RTEMS_EVENT_0\n") |
|
|||
322 | } |
|
|||
323 | } |
|
|||
324 |
|
||||
325 | close(fdSPW); |
|
|||
326 | } |
|
|||
327 |
|
||||
328 | rtems_task spw_tcck_task( rtems_task_argument unused ) { |
|
|||
329 | rtems_status_code status; |
|
|||
330 | rtems_event_set event_out; |
|
|||
331 | unsigned char result; |
|
|||
332 | unsigned int code; |
|
|||
333 |
|
||||
334 | PRINTF("In TCCK ***\n") |
|
|||
335 | while(1){ |
|
|||
336 | status = rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an event to process a TC packet |
|
|||
337 | if (status == RTEMS_SUCCESSFUL) |
|
|||
338 | { |
|
|||
339 | GetCRCAsTwoBytes((unsigned char*)currentTC.packetID, currentTC_COMPUTED_CRC, currentTC_LEN_RCV_AsUnsignedInt + 5); |
|
|||
340 | code = TM_checker(¤tTC); |
|
|||
341 | PRINTF1(" %s\n", errorCCSDSMsg[code]) |
|
|||
342 | if (code == 7){ // if the TC is valid, the TM_LFR_TC_EXE_NOT_IMPLEMENTED packet is sent |
|
|||
343 | result = TM_not_implemented_generator(¤tTC, ¤tTM); |
|
|||
344 | PRINTF1(" %s\n", tmGeneratorMsg[result]) |
|
|||
345 | result = write( fdSPW, (char*)¤tTM, currentTM_length ); |
|
|||
346 | if (result==-1) printf("IN TCCK *** error sending TM of size: %d\n", currentTM_length); |
|
|||
347 | } |
|
|||
348 | else{ // if the TC is not valid, the TM_LFR_TC_EXE_CORRUPTED is sent |
|
|||
349 | result = TM_acceptance_generator(¤tTC, code, ¤tTM); |
|
|||
350 | PRINTF1(" %s\n", tmGeneratorMsg[result]) |
|
|||
351 | result = write( fdSPW, (char*)¤tTM, currentTM_length ); |
|
|||
352 | if (result==-1) printf("IN TCCK *** error sending TM of size: %d\n", currentTM_length); |
|
|||
353 | } |
|
|||
354 |
|
403 | |||
355 | currentTC_processedFlag = 1; |
|
404 | return 0; | |
356 | } |
|
|||
357 | } |
|
|||
358 | } |
|
|||
359 |
|
||||
360 | rtems_task spw_spiq_task(rtems_task_argument unused){ |
|
|||
361 | rtems_event_set event_out; |
|
|||
362 | struct grspw_regs_str *grspw_regs; |
|
|||
363 | grspw_regs = (struct grspw_regs_str *) ADDRESS_GRSPW_REGISTERS; |
|
|||
364 |
|
||||
365 | while(1){ |
|
|||
366 | PRINTF("In SPIQ *** Waiting for SPW_LINKERR_EVENT\n") |
|
|||
367 | rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT |
|
|||
368 |
|
||||
369 | if (rtems_task_suspend(Task_id[1])!=RTEMS_SUCCESSFUL) // suspend RECV task |
|
|||
370 | PRINTF("In SPIQ *** Error suspending RECV Task\n") |
|
|||
371 |
|
||||
372 | configure_spw_link(); |
|
|||
373 |
|
||||
374 | if (rtems_task_restart(Task_id[1], 1)!=RTEMS_SUCCESSFUL) // restart RECV task |
|
|||
375 | PRINTF("In SPIQ *** Error resume RECV Task\n") |
|
|||
376 | } |
|
|||
377 | } |
|
|||
378 |
|
||||
379 | char *link_status(int status){ |
|
|||
380 | return lstates[status]; |
|
|||
381 | } |
|
405 | } | |
382 |
|
406 | |||
383 | void print_statistics(spw_stats *stats) |
|
|||
384 | { |
|
|||
385 | //printf(" ******** STATISTICS ******** \n"); |
|
|||
386 | printf("Transmit link errors: %i\n", stats->tx_link_err); |
|
|||
387 | printf("Receiver RMAP header CRC errors: %i\n", stats->rx_rmap_header_crc_err); |
|
|||
388 | printf("Receiver RMAP data CRC errors: %i\n", stats->rx_rmap_data_crc_err); |
|
|||
389 | printf("Receiver EEP errors: %i\n", stats->rx_eep_err); |
|
|||
390 | printf("Receiver truncation errors: %i\n", stats->rx_truncated); |
|
|||
391 | printf("Parity errors: %i\n", stats->parity_err); |
|
|||
392 | printf("Escape errors: %i\n", stats->escape_err); |
|
|||
393 | printf("Credit errors: %i\n", stats->credit_err); |
|
|||
394 | printf("Disconnect errors: %i\n", stats->disconnect_err); |
|
|||
395 | printf("Write synchronization errors: %i\n", stats->write_sync_err); |
|
|||
396 | printf("Early EOP/EEP: %i\n", stats->early_ep); |
|
|||
397 | printf("Invalid Node Address: %i\n", stats->invalid_address); |
|
|||
398 | printf("Packets transmitted: %i\n", stats->packets_sent); |
|
|||
399 | printf("Packets received: %i\n", stats->packets_received); |
|
|||
400 | } |
|
|||
401 |
|
||||
402 | int configure_spw_link() |
|
407 | int configure_spw_link() | |
403 | { |
|
408 | { | |
404 | rtems_status_code status; |
|
409 | rtems_status_code status; | |
405 |
|
410 | |||
406 | close(fdSPW); // close the device if it is already open |
|
411 | close(fdSPW); // close the device if it is already open | |
407 | fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call reset the hardware |
|
412 | fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call reset the hardware | |
408 | if (fdSPW<0) PRINTF("In configure_spw_link *** Error opening"GRSPW_DEVICE_NAME"\n") |
|
413 | if (fdSPW<0) PRINTF("In configure_spw_link *** Error opening"GRSPW_DEVICE_NAME"\n") | |
409 | while(ioctl(fdSPW, SPACEWIRE_IOCTRL_START, 0) != RTEMS_SUCCESSFUL){ |
|
414 | while(ioctl(fdSPW, SPACEWIRE_IOCTRL_START, 0) != RTEMS_SUCCESSFUL){ | |
410 | PRINTF("In configure_spw_link *** "GRSPW_DEVICE_NAME" not started, retry\n") |
|
415 | PRINTF("In configure_spw_link *** "GRSPW_DEVICE_NAME" not started, retry\n") | |
411 | close(fdSPW); // close the device |
|
416 | close(fdSPW); // close the device | |
412 | fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call reset the hardware |
|
417 | fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call reset the hardware | |
413 | if (fdSPW<0) PRINTF("In configure_spw_link *** Error opening"GRSPW_DEVICE_NAME"\n") |
|
418 | if (fdSPW<0) PRINTF("In configure_spw_link *** Error opening"GRSPW_DEVICE_NAME"\n") | |
414 | rtems_task_wake_after(100); |
|
419 | rtems_task_wake_after(100); | |
415 | } |
|
420 | } | |
416 |
|
421 | |||
417 | PRINTF("In configure_spw_link *** "GRSPW_DEVICE_NAME" opened and started successfully\n") |
|
422 | PRINTF("In configure_spw_link *** "GRSPW_DEVICE_NAME" opened and started successfully\n") | |
418 |
|
423 | |||
419 | // sets a few parameters of the link |
|
424 | // sets a few parameters of the link | |
420 | status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_RMAPEN, 1); // sets the RMAP enable bit |
|
425 | status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_RMAPEN, 1); // sets the RMAP enable bit | |
421 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_RMAPEN\n") |
|
426 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_RMAPEN\n") | |
422 | // |
|
427 | // | |
423 | status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception |
|
428 | status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception | |
424 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n") |
|
429 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n") | |
425 | // |
|
430 | // | |
426 | status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[3]); // sets the task ID to which an event is sent when a |
|
431 | status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[3]); // sets the task ID to which an event is sent when a | |
427 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs |
|
432 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs | |
428 | // |
|
433 | // | |
429 | status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 1); // automatic link-disabling due to link-error interrupts |
|
434 | status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 1); // automatic link-disabling due to link-error interrupts | |
430 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n") |
|
435 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n") | |
431 | // |
|
436 | // | |
432 | status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit |
|
437 | status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit | |
433 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n") |
|
438 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n") | |
|
439 | ||||
|
440 | status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // sets the link-error interrupt bit | |||
|
441 | if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n") | |||
434 | // |
|
442 | // | |
435 |
//status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_DESTKEY, CCSDS_DESTINATION_ID); // sets the destinati |
|
443 | //status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_DESTKEY, CCSDS_DESTINATION_ID); // sets the destination key | |
436 | //if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n") |
|
444 | //if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n") | |
437 | // |
|
445 | // | |
438 | PRINTF("In configure_spw_link *** "GRSPW_DEVICE_NAME" configured successfully\n") |
|
446 | PRINTF("In configure_spw_link *** "GRSPW_DEVICE_NAME" configured successfully\n") | |
439 |
|
447 | |||
440 | return RTEMS_SUCCESSFUL; |
|
448 | return RTEMS_SUCCESSFUL; | |
441 | } |
|
449 | } | |
|
450 | ||||
|
451 | int configure_timer_for_sm_simulation() // configure the timer for the spectral matrices simulation | |||
|
452 | { | |||
|
453 | rtems_status_code status; | |||
|
454 | rtems_isr_entry old_isr_handler; | |||
|
455 | ||||
|
456 | status = rtems_interrupt_catch( spectral_matrices_isr, 0x19, &old_isr_handler) ; // 0x19 comes from sparcv8.pdf p.76 <=> interrupt_level_9 | |||
|
457 | if (status==RTEMS_SUCCESSFUL) PRINTF("In configure_timer_for_sm_simulation *** rtems_interrupt_catch successfullly configured\n") | |||
|
458 | ||||
|
459 | gptimer_regs_t *gptimer_regs; | |||
|
460 | gptimer_regs = (gptimer_regs_t *) REGS_ADDRESS_GPTIMER; | |||
|
461 | gptimer_regs->timer2_reload = 9999; // 10ms timer, base clock frequency is 1 MHz | |||
|
462 | gptimer_regs->timer2_ctrl = gptimer_regs->timer2_ctrl | 0x00000004; // LD load value from the reload register | |||
|
463 | gptimer_regs->timer2_ctrl = gptimer_regs->timer2_ctrl | 0x00000001; // EN enable the timer | |||
|
464 | gptimer_regs->timer2_ctrl = gptimer_regs->timer2_ctrl | 0x00000002; // RS restart | |||
|
465 | gptimer_regs->timer2_ctrl = gptimer_regs->timer2_ctrl | 0x00000008; // IE interrupt enable | |||
|
466 | ||||
|
467 | LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRICES ); | |||
|
468 | ||||
|
469 | return 1; | |||
|
470 | } | |||
|
471 | ||||
|
472 | int configure_timer_for_wf_simulation() // configure the timer for the spectral matrices simulation | |||
|
473 | { | |||
|
474 | rtems_status_code status; | |||
|
475 | rtems_isr_entry old_isr_handler; | |||
|
476 | ||||
|
477 | status = rtems_interrupt_catch( waveforms_isr, 0x1a, &old_isr_handler) ; // 0x19 comes from sparcv8.pdf p.76 <=> interrupt_level_10 | |||
|
478 | if (status==RTEMS_SUCCESSFUL) PRINTF("In configure_timer_for_wf_simulation *** rtems_interrupt_catch successfullly configured\n") | |||
|
479 | ||||
|
480 | gptimer_regs_t *gptimer_regs; | |||
|
481 | gptimer_regs = (gptimer_regs_t *) REGS_ADDRESS_GPTIMER; | |||
|
482 | gptimer_regs->timer3_reload = 7999999; // 1s timer, base clock frequency is 1 MHz | |||
|
483 | gptimer_regs->timer3_ctrl = gptimer_regs->timer3_ctrl | 0x00000004; // LD load value from the reload register | |||
|
484 | gptimer_regs->timer3_ctrl = gptimer_regs->timer3_ctrl | 0x00000001; // EN enable the timer | |||
|
485 | gptimer_regs->timer3_ctrl = gptimer_regs->timer3_ctrl | 0x00000002; // RS restart | |||
|
486 | gptimer_regs->timer3_ctrl = gptimer_regs->timer3_ctrl | 0x00000008; // IE interrupt enable | |||
|
487 | ||||
|
488 | LEON_Unmask_interrupt( IRQ_WAVEFORMS ); | |||
|
489 | ||||
|
490 | return 1; | |||
|
491 | } | |||
442 |
|
492 | |||
443 | rtems_task spw_stat_task(rtems_task_argument argument){ |
|
493 | rtems_task spw_stat_task(rtems_task_argument argument){ | |
444 | int i; |
|
494 | int i; | |
445 | i = 0; |
|
495 | i = 0; | |
446 | PRINTF("In STAT *** \n") |
|
496 | PRINTF("In STAT *** \n") | |
447 | while(1){ |
|
497 | while(1){ | |
448 | rtems_task_wake_after(1000); |
|
498 | rtems_task_wake_after(1000); | |
449 | PRINTF1("%d\n", i) |
|
499 | PRINTF1("%d\n", i) | |
450 | if (i == 2) { |
|
500 | if (i == 2) { | |
451 | rtems_cpu_usage_report(); |
|
501 | rtems_cpu_usage_report(); | |
452 | rtems_cpu_usage_reset(); |
|
502 | rtems_cpu_usage_reset(); | |
453 | i = 0; |
|
503 | i = 0; | |
454 | } |
|
504 | } | |
455 | else i++; |
|
505 | else i++; | |
456 | } |
|
506 | } | |
457 | } |
|
507 | } | |
458 |
|
508 | |||
|
509 | char *link_status(int status){ | |||
|
510 | return lstates[status]; | |||
|
511 | } | |||
459 |
|
512 | |||
|
513 | void print_statistics(spw_stats *stats) | |||
|
514 | { | |||
|
515 | //printf(" ******** STATISTICS ******** \n"); | |||
|
516 | printf("Transmit link errors: %i\n", stats->tx_link_err); | |||
|
517 | printf("Receiver RMAP header CRC errors: %i\n", stats->rx_rmap_header_crc_err); | |||
|
518 | printf("Receiver RMAP data CRC errors: %i\n", stats->rx_rmap_data_crc_err); | |||
|
519 | printf("Receiver EEP errors: %i\n", stats->rx_eep_err); | |||
|
520 | printf("Receiver truncation errors: %i\n", stats->rx_truncated); | |||
|
521 | printf("Parity errors: %i\n", stats->parity_err); | |||
|
522 | printf("Escape errors: %i\n", stats->escape_err); | |||
|
523 | printf("Credit errors: %i\n", stats->credit_err); | |||
|
524 | printf("Disconnect errors: %i\n", stats->disconnect_err); | |||
|
525 | printf("Write synchronization errors: %i\n", stats->write_sync_err); | |||
|
526 | printf("Early EOP/EEP: %i\n", stats->early_ep); | |||
|
527 | printf("Invalid Node Address: %i\n", stats->invalid_address); | |||
|
528 | printf("Packets transmitted: %i\n", stats->packets_sent); | |||
|
529 | printf("Packets received: %i\n", stats->packets_received); | |||
|
530 | } | |||
|
531 | ||||
|
532 | ||||
|
533 | ||||
|
534 | ||||
460 |
|
535 |
@@ -1,207 +1,276 | |||||
1 | #include <TC_handler.h> |
|
1 | #include <TC_handler.h> | |
|
2 | #include <FSW-config.h> | |||
|
3 | ||||
|
4 | char *errorCCSDSMsg[8] = { "ILLEGAL_APID 0", | |||
|
5 | "WRONG_LEN_PACKET 1", | |||
|
6 | "INCOR_CHECKSUM 2", | |||
|
7 | "ILL_TYPE 3", | |||
|
8 | "ILL_SUBTYPE 4", | |||
|
9 | "WRONG_APP_DATA 5", | |||
|
10 | "WRONG_CMD_CODE 6", | |||
|
11 | "CCSDS_TM_VALID 7" | |||
|
12 | }; | |||
2 |
|
13 | |||
3 | unsigned int Crc_opt( unsigned char D, unsigned int Chk) |
|
14 | unsigned int Crc_opt( unsigned char D, unsigned int Chk) | |
4 | { |
|
15 | { | |
5 | return(((Chk << 8) & 0xff00)^lookUpTableForCRC [(((Chk >> 8)^D) & 0x00ff)]); |
|
16 | return(((Chk << 8) & 0xff00)^lookUpTableForCRC [(((Chk >> 8)^D) & 0x00ff)]); | |
6 | } |
|
17 | } | |
7 |
|
18 | |||
8 | void InitLookUpTableForCRC() |
|
19 | void InitLookUpTableForCRC() | |
9 | { |
|
20 | { | |
10 | unsigned int i, tmp; |
|
21 | unsigned int i, tmp; | |
11 | for (i=0; i<256; i++) |
|
22 | for (i=0; i<256; i++) | |
12 | { |
|
23 | { | |
13 | tmp = 0; |
|
24 | tmp = 0; | |
14 | if((i & 1) != 0) tmp = tmp ^ 0x1021; |
|
25 | if((i & 1) != 0) tmp = tmp ^ 0x1021; | |
15 | if((i & 2) != 0) tmp = tmp ^ 0x2042; |
|
26 | if((i & 2) != 0) tmp = tmp ^ 0x2042; | |
16 | if((i & 4) != 0) tmp = tmp ^ 0x4084; |
|
27 | if((i & 4) != 0) tmp = tmp ^ 0x4084; | |
17 | if((i & 8) != 0) tmp = tmp ^ 0x8108; |
|
28 | if((i & 8) != 0) tmp = tmp ^ 0x8108; | |
18 | if((i & 16) != 0) tmp = tmp ^ 0x1231; |
|
29 | if((i & 16) != 0) tmp = tmp ^ 0x1231; | |
19 | if((i & 32) != 0) tmp = tmp ^ 0x2462; |
|
30 | if((i & 32) != 0) tmp = tmp ^ 0x2462; | |
20 | if((i & 64) != 0) tmp = tmp ^ 0x48c4; |
|
31 | if((i & 64) != 0) tmp = tmp ^ 0x48c4; | |
21 | if((i & 128) != 0) tmp = tmp ^ 0x9188; |
|
32 | if((i & 128) != 0) tmp = tmp ^ 0x9188; | |
22 | lookUpTableForCRC[i] = tmp; |
|
33 | lookUpTableForCRC[i] = tmp; | |
23 | } |
|
34 | } | |
24 | } |
|
35 | } | |
25 |
|
36 | |||
26 |
void GetCRCAsTwoBytes(unsigned char* data, unsigned char* crcAsTwoBytes, unsigned int sizeOfData) |
|
37 | void GetCRCAsTwoBytes(unsigned char* data, unsigned char* crcAsTwoBytes, unsigned int sizeOfData) | |
|
38 | { | |||
27 | unsigned int Chk; |
|
39 | unsigned int Chk; | |
28 | int j; |
|
40 | int j; | |
29 | Chk = 0xffff; // reset the syndrom to all ones |
|
41 | Chk = 0xffff; // reset the syndrom to all ones | |
30 | for (j=0; j<sizeOfData; j++) { |
|
42 | for (j=0; j<sizeOfData; j++) { | |
31 | Chk = Crc_opt(data[j], Chk); |
|
43 | Chk = Crc_opt(data[j], Chk); | |
32 | } |
|
44 | } | |
33 | crcAsTwoBytes[0] = (unsigned char) (Chk >> 8); |
|
45 | crcAsTwoBytes[0] = (unsigned char) (Chk >> 8); | |
34 | crcAsTwoBytes[1] = (unsigned char) (Chk & 0x00ff); |
|
46 | crcAsTwoBytes[1] = (unsigned char) (Chk & 0x00ff); | |
35 | } |
|
47 | } | |
36 |
|
48 | |||
37 |
unsigned |
|
49 | unsigned int TC_checker(ccsdsTelecommandPacket_t *TC, unsigned int TC_LEN_RCV, | |
|
50 | TMHeader_t *TM_Header, unsigned int *hlen, char *data) | |||
|
51 | { | |||
|
52 | unsigned int code = 0; | |||
|
53 | unsigned int data_length = 0; | |||
|
54 | unsigned char computed_CRC[2]; | |||
|
55 | unsigned char subtype = 0; | |||
|
56 | ||||
|
57 | subtype = TC->dataFieldHeader[2]; | |||
|
58 | GetCRCAsTwoBytes( (unsigned char*) TC->packetID, computed_CRC, TC_LEN_RCV + 5 ); | |||
|
59 | code = acceptTM( TC, TC_LEN_RCV ) ; | |||
|
60 | //PRINTF1("in TC_checker *** %s\n", errorCCSDSMsg[code]); | |||
|
61 | if ( (code == 0) | (code == 1) | (code == 2) | |||
|
62 | | (code == 3) | (code == 4) | (code == 5) ) | |||
|
63 | { // generate TM_LFR_TC_EXE_CORRUPTED | |||
|
64 | // BUILD HEADER | |||
|
65 | TM_build_header( TM_LFR_TC_EXE_ERR, SID_EXE_CORR, TM_LEN_EXE_CORR, 0, 0, TM_Header); | |||
|
66 | // BUILD DATA | |||
|
67 | TM_build_data( TC, data, SID_EXE_CORR, computed_CRC); | |||
|
68 | data_length = TM_LEN_EXE_CORR + CCSDS_TC_TM_PACKET_OFFSET - TM_HEADER_LEN; | |||
|
69 | } | |||
|
70 | if (subtype == SID_TC_UPDT_TIME){ | |||
|
71 | // BUILD HEADER | |||
|
72 | TM_build_header( TM_LFR_TC_EXE_OK, SID_DEFAULT, TM_LEN_EXE, 0, 0, TM_Header); | |||
|
73 | // BUILD DATA | |||
|
74 | TM_build_data( TC, data, SID_DEFAULT, computed_CRC); | |||
|
75 | data_length = TM_LEN_NOT_IMP + CCSDS_TC_TM_PACKET_OFFSET - TM_HEADER_LEN; | |||
|
76 | } | |||
|
77 | else { // TM_LFR_TC_EXE_NOT_IMPLEMENTED | |||
|
78 | // BUILD HEADER | |||
|
79 | TM_build_header( TM_LFR_TC_EXE_ERR, SID_NOT_IMP, TM_LEN_NOT_IMP, 0, 0, TM_Header); | |||
|
80 | // BUILD DATA | |||
|
81 | TM_build_data( TC, data, SID_NOT_IMP, computed_CRC); | |||
|
82 | data_length = TM_LEN_NOT_IMP + CCSDS_TC_TM_PACKET_OFFSET - TM_HEADER_LEN; | |||
|
83 | } | |||
|
84 | ||||
|
85 | return data_length; | |||
|
86 | } | |||
|
87 | ||||
|
88 | unsigned char acceptTM(ccsdsTelecommandPacket_t * TMPacket, unsigned int TC_LEN_RCV) | |||
38 | { |
|
89 | { | |
39 | unsigned char pid = 0; |
|
90 | unsigned char pid = 0; | |
40 | unsigned char category = 0; |
|
91 | unsigned char category = 0; | |
41 | unsigned int length = 0; |
|
92 | unsigned int length = 0; | |
42 | unsigned char packetType = 0; |
|
93 | unsigned char packetType = 0; | |
43 | unsigned char packetSubtype = 0; |
|
94 | unsigned char packetSubtype = 0; | |
44 | unsigned char * CCSDSContent; |
|
95 | unsigned char * CCSDSContent; | |
45 |
|
96 | |||
46 | // APID check *** APID on 2 bytes |
|
97 | // APID check *** APID on 2 bytes | |
47 | pid = ((TMPacket->packetID[0] & 0x07)<<4) + ( (TMPacket->packetID[1]>>4) & 0x0f ); // PID = 11 *** 7 bits xxxxx210 7654xxxx |
|
98 | pid = ((TMPacket->packetID[0] & 0x07)<<4) + ( (TMPacket->packetID[1]>>4) & 0x0f ); // PID = 11 *** 7 bits xxxxx210 7654xxxx | |
48 | category = (TMPacket->packetID[1] & 0x0f); // PACKET_CATEGORY = 12 *** 4 bits xxxxxxxx xxxx3210 |
|
99 | category = (TMPacket->packetID[1] & 0x0f); // PACKET_CATEGORY = 12 *** 4 bits xxxxxxxx xxxx3210 | |
49 | if (pid!=CCSDS_PROCESS_ID) return ILLEGAL_APID; |
|
100 | if (pid!=CCSDS_PROCESS_ID) return ILLEGAL_APID; | |
50 | if (category!=CCSDS_PACKET_CATEGORY) return ILLEGAL_APID; |
|
101 | if (category!=CCSDS_PACKET_CATEGORY) return ILLEGAL_APID; | |
51 |
|
102 | |||
52 | // packet length check |
|
103 | // packet length check | |
53 | length = TMPacket->packetLength[0] * 256 + TMPacket->packetLength[1]; |
|
104 | length = TMPacket->packetLength[0] * 256 + TMPacket->packetLength[1]; | |
54 |
if (length != |
|
105 | if (length != TC_LEN_RCV ) return WRONG_LEN_PACKET; // LEN RCV != SIZE FIELD | |
55 | if (length >= CCSDS_TC_PKT_MAX_SIZE) return WRONG_LEN_PACKET; // check that the packet does not exceed the MAX size |
|
106 | if (length >= CCSDS_TC_PKT_MAX_SIZE) return WRONG_LEN_PACKET; // check that the packet does not exceed the MAX size | |
56 |
|
107 | |||
57 | packetType = TMPacket->dataFieldHeader[1]; |
|
108 | packetType = TMPacket->dataFieldHeader[1]; | |
58 | packetSubtype = TMPacket->dataFieldHeader[2]; |
|
109 | packetSubtype = TMPacket->dataFieldHeader[2]; | |
59 | // service type, subtype and packet length check |
|
110 | // service type, subtype and packet length check | |
60 | if (packetType == 181){ |
|
111 | if (packetType == 181){ | |
61 | switch(packetSubtype){ //subtype, autorized values are 3, 20, 21, 24, 27, 28, 30, 40, 50, 60, 61 |
|
112 | switch(packetSubtype){ //subtype, autorized values are 3, 20, 21, 24, 27, 28, 30, 40, 50, 60, 61 | |
62 | case 3: |
|
113 | case 3: | |
63 | if (length!=(12-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; |
|
114 | if (length!=(12-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; | |
64 | break; |
|
115 | break; | |
65 | case 20: |
|
116 | case 20: | |
66 | if (length!=(14-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; |
|
117 | if (length!=(14-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; | |
67 | break; |
|
118 | break; | |
68 | case 21: |
|
119 | case 21: | |
69 | if (length!=(20-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; |
|
120 | if (length!=(20-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; | |
70 | break; |
|
121 | break; | |
71 | case 24: |
|
122 | case 24: | |
72 | if (length!=(14-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; |
|
123 | if (length!=(14-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; | |
73 | break; |
|
124 | break; | |
74 | case 27: |
|
125 | case 27: | |
75 | if (length!=(14-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; |
|
126 | if (length!=(14-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; | |
76 | break; |
|
127 | break; | |
77 | case 28: |
|
128 | case 28: | |
78 | if (length!=(14-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; |
|
129 | if (length!=(14-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; | |
79 | break; |
|
130 | break; | |
80 | case 30: |
|
131 | case 30: | |
81 | if (length!=(12-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; |
|
132 | if (length!=(12-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; | |
82 | break; |
|
133 | break; | |
83 | case 40: |
|
134 | case 40: | |
84 | if (length!=(20-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; |
|
135 | if (length!=(20-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; | |
85 | break; |
|
136 | break; | |
86 | case 50: |
|
137 | case 50: | |
87 | if (length!=(50-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; |
|
138 | if (length!=(50-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; | |
88 | break; |
|
139 | break; | |
89 | case 60: |
|
140 | case 60: | |
90 | if (length!=(12-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; |
|
141 | if (length!=(12-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; | |
91 | break; |
|
142 | break; | |
92 | case 61: |
|
143 | case 61: | |
93 | if (length!=(12-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; |
|
144 | if (length!=(12-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; | |
94 | break; |
|
145 | break; | |
95 | default: |
|
146 | default: | |
96 | return ILL_SUBTYPE; |
|
147 | return ILL_SUBTYPE; | |
97 | break; |
|
148 | break; | |
98 | } |
|
149 | } | |
99 | } |
|
150 | } | |
100 | else if (packetType == 9){ |
|
151 | else if (packetType == 9){ | |
101 | if (packetSubtype!=129) return ILL_SUBTYPE; |
|
152 | if (packetSubtype!=129) return ILL_SUBTYPE; | |
102 | if (length!=(18-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; |
|
153 | if (length!=(18-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET; | |
103 | } |
|
154 | } | |
104 | else return ILL_TYPE; |
|
155 | else return ILL_TYPE; | |
105 |
|
156 | |||
106 | // source ID check // Source ID not documented in the ICD |
|
157 | // source ID check // Source ID not documented in the ICD | |
107 |
|
158 | |||
108 | // packet error control, CRC check |
|
159 | // packet error control, CRC check | |
109 | CCSDSContent = (unsigned char*) TMPacket->packetID; |
|
160 | CCSDSContent = (unsigned char*) TMPacket->packetID; | |
110 | GetCRCAsTwoBytes(CCSDSContent, currentTC_COMPUTED_CRC, length + CCSDS_TC_TM_PACKET_OFFSET - 2); // 2 CRC bytes removed from the calculation of the CRC |
|
161 | GetCRCAsTwoBytes(CCSDSContent, currentTC_COMPUTED_CRC, length + CCSDS_TC_TM_PACKET_OFFSET - 2); // 2 CRC bytes removed from the calculation of the CRC | |
111 | if (currentTC_COMPUTED_CRC[0] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -2]) return INCOR_CHECKSUM; |
|
162 | if (currentTC_COMPUTED_CRC[0] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -2]) return INCOR_CHECKSUM; | |
112 | if (currentTC_COMPUTED_CRC[1] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -1]) return INCOR_CHECKSUM; |
|
163 | if (currentTC_COMPUTED_CRC[1] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -1]) return INCOR_CHECKSUM; | |
113 |
|
164 | |||
114 | return CCSDS_TM_VALID; |
|
165 | return CCSDS_TM_VALID; | |
115 | } |
|
166 | } | |
116 |
|
167 | |||
117 | unsigned char TM_acceptance_generator(ccsdsTelecommandPacket_t * TCPacket, unsigned int code, ccsdsTelemetryPacket_t * TMPacket) |
|
168 | unsigned char TM_build_header( enum TM_TYPE tm_type, unsigned int SID, unsigned int packetLength, | |
|
169 | unsigned int coarseTime, unsigned int fineTime, TMHeader_t *TMHeader) | |||
118 | { |
|
170 | { | |
119 | unsigned int packetLength = 0; |
|
171 | TMHeader->targetLogicalAddress = CCSDS_DESTINATION_ID; | |
120 | packetLength = TMPacket->packetLength[0] * 256 + TMPacket->packetLength[1]; |
|
172 | TMHeader->protocolIdentifier = 0x02; | |
121 | if ( (code == 0) | (code == 1) | (code == 2) |
|
173 | TMHeader->reserved = 0x00; | |
122 | | (code == 3) | (code == 4) | (code == 5) ) |
|
174 | TMHeader->userApplication = 0x00; | |
123 | { // generated TM_LFR_TC_EXE_CORRUPTED |
|
175 | TMHeader->packetID[0] = 0x0c; | |
124 | TMPacket->targetLogicalAddress = CCSDS_DESTINATION_ID; |
|
176 | TMHeader->packetSequenceControl[0] = 0xc0; | |
125 | TMPacket->protocolIdentifier = 0x02; |
|
177 | TMHeader->packetSequenceControl[1] = 0x00; | |
126 | TMPacket->reserved = 0x00; |
|
178 | TMHeader->packetLength[0] = (unsigned char) (packetLength>>8); | |
127 | TMPacket->userApplication = 0x00; |
|
179 | TMHeader->packetLength[1] = (unsigned char) packetLength; | |
128 | // |
|
180 | TMHeader->dataFieldHeader[0] = 0x10; | |
129 | TMPacket->packetID[0] = 0x0c; |
|
181 | TMHeader->dataFieldHeader[3] = CCSDS_DESTINATION_ID; | |
130 | TMPacket->packetID[1] = 0xc1; |
|
182 | switch (tm_type){ | |
131 | TMPacket->packetSequenceControl[0] = 0xc0; |
|
183 | case(TM_LFR_TC_EXE_OK): | |
132 |
TM |
|
184 | TMHeader->packetID[1] = 0xc1; | |
133 | TMPacket->packetLength[0] = 0x00; |
|
185 | TMHeader->dataFieldHeader[1] = 1; // type | |
134 | TMPacket->packetLength[1] = SIZE_TM_LFR_TC_EXE_CORRUPTED-CCSDS_TC_TM_PACKET_OFFSET; |
|
186 | TMHeader->dataFieldHeader[2] = 7; // subtype | |
135 |
|
|
187 | break; | |
136 | TMPacket->dataFieldHeader[0] = 0x10; |
|
188 | case(TM_LFR_TC_EXE_ERR): | |
137 | TMPacket->dataFieldHeader[1] = 0x01; // service type |
|
189 | TMHeader->packetID[1] = 0xc1; | |
138 |
TM |
|
190 | TMHeader->dataFieldHeader[1] = 1; // type | |
139 |
TM |
|
191 | TMHeader->dataFieldHeader[2] = 8; // subtype | |
140 | TMPacket->dataFieldHeader[4] = 0x00; // time |
|
192 | break; | |
141 | TMPacket->dataFieldHeader[5] = 0x00; // time |
|
193 | case(TM_LFR_HK): | |
142 | TMPacket->dataFieldHeader[6] = 0x00; // time |
|
194 | TMHeader->packetID[1] = 0xc4; | |
143 |
TM |
|
195 | TMHeader->dataFieldHeader[1] = 3; // type | |
144 |
TM |
|
196 | TMHeader->dataFieldHeader[2] = 25; // subtype | |
145 | TMPacket->dataFieldHeader[9] = 0x00; // time |
|
197 | break; | |
146 | // |
|
198 | case(TM_LFR_SCI): | |
147 | TMPacket->data[0] = 0x9c; // failure code |
|
199 | TMHeader->packetID[1] = 0xcc; | |
148 |
TM |
|
200 | TMHeader->dataFieldHeader[1] = 21; // type | |
149 | TMPacket->data[2] = TCPacket->packetID[0]; |
|
201 | TMHeader->dataFieldHeader[2] = 3; // subtype | |
150 | TMPacket->data[3] = TCPacket->packetID[1]; |
|
202 | break; | |
151 | TMPacket->data[4] = TCPacket->packetSequenceControl[0]; |
|
203 | case(TM_LFR_SCI_SBM): | |
152 | TMPacket->data[5] = TCPacket->packetSequenceControl[1]; |
|
204 | TMHeader->packetID[1] = 0xfc; | |
153 |
TM |
|
205 | TMHeader->dataFieldHeader[1] = 21; // type | |
154 |
TM |
|
206 | TMHeader->dataFieldHeader[2] = 3; // subtype | |
155 | TMPacket->data[8] = currentTC_LEN_RCV[0]; |
|
207 | break; | |
156 | TMPacket->data[9] = currentTC_LEN_RCV[1]; |
|
208 | case(TM_LFR_PAR_DUMP): | |
157 | TMPacket->data[10] = TCPacket->packetLength[0]; |
|
209 | TMHeader->packetID[1] = 0xc9; | |
158 | TMPacket->data[11] = TCPacket->packetLength[1]; |
|
210 | TMHeader->dataFieldHeader[1] = 181; // type | |
159 | TMPacket->data[12] = TCPacket->dataAndCRC[packetLength]; |
|
211 | TMHeader->dataFieldHeader[2] = 31; // subtype | |
160 | TMPacket->data[13] = TCPacket->dataAndCRC[packetLength+1]; |
|
212 | break; | |
161 | TMPacket->data[14] = currentTC_COMPUTED_CRC[0]; |
|
213 | default: | |
162 | TMPacket->data[15] = currentTC_COMPUTED_CRC[1]; |
|
214 | return 0; | |
163 | // |
|
215 | } | |
164 | currentTM_length = SIZE_TM_LFR_TC_EXE_CORRUPTED-CCSDS_TC_TM_PACKET_OFFSET; |
|
216 | /*TMHeader->dataFieldHeader[4] = (unsigned char) (coarseTime>>24); | |
165 | } |
|
217 | TMHeader->dataFieldHeader[5] = (unsigned char) (coarseTime>>16); | |
166 | else return 0; |
|
218 | TMHeader->dataFieldHeader[6] = (unsigned char) (coarseTime>>8); | |
|
219 | TMHeader->dataFieldHeader[7] = (unsigned char) (coarseTime); | |||
|
220 | TMHeader->dataFieldHeader[8] = (unsigned char) (fineTime>>8); | |||
|
221 | TMHeader->dataFieldHeader[9] = (unsigned char) (fineTime);*/ | |||
|
222 | TMHeader->dataFieldHeader[4] = 0xaa; | |||
|
223 | TMHeader->dataFieldHeader[5] = 0xbb; | |||
|
224 | TMHeader->dataFieldHeader[6] = 0xcc; | |||
|
225 | TMHeader->dataFieldHeader[7] = 0xdd; | |||
|
226 | TMHeader->dataFieldHeader[8] = 0xee; | |||
|
227 | TMHeader->dataFieldHeader[9] = 0xff; | |||
167 | return 1; |
|
228 | return 1; | |
168 | } |
|
229 | } | |
169 |
|
230 | |||
170 | unsigned char TM_not_implemented_generator(ccsdsTelecommandPacket_t * TCPacket, ccsdsTelemetryPacket_t * TMPacket) |
|
231 | unsigned char TM_build_data(ccsdsTelecommandPacket_t *TC, char* data, unsigned int SID, unsigned char *computed_CRC) | |
171 | { |
|
232 | { | |
172 | TMPacket->targetLogicalAddress = CCSDS_DESTINATION_ID; |
|
233 | unsigned int packetLength; | |
173 | TMPacket->protocolIdentifier = 0x02; |
|
234 | packetLength = TC->packetLength[0] * 256 + TC->packetLength[1]; | |
174 | TMPacket->reserved = 0x00; |
|
235 | switch (SID){ | |
175 | TMPacket->userApplication = 0x00; |
|
236 | case (SID_NOT_EXE): | |
176 | // |
|
237 | break; | |
177 | TMPacket->packetID[0] = 0x0c; |
|
238 | case (SID_NOT_IMP): | |
178 | TMPacket->packetID[1] = 0xc1; |
|
239 | data[0] = 0x9c; | |
179 | TMPacket->packetSequenceControl[0] = 0xc0; |
|
240 | data[1] = 0x42; | |
180 | TMPacket->packetSequenceControl[1] = 0x00; |
|
241 | data[2] = TC->packetID[0]; | |
181 | TMPacket->packetLength[0] = 0x00; |
|
242 | data[3] = TC->packetID[1]; | |
182 | TMPacket->packetLength[1] = SIZE_TM_LFR_TC_EXE_NOT_IMPLEMENTED-CCSDS_TC_TM_PACKET_OFFSET; |
|
243 | data[4] = TC->packetSequenceControl[0]; | |
183 | // |
|
244 | data[5] = TC->packetSequenceControl[1]; | |
184 | TMPacket->dataFieldHeader[0] = 0x10; |
|
245 | data[6] = TC->dataFieldHeader[1]; // type | |
185 |
|
|
246 | data[7] = TC->dataFieldHeader[2]; // subtype | |
186 | TMPacket->dataFieldHeader[2] = 0x08; // service subtype |
|
247 | break; | |
187 | TMPacket->dataFieldHeader[3] = CCSDS_DESTINATION_ID; |
|
248 | case (SID_EXE_ERR): | |
188 | TMPacket->dataFieldHeader[4] = 0x00; // time |
|
249 | break; | |
189 | TMPacket->dataFieldHeader[5] = 0x00; // time |
|
250 | case (SID_EXE_CORR): | |
190 | TMPacket->dataFieldHeader[6] = 0x00; // time |
|
251 | data[0] = 0x9c; | |
191 | TMPacket->dataFieldHeader[7] = 0x00; // time |
|
252 | data[1] = 0x45; | |
192 | TMPacket->dataFieldHeader[8] = 0x00; // time |
|
253 | data[2] = TC->packetID[0]; | |
193 | TMPacket->dataFieldHeader[9] = 0x00; // time |
|
254 | data[3] = TC->packetID[1]; | |
194 | // |
|
255 | data[4] = TC->packetSequenceControl[0]; | |
195 | TMPacket->data[0] = 0x9c; // failure code |
|
256 | data[5] = TC->packetSequenceControl[1]; | |
196 | TMPacket->data[1] = 0x42; // failure code |
|
257 | data[6] = TC->dataFieldHeader[1]; // type | |
197 | TMPacket->data[2] = TCPacket->packetID[0]; |
|
258 | data[7] = TC->dataFieldHeader[2]; // subtype | |
198 | TMPacket->data[3] = TCPacket->packetID[1]; |
|
259 | data[8] = currentTC_LEN_RCV[0]; | |
199 | TMPacket->data[4] = TCPacket->packetSequenceControl[0]; |
|
260 | data[9] = currentTC_LEN_RCV[1]; | |
200 | TMPacket->data[5] = TCPacket->packetSequenceControl[1]; |
|
261 | data[10] = TC->packetLength[0]; | |
201 | TMPacket->data[6] = TCPacket->dataFieldHeader[1]; // type |
|
262 | data[11] = TC->packetLength[1]; | |
202 | TMPacket->data[7] = TCPacket->dataFieldHeader[2]; // subtype |
|
263 | data[12] = TC->dataAndCRC[packetLength]; | |
203 | // |
|
264 | data[13] = TC->dataAndCRC[packetLength+1]; | |
204 | currentTM_length = SIZE_TM_LFR_TC_EXE_NOT_IMPLEMENTED-CCSDS_TC_TM_PACKET_OFFSET; |
|
265 | data[14] = computed_CRC[0]; | |
205 |
|
266 | data[15] = computed_CRC[1]; | ||
|
267 | break; | |||
|
268 | default: | |||
|
269 | return 0; | |||
|
270 | } | |||
206 | return 1; |
|
271 | return 1; | |
207 | } |
|
272 | } | |
|
273 | ||||
|
274 | ||||
|
275 | ||||
|
276 |
General Comments 0
You need to be logged in to leave comments.
Login now