##// END OF EJS Templates
Several bugs corrected on the TC handler (related TMs are now compliant)...
paul@pc-solar1.lab-lpp.local -
r3:a7739b90c971 default
parent child
Show More
@@ -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 1358144203 source:/opt/DEV_PLE/src/FSW-rtems-processing.c
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 1358144202 /opt/DEV_PLE/header/FSW-rtems-processing.h
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 1358441917 source:/opt/DEV_PLE/src/FSW-rtems.c
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 1358144203 /opt/DEV_PLE/src/FSW-config.c
72 1358847387 /opt/DEV_PLE/src/FSW-config.c
70 <drvmgr/ambapp_bus.h>
73 <drvmgr/ambapp_bus.h>
71
74
72 1358441070 source:/opt/DEV_PLE/src/TC_handler.c
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 1358435124 /opt/DEV_PLE/header/TC_handler.h
79 1359377846 /opt/DEV_PLE/header/TC_handler.h
80 <stdio.h>
76 <ccsds_types.h>
81 <ccsds_types.h>
77
82
78 1358440633 /opt/DEV_PLE/header/ccsds_types.h
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-rtems-processing.h" open="0" top="0" tabpos="3">
4 <File name="../header/FSW-config.h" open="0" top="0" tabpos="0">
5 <Cursor position="82" topLine="0" />
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/TC_handler.h" open="1" top="0" tabpos="2">
13 <File name="../header/ccsds_types.h" open="1" top="0" tabpos="4">
8 <Cursor position="352" topLine="0" />
14 <Cursor position="1571" topLine="40" />
9 </File>
15 </File>
10 <File name="../header/ccsds_types.h" open="1" top="0" tabpos="3">
16 <File name="../header/grlibregs.h" open="0" top="0" tabpos="6">
11 <Cursor position="1263" topLine="31" />
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="1" tabpos="1">
25 <File name="../src/FSW-rtems.c" open="1" top="0" tabpos="1">
17 <Cursor position="91" topLine="0" />
26 <Cursor position="8119" topLine="99" />
18 </File>
27 </File>
19 <File name="../src/TC_handler.c" open="1" top="0" tabpos="4">
28 <File name="../src/TC_handler.c" open="1" top="1" tabpos="2">
20 <Cursor position="7237" topLine="36" />
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 <ccsds_types.h>
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 TM_checker(ccsdsTelecommandPacket_t * TMPacket);
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 data[CCSDS_TM_PKT_MAX_SIZE-16];
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)48+4}},
7 {"txDataSize", KEY_TYPE_INT, {(unsigned int)4096}},
8 {"txHdrSize", KEY_TYPE_INT, {(unsigned int)10}},
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 struct spectral_matrices_regs_str{
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 BPPR\n");
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_APB_SPECTRAL_MATRICES;
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_APB_SPECTRAL_MATRICES;
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 ); /* should not return */
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 //exit( 1 );
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 *errorCCSDSMsg[8] = { "ILLEGAL_APID 0",
67 /*char *tmGeneratorMsg[2] = { "NOTHING_TO_DO",
77 "WRONG_LEN_PACKET 1",
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*) &currentTC, 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(&currentTC, 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[2] = rtems_build_name( 'T', 'C', 'C', 'K' );
314 Task_name[3] = rtems_build_name( 'S', 'P', 'I', 'Q' );
145 Task_name[3] = rtems_build_name( 'S', 'P', 'I', 'Q' );
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 sem_tc_fifo_name = rtems_build_name( 'S', 'E', 'M', '0' );
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 // BPPR
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_semaphore_create(
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*) &currentTC, 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(&currentTC);
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(&currentTC, &currentTM);
344 PRINTF1(" %s\n", tmGeneratorMsg[result])
345 result = write( fdSPW, (char*)&currentTM, 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(&currentTC, code, &currentTM);
350 PRINTF1(" %s\n", tmGeneratorMsg[result])
351 result = write( fdSPW, (char*)&currentTM, 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 destinatino key
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 char TM_checker(ccsdsTelecommandPacket_t * TMPacket)
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 != (currentTC_LEN_RCV[0] * 256 + currentTC_LEN_RCV[1]) ) return WRONG_LEN_PACKET; // LEN RCV != SIZE FIELD
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 TMPacket->packetSequenceControl[1] = 0x00;
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 TMPacket->dataFieldHeader[2] = 0x08; // service subtype
190 TMHeader->dataFieldHeader[1] = 1; // type
139 TMPacket->dataFieldHeader[3] = CCSDS_DESTINATION_ID;
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 TMPacket->dataFieldHeader[7] = 0x00; // time
195 TMHeader->dataFieldHeader[1] = 3; // type
144 TMPacket->dataFieldHeader[8] = 0x00; // time
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 TMPacket->data[1] = 0x45; // failure code
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 TMPacket->data[6] = TCPacket->dataFieldHeader[1]; // type
205 TMHeader->dataFieldHeader[1] = 21; // type
154 TMPacket->data[7] = TCPacket->dataFieldHeader[2]; // subtype
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 TMPacket->dataFieldHeader[1] = 0x01; // service type
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