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

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

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
Show file before | Show file after | Hide comments
@@ -1,54 +1,60
1 1 <?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
2 2 <CodeBlocks_project_file>
3 3 <FileVersion major="1" minor="6" />
4 4 <Project>
5 5 <Option title="FSW-rtems" />
6 6 <Option pch_mode="2" />
7 7 <Option compiler="sparc_rtems_gcc" />
8 8 <Build>
9 9 <Target title="Debug">
10 10 <Option output="bin/Debug/fsw" prefix_auto="1" extension_auto="1" />
11 11 <Option object_output="obj/Debug/" />
12 12 <Option type="1" />
13 13 <Option compiler="sparc_rtems_gcc_compiler" />
14 14 <Compiler>
15 <Add option="-fexpensive-optimizations" />
15 16 <Add option="-O3" />
16 17 <Add option="-g" />
17 18 <Add directory="../header" />
18 19 <Add directory="../src" />
19 20 </Compiler>
20 21 </Target>
21 22 <Target title="Release">
22 23 <Option output="bin/Release/FSW-rtems" prefix_auto="1" extension_auto="1" />
23 24 <Option object_output="obj/Release/" />
24 25 <Option type="1" />
25 26 <Option compiler="sparc_rtems_gcc" />
26 27 <Compiler>
27 28 <Add option="-O2" />
28 29 </Compiler>
29 30 <Linker>
30 31 <Add option="-s" />
31 32 </Linker>
32 33 </Target>
33 34 </Build>
34 35 <Compiler>
35 36 <Add option="-Wall" />
36 37 </Compiler>
38 <Unit filename="../header/FSW-config.h" />
37 39 <Unit filename="../header/FSW-rtems-processing.h" />
38 40 <Unit filename="../header/TC_handler.h" />
39 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 46 <Unit filename="../src/FSW-rtems-processing.c">
41 47 <Option compilerVar="CC" />
42 48 </Unit>
43 49 <Unit filename="../src/FSW-rtems.c">
44 50 <Option compilerVar="CC" />
45 51 </Unit>
46 52 <Unit filename="../src/TC_handler.c">
47 53 <Option compilerVar="CC" />
48 54 </Unit>
49 55 <Extensions>
50 56 <code_completion />
51 57 <debugger />
52 58 </Extensions>
53 59 </Project>
54 60 </CodeBlocks_project_file>
Show file before | Show file after | Hide comments
@@ -1,79 +1,91
1 1 # depslib dependency file v1.0
2 2 1355231335 source:e:\missions\solo\dev_ple\src\fsw-rtems.c
3 3 <rtems.h>
4 4 <bsp.h>
5 5 <rtems/confdefs.h>
6 6 <drvmgr/drvmgr_confdefs.h>
7 7 <rtems.h>
8 8 <errno.h>
9 9 <fcntl.h>
10 10 <stdio.h>
11 11 <stdlib.h>
12 12 <FSW-config.c>
13 13 <grspw.h>
14 14 <apbuart.h>
15 15 <FSW-rtems-processing.h>
16 16
17 17 1355231335 -processing.h>
18 18
19 19 1350983928 source:e:\missions\solo\dev_ple\src\grspw.c
20 20 <..\header\grspw.h>
21 21 <stdio.h>
22 22
23 23 1354190601
24 24
25 25 1350983928 e:\missions\solo\dev_ple\header\grspw.h
26 26
27 27 1351173364 source:e:\missions\solo\dev_ple\src\fsw-config.c
28 28 <drvmgr/ambapp_bus.h>
29 29
30 30 1354190602 bapp_bus.h>
31 31
32 32 1354190602 e:\missions\solo\dev_ple\src\fsw-config.c
33 33 <drvmgr/ambapp_bus.h>
34 34
35 35 1354190602 source:e:\missions\solo\dev_ple\src\fsw-rtems-processing.c
36 36 <FSW-rtems-processing.h>
37 37 <math.h>
38 38 <stdio.h>
39 39 <leon.h>
40 40
41 41 1354190601 e:\missions\solo\dev_ple\header\fsw-rtems-processing.h
42 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 45 <FSW-rtems-processing.h>
46 46 <math.h>
47 47 <stdio.h>
48 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 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 56 <rtems.h>
55 57 <bsp.h>
56 58 <rtems/confdefs.h>
57 59 <drvmgr/drvmgr_confdefs.h>
58 60 <rtems.h>
59 61 <errno.h>
60 62 <fcntl.h>
61 63 <stdio.h>
62 64 <stdlib.h>
63 <FSW-config.c>
64 65 <grspw.h>
65 66 <apbuart.h>
67 <FSW-config.h>
66 68 <TC_handler.h>
67 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 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 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 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
Show file before | Show file after | Hide comments
@@ -1,22 +1,31
1 1 <?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
2 2 <CodeBlocks_layout_file>
3 3 <ActiveTarget name="Debug" />
4 <File name="../header/FSW-rtems-processing.h" open="0" top="0" tabpos="3">
5 <Cursor position="82" topLine="0" />
4 <File name="../header/FSW-config.h" open="0" top="0" tabpos="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 12 </File>
7 <File name="../header/TC_handler.h" open="1" top="0" tabpos="2">
8 <Cursor position="352" topLine="0" />
13 <File name="../header/ccsds_types.h" open="1" top="0" tabpos="4">
14 <Cursor position="1571" topLine="40" />
9 15 </File>
10 <File name="../header/ccsds_types.h" open="1" top="0" tabpos="3">
11 <Cursor position="1263" topLine="31" />
16 <File name="../header/grlibregs.h" open="0" top="0" tabpos="6">
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 21 </File>
13 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 24 </File>
16 <File name="../src/FSW-rtems.c" open="1" top="1" tabpos="1">
17 <Cursor position="91" topLine="0" />
25 <File name="../src/FSW-rtems.c" open="1" top="0" tabpos="1">
26 <Cursor position="8119" topLine="99" />
18 27 </File>
19 <File name="../src/TC_handler.c" open="1" top="0" tabpos="4">
20 <Cursor position="7237" topLine="36" />
28 <File name="../src/TC_handler.c" open="1" top="1" tabpos="2">
29 <Cursor position="7593" topLine="53" />
21 30 </File>
22 31 </CodeBlocks_layout_file>
Show file before | Show file after | Hide comments
@@ -1,32 +1,45
1 1 #ifndef FSW_RTEMS_PROCESSING_H_INCLUDED
2 2 #define FSW_RTEMS_PROCESSING_H_INCLUDED
3 3
4 #include <rtems.h>
5
6 #define ADDRESS_APB_SPECTRAL_MATRICES 0x80000700
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
4 #include <rtems.h>
5 #include <grlibregs.h>
6 #include <FSW-config.h>
11 7
12 8 #define NB_BINS_SPECTRAL_MATRIX 128
13 9 #define NB_VALUES_PER_SPECTRAL_MATRIX 25
14 10 #define TOTAL_SIZE_SPECTRAL_MATRIX NB_BINS_SPECTRAL_MATRIX * NB_VALUES_PER_SPECTRAL_MATRIX
15 11 #define NB_BINS_COMPRESSED_MATRIX_f0 11
16 12 #define SIZE_COMPRESSED_SPECTRAL_MATRIX_f1 13
17 13 #define SIZE_COMPRESSED_SPECTRAL_MATRIX_f2 12
18 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
20
21 rtems_isr spectral_matrices_isr( rtems_vector_number vector );
15 #define NB_AVERAGE_NORMAL_f0 96*4
16 #define NB_SM_TO_RECEIVE_BEFORE_AVF0 8
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 33 rtems_task spw_bppr_task(rtems_task_argument argument);
23 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 38 rtems_task spw_bppr_task_rate_monotonic(rtems_task_argument argument);
26 39 void matrix_average(volatile int *spectral_matrix, float *averaged_spectral_matrix);
27 40 void matrix_compression(float *averaged_spectral_matrix, unsigned char fChannel, float *compressed_spectral_matrix);
28 41 void matrix_reset(float *averaged_spectral_matrix);
29 42 void BP1_set(float * compressed_spectral_matrix, unsigned char nb_bins_compressed_spectral_matrix, unsigned char * LFR_BP1);
30 43 void BP2_set(float * compressed_spectral_matrix, unsigned char nb_bins_compressed_spectral_matrix);
31 44
32 45 #endif // FSW_RTEMS_PROCESSING_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,22 +1,25
1 1 #ifndef TC_HANDLER_H_INCLUDED
2 2 #define TC_HANDLER_H_INCLUDED
3
4 #include <ccsds_types.h>
3
4 #include <stdio.h>
5 #include <ccsds_types.h>
5 6
6 7 unsigned char currentTC_LEN_RCV[2]; // SHALL be equal to the current TC packet estimated packet length field
7 8 unsigned char currentTC_COMPUTED_CRC[2];
8 9 unsigned int currentTC_LEN_RCV_AsUnsignedInt;
9 10 unsigned int currentTM_length;
10 ccsdsTelemetryPacket_t currentTM;
11 ccsdsTelecommandPacket_t currentTC;
12 11 unsigned char currentTC_processedFlag;
13 12
14 13 unsigned int lookUpTableForCRC[256];
15 14 void InitLookUpTableForCRC();
16 15 void GetCRCAsTwoBytes(unsigned char* data, unsigned char* crcAsTwoBytes, unsigned int sizeOfData);
17 16
18 unsigned char TM_checker(ccsdsTelecommandPacket_t * TMPacket);
19 unsigned char TM_acceptance_generator(ccsdsTelecommandPacket_t * TCPacket, unsigned int code, ccsdsTelemetryPacket_t * TMPacket);
20 unsigned char TM_not_implemented_generator(ccsdsTelecommandPacket_t * TCPacket, ccsdsTelemetryPacket_t * TMPacket);
17 unsigned char acceptTM(ccsdsTelecommandPacket_t * TMPacket, unsigned int TC_LEN_RCV);
18
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 25 #endif // TC_HANDLER_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,63 +1,118
1 1 #ifndef CCSDS_H_INCLUDED
2 2 #define CCSDS_H_INCLUDED
3 3
4 4 #define CCSDS_TELEMETRY_HEADER_LENGTH 16+4
5 5 #define CCSDS_TM_PKT_MAX_SIZE 4412
6 6 #define CCSDS_TELECOMMAND_HEADER_LENGTH 10+4
7 7 #define CCSDS_TC_PKT_MAX_SIZE 248
8 8 #define CCSDS_TC_TM_PACKET_OFFSET 7
9 9 #define CCSDS_PROCESS_ID 76
10 10 #define CCSDS_PACKET_CATEGORY 12
11 11 #define CCSDS_DESTINATION_ID 0x21
12 #define CCSDS_PROTOCOLE_ID 0x02
13 #define CCSDS_USER_APP 0x00
12 14
13 15 #define SIZE_TM_LFR_TC_EXE_NOT_IMPLEMENTED 24
14 16 #define SIZE_TM_LFR_TC_EXE_CORRUPTED 32
15 17
16 18 #define ILLEGAL_APID 0
17 19 #define WRONG_LEN_PACKET 1
18 20 #define INCOR_CHECKSUM 2
19 21 #define ILL_TYPE 3
20 22 #define ILL_SUBTYPE 4
21 23 #define WRONG_APP_DATA 5
22 24 #define WRONG_CMD_CODE 6
23 25 #define CCSDS_TM_VALID 7
24 26 //
25 27 #define TC_NOT_EXE 40000
26 28 #define WRONG_SRC_ID 40001
27 29 #define FUNCT_NOT_IMPL 40002
28 30 #define FAIL_DETECTED 40003
29 31 #define NOT_ALLOWED 40004
30 32 #define CORRUPTED 40005
31 //
32 #define CCSDS_ERR_SRC 8
33 #define CCSDS_ERR_CAT 9
34
35 struct ccsdsTelemetryPacket_str
33 //
34 #define SID_TC_RESET 3
35 #define SID_TC_LOAD_COMM 20
36 #define SID_TC_LOAD_NORM 21
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 79 volatile unsigned char targetLogicalAddress;
38 80 volatile unsigned char protocolIdentifier;
39 81 volatile unsigned char reserved;
40 82 volatile unsigned char userApplication;
41 83 volatile unsigned char packetID[2];
42 84 volatile unsigned char packetSequenceControl[2];
43 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 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 104 struct ccsdsTelecommandPacket_str
50 105 {
51 //unsigned char targetLogicalAddress;
106 //unsigned char targetLogicalAddress; // removed by the grspw module
52 107 volatile unsigned char protocolIdentifier;
53 108 volatile unsigned char reserved;
54 109 volatile unsigned char userApplication;
55 110 volatile unsigned char packetID[2];
56 111 volatile unsigned char packetSequenceControl[2];
57 112 volatile unsigned char packetLength[2];
58 113 volatile unsigned char dataFieldHeader[4];
59 114 volatile unsigned char dataAndCRC[CCSDS_TC_PKT_MAX_SIZE-10];
60 115 };
61 116 typedef struct ccsdsTelecommandPacket_str ccsdsTelecommandPacket_t;
62 117
63 118 #endif // CCSDS_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,53 +1,53
1 1 #include <drvmgr/ambapp_bus.h>
2 2
3 3 // GRSPW0 resources
4 4 struct drvmgr_key grlib_grspw_0n1_res[] = {
5 5 {"txBdCnt", KEY_TYPE_INT, {(unsigned int)10}},
6 6 {"rxBdCnt", KEY_TYPE_INT, {(unsigned int)10}},
7 {"txDataSize", KEY_TYPE_INT, {(unsigned int)48+4}},
8 {"txHdrSize", KEY_TYPE_INT, {(unsigned int)10}},
7 {"txDataSize", KEY_TYPE_INT, {(unsigned int)4096}},
8 {"txHdrSize", KEY_TYPE_INT, {(unsigned int)20+6}}, // 6 is for the auxiliary header, when needed
9 9 {"rxPktSize", KEY_TYPE_INT, {(unsigned int)248+4}},
10 10 KEY_EMPTY
11 11 };
12 12
13 13 #if 0
14 14 /* APBUART0 */
15 15 struct drvmgr_key grlib_drv_res_apbuart0[] =
16 16 {
17 17 {"mode", KEY_TYPE_INT, {(unsigned int)1}},
18 18 {"syscon", KEY_TYPE_INT, {(unsigned int)1}},
19 19 KEY_EMPTY
20 20 };
21 21 /* APBUART1 */
22 22 struct drvmgr_key grlib_drv_res_apbuart1[] =
23 23 {
24 24 {"mode", KEY_TYPE_INT, {(unsigned int)1}},
25 25 {"syscon", KEY_TYPE_INT, {(unsigned int)0}},
26 26 KEY_EMPTY
27 27 };
28 28 /* LEON3 System with driver configuration for 2 APBUARTs, the
29 29 * the rest of the AMBA device drivers use their defaults.
30 30 */
31 31
32 32 /* Override default debug UART assignment.
33 33 * 0 = Default APBUART. APBUART[0], but on MP system CPU0=APBUART0,
34 34 * CPU1=APBUART1...
35 35 * 1 = APBUART[0]
36 36 * 2 = APBUART[1]
37 37 * 3 = APBUART[2]
38 38 * ...
39 39 */
40 40 //int debug_uart_index = 2; /* second UART -- APBUART[1] */
41 41 #endif
42 42
43 43 // If RTEMS_DRVMGR_STARTUP is defined we override the "weak defaults" that is defined by the LEON3 BSP.
44 44
45 45 struct drvmgr_bus_res grlib_drv_resources = {
46 46 .next = NULL,
47 47 .resource = {
48 48 {DRIVER_AMBAPP_GAISLER_GRSPW_ID, 0, &grlib_grspw_0n1_res[0]},
49 49 // {DRIVER_AMBAPP_GAISLER_APBUART_ID, 0, &grlib_drv_res_apbuart0[0]},
50 50 // {DRIVER_AMBAPP_GAISLER_APBUART_ID, 1, &grlib_drv_res_apbuart1[0]},
51 51 RES_EMPTY /* Mark end of device resource array */
52 52 }
53 53 };
Show file before | Show file after | Hide comments
@@ -1,397 +1,423
1 1 #include <FSW-rtems-processing.h>
2 2 #include<math.h>
3 3 #include <stdio.h>
4 4 #include <leon.h>
5 5
6 6 float k14_re = 1;
7 7 float k14_im = 1;
8 8 float k14_bis_re = 1;
9 9 float k14_bis_im = 1;
10 10 float k14_tris_re = 1;
11 11 float k14_tris_im = 1;
12 12 float k15_re = 1;
13 13 float k15_im = 1;
14 14 float k15_bis_re = 1;
15 15 float k15_bis_im = 1;
16 16 float k24_re = 1;
17 17 float k24_im = 1;
18 18 float k24_bis_re = 1;
19 19 float k24_bis_im = 1;
20 20 float k24_tris_re = 1;
21 21 float k24_tris_im = 1;
22 22 float k25_re = 1;
23 23 float k25_im = 1;
24 24 float k25_bis_re = 1;
25 25 float k25_bis_im = 1;
26 26 float k34_re = 1;
27 27 float k34_im = 1;
28 28 float k44 = 1;
29 29 float k55 = 1;
30 30 float k45_re = 1;
31 31 float k45_im = 1;
32 32 float alpha_M = M_PI/4;
33 33
34 34 volatile int spectral_matrix_f0_a[TOTAL_SIZE_SPECTRAL_MATRIX];
35 35 volatile int spectral_matrix_f0_b[TOTAL_SIZE_SPECTRAL_MATRIX];
36 36 volatile int spectral_matrix_f0_c[TOTAL_SIZE_SPECTRAL_MATRIX];
37 37 volatile int spectral_matrix_f0_d[TOTAL_SIZE_SPECTRAL_MATRIX];
38 38 volatile int spectral_matrix_f0_e[TOTAL_SIZE_SPECTRAL_MATRIX];
39 39 volatile int spectral_matrix_f0_f[TOTAL_SIZE_SPECTRAL_MATRIX];
40 40 volatile int spectral_matrix_f0_g[TOTAL_SIZE_SPECTRAL_MATRIX];
41 41 volatile int spectral_matrix_f0_h[TOTAL_SIZE_SPECTRAL_MATRIX];
42 42 float averaged_spectral_matrix_f0[TOTAL_SIZE_SPECTRAL_MATRIX];
43 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 48 extern rtems_id Task_id[]; /* array of task ids */
47
48 struct spectral_matrices_regs_str{
49 volatile int ctrl;
50 volatile int address0;
51 volatile int address1;
52 };
53 struct spectral_matrices_regs_str *spectral_matrices_regs;
49
50 spectral_matrices_regs_t *spectral_matrices_regs;
54 51
55 52 // Interrupt Service Routine for spectral matrices processing
56 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)
59 printf("In spectral_matrices_isr *** Error sending event to BPPR\n");
60 }
55 if (rtems_event_send( Task_id[4], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
56 printf("In spectral_matrices_isr *** Error sending event to AVF0\n");
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 78 rtems_task spw_bppr_task(rtems_task_argument argument)
63 79 {
64 80 rtems_status_code status;
65 81 rtems_event_set event_out;
66 82 static int nb_average_f0 = 0;
67 83 //static int nb_average_f1 = 0;
68 84 //static int nb_average_f2 = 0;
69 85
70 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 89 spectral_matrices_regs->address0 = (volatile int) spectral_matrix_f0_a;
74 90 spectral_matrices_regs->address1 = (volatile int) spectral_matrix_f0_b;
75 91
76 92 printf("In BPPR ***\n");
77 93
78 94 while(1){ // wait for an event to begin with the processing
79 95 status = rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out);
80 96 if (status == RTEMS_SUCCESSFUL){
81 97 if ((spectral_matrices_regs->ctrl & 0x00000001)==1){
82 98 matrix_average(spectral_matrix_f0_a, averaged_spectral_matrix_f0);
83 99 spectral_matrices_regs->ctrl = spectral_matrices_regs->ctrl & 0xfffffffe;
84 100 //printf("f0_a\n");
85 101 nb_average_f0++;
86 102 }
87 103 if (((spectral_matrices_regs->ctrl>>1) & 0x00000001)==1){
88 104 matrix_average(spectral_matrix_f0_b, compressed_spectral_matrix_f0);
89 105 spectral_matrices_regs->ctrl = spectral_matrices_regs->ctrl & 0xfffffffd;
90 106 //printf("f0_b\n");
91 107 nb_average_f0++;
92 108 }
93 109 if (nb_average_f0 == NB_AVERAGE_NORMAL_f0){
94 110 matrix_compression(averaged_spectral_matrix_f0, 0, compressed_spectral_matrix_f0);
95 111 //printf("f0 compressed\n");
96 112 nb_average_f0 = 0;
97 113 matrix_reset(averaged_spectral_matrix_f0);
98 114 }
99 115 }
100 116 }
101 117 }
102 118
103 119 void matrix_average(volatile int *spectral_matrix, float *averaged_spectral_matrix)
104 120 {
105 121 int i;
106 122 for(i=0; i<TOTAL_SIZE_SPECTRAL_MATRIX; i++){
107 123 averaged_spectral_matrix[i] = averaged_spectral_matrix[i] + spectral_matrix_f0_a[i]
108 124 + spectral_matrix_f0_b[i]
109 125 + spectral_matrix_f0_c[i]
110 126 + spectral_matrix_f0_d[i]
111 127 + spectral_matrix_f0_e[i]
112 128 + spectral_matrix_f0_f[i]
113 129 + spectral_matrix_f0_g[i]
114 130 + spectral_matrix_f0_h[i];
115 131 }
116 132 }
117 133
118 134 void matrix_reset(float *averaged_spectral_matrix)
119 135 {
120 136 int i;
121 137 for(i=0; i<TOTAL_SIZE_SPECTRAL_MATRIX; i++){
122 138 averaged_spectral_matrix_f0[i] = 0;
123 139 }
124 140 }
125 141
126 142 void matrix_compression(float *averaged_spectral_matrix, unsigned char fChannel, float *compressed_spectral_matrix)
127 143 {
128 144 int i, j;
129 145 switch (fChannel){
130 146 case 0:
131 147 for(i=0;i<NB_BINS_COMPRESSED_MATRIX_f0;i++){
132 148 j = 17 + i * 8;
133 149 compressed_spectral_matrix[i] = (averaged_spectral_matrix[j]
134 150 + averaged_spectral_matrix[j+1]
135 151 + averaged_spectral_matrix[j+2]
136 152 + averaged_spectral_matrix[j+3]
137 153 + averaged_spectral_matrix[j+4]
138 154 + averaged_spectral_matrix[j+5]
139 155 + averaged_spectral_matrix[j+6]
140 156 + averaged_spectral_matrix[j+7])/(8*NB_AVERAGE_NORMAL_f0);
141 157 }
142 158 break;
143 159 case 1:
144 160 // case fChannel = f1 tp be completed later
145 161 break;
146 162 case 2:
147 163 // case fChannel = f1 tp be completed later
148 164 break;
149 165 default:
150 166 break;
151 167 }
152 168 }
153 169
154 170 void BP1_set(float * compressed_spectral_matrix, unsigned char nb_bins_compressed_spectral_matrix, unsigned char * LFR_BP1){
155 171 int i, j;
156 172 unsigned char tmp_u_char;
157 173 unsigned char * pt_char;
158 174 float PSDB, PSDE;
159 175 float NVEC_V0, NVEC_V1, NVEC_V2;
160 176 float significand;
161 177 int exponent;
162 178 float aux, tr_SB_SB, tmp;
163 179 float e_cross_b_re, e_cross_b_im;
164 180 float n_cross_e_scal_b_re = 0, n_cross_e_scal_b_im = 0;
165 181 float nx = 0, ny = 0;
166 182 float bz_bz_star = 0;
167 183 for(i=0; i<nb_bins_compressed_spectral_matrix; i++){
168 184 //==============================================
169 185 // BP1 PSD == B PAR_LFR_SC_BP1_PE_FL0 == 16 bits
170 186 PSDB = compressed_spectral_matrix[i*30] // S11
171 187 + compressed_spectral_matrix[i*30+10] // S22
172 188 + compressed_spectral_matrix[i*30+18]; // S33
173 189 significand = frexp(PSDB, &exponent);
174 190 pt_char = (unsigned char*) &PSDB;
175 191 LFR_BP1[i*9+8] = pt_char[0]; // bits 31 downto 24 of the float
176 192 LFR_BP1[i*9+7] = pt_char[1]; // bits 23 downto 16 of the float
177 193 //==============================================
178 194 // BP1 PSD == E PAR_LFR_SC_BP1_PB_FL0 == 16 bits
179 195 PSDE = compressed_spectral_matrix[i*30+24] * k44 // S44
180 196 + compressed_spectral_matrix[i*30+28] * k55 // S55
181 197 + compressed_spectral_matrix[i*30+26] * k45_re // S45
182 198 - compressed_spectral_matrix[i*30+27] * k45_im; // S45
183 199 pt_char = (unsigned char*) &PSDE;
184 200 LFR_BP1[i*9+6] = pt_char[0]; // bits 31 downto 24 of the float
185 201 LFR_BP1[i*9+5] = pt_char[1]; // bits 23 downto 16 of the float
186 202 //==============================================================================
187 203 // BP1 normal wave vector == PAR_LFR_SC_BP1_NVEC_V0_F0 == 8 bits
188 204 // == PAR_LFR_SC_BP1_NVEC_V1_F0 == 8 bits
189 205 // == PAR_LFR_SC_BP1_NVEC_V2_F0 == 1 bits
190 206 tmp = sqrt(
191 207 compressed_spectral_matrix[i*30+3]*compressed_spectral_matrix[i*30+3] //Im S12
192 208 +compressed_spectral_matrix[i*30+5]*compressed_spectral_matrix[i*30+5] //Im S13
193 209 +compressed_spectral_matrix[i*30+5]*compressed_spectral_matrix[i*30+13] //Im S23
194 210 );
195 211 NVEC_V0 = compressed_spectral_matrix[i*30+13] / tmp; // Im S23
196 212 NVEC_V1 = -compressed_spectral_matrix[i*30+5] / tmp; // Im S13
197 213 NVEC_V2 = compressed_spectral_matrix[i*30+1] / tmp; // Im S12
198 214 LFR_BP1[i*9+4] = (char) (NVEC_V0*256);
199 215 LFR_BP1[i*9+3] = (char) (NVEC_V1*256);
200 216 pt_char = (unsigned char*) &NVEC_V2;
201 217 LFR_BP1[i*9+2] = pt_char[0] & 0x80; // extract the sign of NVEC_V2
202 218 //=======================================================
203 219 // BP1 ellipticity == PAR_LFR_SC_BP1_ELLIP_F0 == 4 bits
204 220 aux = 2*tmp / PSDB; // compute the ellipticity
205 221 tmp_u_char = (unsigned char) (aux*(16-1)); // convert the ellipticity
206 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 224 // BP1 degree of polarization == PAR_LFR_SC_BP1_DOP_F0 == 3 bits
209 225 for(j = 0; j<NB_VALUES_PER_SPECTRAL_MATRIX;j++){
210 226 tr_SB_SB = compressed_spectral_matrix[i*30]*compressed_spectral_matrix[i*30]
211 227 + compressed_spectral_matrix[i*30+10]*compressed_spectral_matrix[i*30+10]
212 228 + compressed_spectral_matrix[i*30+18]*compressed_spectral_matrix[i*30+18]
213 229 + 2 * compressed_spectral_matrix[i*30+2]*compressed_spectral_matrix[i*30+2]
214 230 + 2 * compressed_spectral_matrix[i*30+3]*compressed_spectral_matrix[i*30+3]
215 231 + 2 * compressed_spectral_matrix[i*30+4]*compressed_spectral_matrix[i*30+4]
216 232 + 2 * compressed_spectral_matrix[i*30+5]*compressed_spectral_matrix[i*30+5]
217 233 + 2 * compressed_spectral_matrix[i*30+12]*compressed_spectral_matrix[i*30+12]
218 234 + 2 * compressed_spectral_matrix[i*30+13]*compressed_spectral_matrix[i*30+13];
219 235 }
220 236 aux = PSDB*PSDB;
221 237 tmp = ( 3*tr_SB_SB - aux ) / ( 2 * aux );
222 238 tmp_u_char = (unsigned char) (NVEC_V0*(8-1));
223 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 241 // BP1 z-component of the normalized Poynting flux == PAR_LFR_SC_BP1_SZ_F0 == 8 bits (7+1)
226 242 e_cross_b_re = compressed_spectral_matrix[i*30+20]*k34_re
227 243 + compressed_spectral_matrix[i*30+6]*k14_re
228 244 + compressed_spectral_matrix[i*30+8]*k15_re
229 245 + compressed_spectral_matrix[i*30+14]*k24_re
230 246 + compressed_spectral_matrix[i*30+16]*k25_re;
231 247 e_cross_b_im = compressed_spectral_matrix[i*30+21]*k34_im
232 248 + compressed_spectral_matrix[i*30+7]*k14_im
233 249 + compressed_spectral_matrix[i*30+9]*k15_im
234 250 + compressed_spectral_matrix[i*30+15]*k24_im
235 251 + compressed_spectral_matrix[i*30+17]*k25_im;
236 252 tmp = e_cross_b_re / PSDE; // compute ReaSz
237 253 LFR_BP1[i*9+1] = ((unsigned char) (tmp * 128)) & 0x7f; // is it always positive?
238 254 tmp = e_cross_b_re * e_cross_b_im;
239 255 pt_char = (unsigned char*) &tmp;
240 256 LFR_BP1[i*9+1] = LFR_BP1[i*9+1] | (pt_char[0] & 0x80); // extract the sign of ArgSz
241 257 //======================================================================
242 258 // BP1 phase velocity estimator == PAR_LFR_SC_BP1_VPHI_F0 == 8 bits (7+1)
243 259 nx = -sin(alpha_M)*NVEC_V0 - cos(alpha_M)*NVEC_V1;
244 260 ny = NVEC_V2;
245 261 bz_bz_star = cos(alpha_M) * cos(alpha_M) * compressed_spectral_matrix[i*30] // re S11
246 262 + sin(alpha_M) * sin(alpha_M) * compressed_spectral_matrix[i*30+10] // re S22
247 263 - 2 * sin(alpha_M) * cos(alpha_M) * compressed_spectral_matrix[i*30+2]; // re S12
248 264 n_cross_e_scal_b_re = nx * (compressed_spectral_matrix[i*30+8]*k15_bis_re
249 265 +compressed_spectral_matrix[i*30+6]*k14_bis_re
250 266 +compressed_spectral_matrix[i*30+16]*k25_bis_re
251 267 +compressed_spectral_matrix[i*30+14]*k24_bis_re)
252 268 + ny * (compressed_spectral_matrix[i*30+6]*k14_tris_re
253 269 +compressed_spectral_matrix[i*30+14]*k24_tris_re);
254 270 n_cross_e_scal_b_im = nx * (compressed_spectral_matrix[i*30+8]*k15_bis_im
255 271 +compressed_spectral_matrix[i*30+6]*k14_bis_im
256 272 +compressed_spectral_matrix[i*30+16]*k25_bis_im
257 273 +compressed_spectral_matrix[i*30+14]*k24_bis_im)
258 274 + ny * (compressed_spectral_matrix[i*30+6]*k14_tris_im
259 275 +compressed_spectral_matrix[i*30+14]*k24_tris_im);
260 276 tmp = n_cross_e_scal_b_re / bz_bz_star;
261 277 LFR_BP1[i*9+0] = ((unsigned char) (tmp * 128)) & 0x7f; // is it always positive?
262 278 tmp = n_cross_e_scal_b_re * n_cross_e_scal_b_im;
263 279 pt_char = (unsigned char*) &tmp;
264 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 285 void BP2_set(float * compressed_spectral_matrix, unsigned char nb_bins_compressed_spectral_matrix){
270 286 // BP2 autocorrelation
271 287 int i, aux = 0;
272 288 for(i = 0; i<nb_bins_compressed_spectral_matrix; i++){
273 289 // S12
274 290 aux = sqrt(compressed_spectral_matrix[i*30]*compressed_spectral_matrix[i*30+10]);
275 291 compressed_spectral_matrix[i*30+2] = compressed_spectral_matrix[i*30+2] / aux;
276 292 compressed_spectral_matrix[i*30+3] = compressed_spectral_matrix[i*30+3] / aux;
277 293 // S13
278 294 aux = sqrt(compressed_spectral_matrix[i*30]*compressed_spectral_matrix[i*30+18]);
279 295 compressed_spectral_matrix[i*30+4] = compressed_spectral_matrix[i*30+4] / aux;
280 296 compressed_spectral_matrix[i*30+5] = compressed_spectral_matrix[i*30+5] / aux;
281 297 // S23
282 298 aux = sqrt(compressed_spectral_matrix[i*30+12]*compressed_spectral_matrix[i*30+18]);
283 299 compressed_spectral_matrix[i*30+12] = compressed_spectral_matrix[i*30+12] / aux;
284 300 compressed_spectral_matrix[i*30+13] = compressed_spectral_matrix[i*30+13] / aux;
285 301 // S45
286 302 aux = sqrt(compressed_spectral_matrix[i*30+24]*compressed_spectral_matrix[i*30+28]);
287 303 compressed_spectral_matrix[i*30+26] = compressed_spectral_matrix[i*30+26] / aux;
288 304 compressed_spectral_matrix[i*30+27] = compressed_spectral_matrix[i*30+27] / aux;
289 305 // S14
290 306 aux = sqrt(compressed_spectral_matrix[i*30]*compressed_spectral_matrix[i*30+24]);
291 307 compressed_spectral_matrix[i*30+6] = compressed_spectral_matrix[i*30+6] / aux;
292 308 compressed_spectral_matrix[i*30+7] = compressed_spectral_matrix[i*30+7] / aux;
293 309 // S15
294 310 aux = sqrt(compressed_spectral_matrix[i*30]*compressed_spectral_matrix[i*30+28]);
295 311 compressed_spectral_matrix[i*30+8] = compressed_spectral_matrix[i*30+8] / aux;
296 312 compressed_spectral_matrix[i*30+9] = compressed_spectral_matrix[i*30+9] / aux;
297 313 // S24
298 314 aux = sqrt(compressed_spectral_matrix[i*10]*compressed_spectral_matrix[i*30+24]);
299 315 compressed_spectral_matrix[i*30+14] = compressed_spectral_matrix[i*30+14] / aux;
300 316 compressed_spectral_matrix[i*30+15] = compressed_spectral_matrix[i*30+15] / aux;
301 317 // S25
302 318 aux = sqrt(compressed_spectral_matrix[i*10]*compressed_spectral_matrix[i*30+28]);
303 319 compressed_spectral_matrix[i*30+16] = compressed_spectral_matrix[i*30+16] / aux;
304 320 compressed_spectral_matrix[i*30+17] = compressed_spectral_matrix[i*30+17] / aux;
305 321 // S34
306 322 aux = sqrt(compressed_spectral_matrix[i*18]*compressed_spectral_matrix[i*30+24]);
307 323 compressed_spectral_matrix[i*30+20] = compressed_spectral_matrix[i*30+20] / aux;
308 324 compressed_spectral_matrix[i*30+21] = compressed_spectral_matrix[i*30+21] / aux;
309 325 // S35
310 326 aux = sqrt(compressed_spectral_matrix[i*18]*compressed_spectral_matrix[i*30+28]);
311 327 compressed_spectral_matrix[i*30+22] = compressed_spectral_matrix[i*30+22] / aux;
312 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 381 rtems_task spw_bppr_task_rate_monotonic(rtems_task_argument argument)
317 {
382 {/*
318 383 rtems_status_code status;
319 384 //static int nb_average_f1 = 0;
320 385 //static int nb_average_f2 = 0;
321 386
322 387 rtems_name name;
323 388 rtems_id period;
324 389 name = rtems_build_name( 'P', 'E', 'R', 'D' );
325 390 status = rtems_rate_monotonic_create( name, &period );
326 391 if( status != RTEMS_SUCCESSFUL ) {
327 392 printf( "rtems_rate_monotonic_create failed with status of %d\n", status );
328 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 397 spectral_matrices_regs->address0 = (volatile int) spectral_matrix_f0_a;
333 398 spectral_matrices_regs->address1 = (volatile int) spectral_matrix_f0_b;
334 399
335 400 printf("In BPPR BIS ***\n");
336 401
337 402 while(1){ // launch the rate monotonic task
338 403 if ( rtems_rate_monotonic_period( period, 8 ) == RTEMS_TIMEOUT ){
339 404 printf("TIMEOUT\n");
340 405 //break;
341 406 }
342 407 status = rtems_event_send( Task_id[6], RTEMS_EVENT_0 ); // sending an event to the task 6, AVF0
343 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 411 status = rtems_rate_monotonic_delete( period );
347 412 if ( status != RTEMS_SUCCESSFUL ) {
348 413 printf( "rtems_rate_monotonic_delete failed with status of %d.\n", status );
349 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 417 printf( "rtems_task_delete returned with status of %d.\n", status );
353 //exit( 1 );
354 }
355
356 rtems_task spw_avf0_task(rtems_task_argument argument){
357 int i;
358 static int nb_average;
359 rtems_event_set event_out;
360 rtems_status_code status;
361
362 nb_average = 0;
418 //exit( 1 );*/
419 }
420
421
422
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
Show file before | Show file after | Hide comments
This diff has been collapsed as it changes many lines, (579 lines changed) Show them Hide them
@@ -1,460 +1,535
1 1 //*************************
2 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 5 #define OFFSET_COARSE_TIME 4+10-1 // -1 => the receiver suppresses the target logical address
17 6 #define OFFSET_FINE_TIME OFFSET_COARSE_TIME+4
18 7 #define TC_FIFO_SIZE 5
19 8
20 9 #define ADDRESS_APBUART_REGISTERS 0x80000100
21 10 #define ADDRESS_GRSPW_REGISTERS 0x80000500
22 11 #define APBUART_CTRL_REG_MASK_DB 0xfffff7ff
23 12
24 13 #define GRSPW_DEVICE_NAME "/dev/grspw0"
25 14 #define UART_DEVICE_NAME "/dev/console"
26 15
27 16 #include <rtems.h>
28 17
29 18 /* configuration information */
30 19
31 20 #define CONFIGURE_INIT
32 21 #include <bsp.h> /* for device driver prototypes */
33 22
34 23 /* configuration information */
35 24
36 25 #define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
37 26 #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
38 27
39 28 #define CONFIGURE_MAXIMUM_TASKS 10
40 29 #define CONFIGURE_RTEMS_INIT_TASKS_TABLE
41 30 #define CONFIGURE_EXTRA_TASK_STACKS (3 * RTEMS_MINIMUM_STACK_SIZE)
42 31 #define CONFIGURE_LIBIO_MAXIMUM_FILE_DESCRIPTORS 32
43 32 #define CONFIGURE_INIT_TASK_PRIORITY 100
44 33 #define CONFIGURE_MAXIMUM_DRIVERS 16
45 34 #define CONFIGURE_MAXIMUM_PERIODS 1
46 35
47 36 #include <rtems/confdefs.h>
48 37
49 38 /* If --drvmgr was enabled during the configuration of the RTEMS kernel */
50 39 #ifdef RTEMS_DRVMGR_STARTUP
51 40 #ifdef LEON3
52 41 /* Add Timer and UART Driver */
53 42 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
54 43 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
55 44 #endif
56 45 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
57 46 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
58 47 #endif
59 48 #endif
60 49 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
61 50 #include <drvmgr/drvmgr_confdefs.h>
62 51 #endif
63 52
64 53 #include <rtems.h>
65 54 #include <errno.h>
66 55 #include <fcntl.h>
67 56 #include <stdio.h>
68 57 #include <stdlib.h>
69 58
70 #include <FSW-config.c>
59 //#include <FSW-config.c>
71 60 #include <grspw.h>
72 61 #include <apbuart.h>
62 #include <FSW-config.h>
73 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",
77 "WRONG_LEN_PACKET 1",
78 "INCOR_CHECKSUM 2",
79 "ILL_TYPE 3",
80 "ILL_SUBTYPE 4",
81 "WRONG_APP_DATA 5",
82 "WRONG_CMD_CODE 6",
83 "CCSDS_TM_VALID 7"
84 };
67 /*char *tmGeneratorMsg[2] = { "NOTHING_TO_DO",
68 "TM_GENERATED"
69 };*/
85 70
86 char *tmGeneratorMsg[2] = { "NOTHING_TO_DO",
87 "TM_GENERATED"
88 };
89
71 // RTEMS TASKS
90 72 rtems_task Init( rtems_task_argument argument); /* forward declaration needed */
91 73 rtems_task spw_recv_task(rtems_task_argument argument);
92 rtems_task spw_tcck_task(rtems_task_argument argument);
93 74 rtems_task spw_spiq_task(rtems_task_argument argument);
94 rtems_task spw_stat_task(rtems_task_argument argument);
95 int configure_spw_link();
75 rtems_task spw_stat_task(rtems_task_argument argument);
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 86 extern int rtems_cpu_usage_report();
97 87 extern int rtems_cpu_usage_reset();
98 88
99 89 char *link_status(int status);
100 90 void print_statistics(spw_stats *);
101 91 extern int sched_yield();
102 92 extern int errno;
103 93
104 94 char *lstates[6] = {"Error-reset",
105 95 "Error-wait",
106 96 "Ready",
107 97 "Started",
108 98 "Connecting",
109 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 102 int fdSPW;
131 103 int fdUART;
132 104
133 105 rtems_id Task_id[10]; /* array of task ids */
134 rtems_name Task_name[10]; /* array of task names */
135 rtems_name sem_tc_fifo_name;
136 rtems_id sem_tc_fifo_id;
106 rtems_name Task_name[10]; /* array of task names */
137 107
138 108 rtems_task Init( rtems_task_argument ignored ) {
139 109 rtems_status_code status;
140 //struct apbuart_regs_str *apbuart_regs;
141 rtems_isr_entry old_isr_handler;
110
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 313 Task_name[1] = rtems_build_name( 'R', 'E', 'C', 'V' );
144 Task_name[2] = rtems_build_name( 'T', 'C', 'C', 'K' );
145 Task_name[3] = rtems_build_name( 'S', 'P', 'I', 'Q' );
146 Task_name[4] = rtems_build_name( 'B', 'P', 'P', 'R' );
314 Task_name[3] = rtems_build_name( 'S', 'P', 'I', 'Q' );
315 Task_name[4] = rtems_build_name( 'S', 'M', 'I', 'Q' );
147 316 Task_name[5] = rtems_build_name( 'S', 'T', 'A', 'T' );
148 317 Task_name[6] = rtems_build_name( 'A', 'V', 'F', '0' );
149 Task_name[7] = rtems_build_name( 'B', 'P', 'F', '0' );
150 sem_tc_fifo_name = rtems_build_name( 'S', 'E', 'M', '0' );
318 Task_name[7] = rtems_build_name( 'B', 'P', 'F', '0' );
319 Task_name[8] = rtems_build_name( 'W', 'F', 'R', 'M' );
151 320
152 321 // RECV
153 322 status = rtems_task_create(
154 323 Task_name[1], 200, RTEMS_MINIMUM_STACK_SIZE * 2,
155 324 RTEMS_DEFAULT_MODES,
156 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 327 // SPIQ
165 328 status = rtems_task_create(
166 329 Task_name[3], 50, RTEMS_MINIMUM_STACK_SIZE * 2,
167 330 RTEMS_DEFAULT_MODES,
168 331 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[3]
169 332 );
170 // BPPR
333 // SMIQ
171 334 status = rtems_task_create(
172 335 Task_name[4], 10, RTEMS_MINIMUM_STACK_SIZE * 2,
173 336 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
174 337 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[4]
175 338 );
176 339 // STAT
177 340 status = rtems_task_create(
178 341 Task_name[5], 200, RTEMS_MINIMUM_STACK_SIZE * 2,
179 342 RTEMS_DEFAULT_MODES,
180 343 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[5]
181 344 );
182 345 // AVF0
183 346 status = rtems_task_create(
184 347 Task_name[6], 50, RTEMS_MINIMUM_STACK_SIZE * 2,
185 348 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
186 349 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[6]
187 350 );
188 351 // BPF0
189 352 status = rtems_task_create(
190 353 Task_name[7], 50, RTEMS_MINIMUM_STACK_SIZE * 2,
191 354 RTEMS_DEFAULT_MODES,
192 355 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[7]
193 );
194
195 status = rtems_semaphore_create(
196 sem_tc_fifo_name,
197 1,
198 RTEMS_BINARY_SEMAPHORE,
199 1,
200 &sem_tc_fifo_id
201 );
202
203 //********************************************
204 // Send the console outputs on the serial port
205 //apbuart_regs = (struct apbuart_regs_str *) ADDRESS_APBUART_REGISTERS;
206 //apbuart_regs->ctrl = apbuart_regs->ctrl & APBUART_CTRL_REG_MASK_DB;
207 //PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
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
356 );
357 // WFRM
358 status = rtems_task_create(
359 Task_name[8], 100, RTEMS_MINIMUM_STACK_SIZE * 2,
360 RTEMS_DEFAULT_MODES,
361 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[8]
362 );
363
364 return 0;
365 }
366
367 int start_all_tasks()
368 {
369 rtems_status_code status;
370
221 371 status = rtems_task_start( Task_id[3], spw_spiq_task, 1 );
222 372 if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_SPIQ\n")
223 373
224 374 status = rtems_task_start( Task_id[1], spw_recv_task, 1 );
225 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 );
228 //if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_BPPR\n")
377 //status = rtems_task_start( Task_id[4], spw_bppr_task_rate_monotonic, 1 );
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 381 status = rtems_task_start( Task_id[5], spw_stat_task, 1 );
231 382 if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_STAT\n")
232 383
233 384 status = rtems_task_start( Task_id[6], spw_avf0_task, 1 );
234 385 if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_AVF0\n")
235 386
236 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
240
241 status = rtems_task_delete(RTEMS_SELF);
242 }
390 status = rtems_task_start( Task_id[8], spw_wfrm_task, 1 );
391 if (status!=RTEMS_SUCCESSFUL) PRINTF("In INIT *** Error starting TASK_WFRM\n")
243 392
244 rtems_task spw_recv_task_old( rtems_task_argument unused ) {
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);
393 return 0;
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){
305 while(currentTC_processedFlag == 0) sched_yield();
306 len = read(fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE); // the call to read is blocking
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 }
400 apbuart_regs = (struct apbuart_regs_str *) ADDRESS_APBUART_REGISTERS;
401 apbuart_regs->ctrl = apbuart_regs->ctrl & APBUART_CTRL_REG_MASK_DB;
402 PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
354 403
355 currentTC_processedFlag = 1;
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];
404 return 0;
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 407 int configure_spw_link()
403 408 {
404 409 rtems_status_code status;
405 410
406 411 close(fdSPW); // close the device if it is already open
407 412 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call reset the hardware
408 413 if (fdSPW<0) PRINTF("In configure_spw_link *** Error opening"GRSPW_DEVICE_NAME"\n")
409 414 while(ioctl(fdSPW, SPACEWIRE_IOCTRL_START, 0) != RTEMS_SUCCESSFUL){
410 415 PRINTF("In configure_spw_link *** "GRSPW_DEVICE_NAME" not started, retry\n")
411 416 close(fdSPW); // close the device
412 417 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call reset the hardware
413 418 if (fdSPW<0) PRINTF("In configure_spw_link *** Error opening"GRSPW_DEVICE_NAME"\n")
414 419 rtems_task_wake_after(100);
415 420 }
416 421
417 422 PRINTF("In configure_spw_link *** "GRSPW_DEVICE_NAME" opened and started successfully\n")
418 423
419 424 // sets a few parameters of the link
420 425 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_RMAPEN, 1); // sets the RMAP enable bit
421 426 if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_RMAPEN\n")
422 427 //
423 428 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
424 429 if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
425 430 //
426 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 432 if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
428 433 //
429 434 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 1); // automatic link-disabling due to link-error interrupts
430 435 if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
431 436 //
432 437 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
433 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 444 //if (status!=RTEMS_SUCCESSFUL) PRINTF("In RECV *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
437 445 //
438 446 PRINTF("In configure_spw_link *** "GRSPW_DEVICE_NAME" configured successfully\n")
439 447
440 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 493 rtems_task spw_stat_task(rtems_task_argument argument){
444 494 int i;
445 495 i = 0;
446 496 PRINTF("In STAT *** \n")
447 497 while(1){
448 498 rtems_task_wake_after(1000);
449 499 PRINTF1("%d\n", i)
450 500 if (i == 2) {
451 501 rtems_cpu_usage_report();
452 502 rtems_cpu_usage_reset();
453 503 i = 0;
454 504 }
455 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
Show file before | Show file after | Hide comments
@@ -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 14 unsigned int Crc_opt( unsigned char D, unsigned int Chk)
4 15 {
5 16 return(((Chk << 8) & 0xff00)^lookUpTableForCRC [(((Chk >> 8)^D) & 0x00ff)]);
6 17 }
7 18
8 19 void InitLookUpTableForCRC()
9 20 {
10 21 unsigned int i, tmp;
11 22 for (i=0; i<256; i++)
12 23 {
13 24 tmp = 0;
14 25 if((i & 1) != 0) tmp = tmp ^ 0x1021;
15 26 if((i & 2) != 0) tmp = tmp ^ 0x2042;
16 27 if((i & 4) != 0) tmp = tmp ^ 0x4084;
17 28 if((i & 8) != 0) tmp = tmp ^ 0x8108;
18 29 if((i & 16) != 0) tmp = tmp ^ 0x1231;
19 30 if((i & 32) != 0) tmp = tmp ^ 0x2462;
20 31 if((i & 64) != 0) tmp = tmp ^ 0x48c4;
21 32 if((i & 128) != 0) tmp = tmp ^ 0x9188;
22 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 39 unsigned int Chk;
28 40 int j;
29 41 Chk = 0xffff; // reset the syndrom to all ones
30 42 for (j=0; j<sizeOfData; j++) {
31 43 Chk = Crc_opt(data[j], Chk);
32 44 }
33 45 crcAsTwoBytes[0] = (unsigned char) (Chk >> 8);
34 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 90 unsigned char pid = 0;
40 91 unsigned char category = 0;
41 92 unsigned int length = 0;
42 93 unsigned char packetType = 0;
43 94 unsigned char packetSubtype = 0;
44 95 unsigned char * CCSDSContent;
45 96
46 97 // APID check *** APID on 2 bytes
47 98 pid = ((TMPacket->packetID[0] & 0x07)<<4) + ( (TMPacket->packetID[1]>>4) & 0x0f ); // PID = 11 *** 7 bits xxxxx210 7654xxxx
48 99 category = (TMPacket->packetID[1] & 0x0f); // PACKET_CATEGORY = 12 *** 4 bits xxxxxxxx xxxx3210
49 100 if (pid!=CCSDS_PROCESS_ID) return ILLEGAL_APID;
50 101 if (category!=CCSDS_PACKET_CATEGORY) return ILLEGAL_APID;
51 102
52 103 // packet length check
53 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 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 108 packetType = TMPacket->dataFieldHeader[1];
58 109 packetSubtype = TMPacket->dataFieldHeader[2];
59 110 // service type, subtype and packet length check
60 111 if (packetType == 181){
61 112 switch(packetSubtype){ //subtype, autorized values are 3, 20, 21, 24, 27, 28, 30, 40, 50, 60, 61
62 113 case 3:
63 114 if (length!=(12-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET;
64 115 break;
65 116 case 20:
66 117 if (length!=(14-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET;
67 118 break;
68 119 case 21:
69 120 if (length!=(20-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET;
70 121 break;
71 122 case 24:
72 123 if (length!=(14-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET;
73 124 break;
74 125 case 27:
75 126 if (length!=(14-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET;
76 127 break;
77 128 case 28:
78 129 if (length!=(14-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET;
79 130 break;
80 131 case 30:
81 132 if (length!=(12-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET;
82 133 break;
83 134 case 40:
84 135 if (length!=(20-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET;
85 136 break;
86 137 case 50:
87 138 if (length!=(50-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET;
88 139 break;
89 140 case 60:
90 141 if (length!=(12-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET;
91 142 break;
92 143 case 61:
93 144 if (length!=(12-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET;
94 145 break;
95 146 default:
96 147 return ILL_SUBTYPE;
97 148 break;
98 149 }
99 150 }
100 151 else if (packetType == 9){
101 152 if (packetSubtype!=129) return ILL_SUBTYPE;
102 153 if (length!=(18-CCSDS_TC_TM_PACKET_OFFSET)) return WRONG_LEN_PACKET;
103 154 }
104 155 else return ILL_TYPE;
105 156
106 157 // source ID check // Source ID not documented in the ICD
107 158
108 159 // packet error control, CRC check
109 160 CCSDSContent = (unsigned char*) TMPacket->packetID;
110 161 GetCRCAsTwoBytes(CCSDSContent, currentTC_COMPUTED_CRC, length + CCSDS_TC_TM_PACKET_OFFSET - 2); // 2 CRC bytes removed from the calculation of the CRC
111 162 if (currentTC_COMPUTED_CRC[0] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -2]) return INCOR_CHECKSUM;
112 163 if (currentTC_COMPUTED_CRC[1] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -1]) return INCOR_CHECKSUM;
113 164
114 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;
120 packetLength = TMPacket->packetLength[0] * 256 + TMPacket->packetLength[1];
121 if ( (code == 0) | (code == 1) | (code == 2)
122 | (code == 3) | (code == 4) | (code == 5) )
123 { // generated TM_LFR_TC_EXE_CORRUPTED
124 TMPacket->targetLogicalAddress = CCSDS_DESTINATION_ID;
125 TMPacket->protocolIdentifier = 0x02;
126 TMPacket->reserved = 0x00;
127 TMPacket->userApplication = 0x00;
128 //
129 TMPacket->packetID[0] = 0x0c;
130 TMPacket->packetID[1] = 0xc1;
131 TMPacket->packetSequenceControl[0] = 0xc0;
132 TMPacket->packetSequenceControl[1] = 0x00;
133 TMPacket->packetLength[0] = 0x00;
134 TMPacket->packetLength[1] = SIZE_TM_LFR_TC_EXE_CORRUPTED-CCSDS_TC_TM_PACKET_OFFSET;
135 //
136 TMPacket->dataFieldHeader[0] = 0x10;
137 TMPacket->dataFieldHeader[1] = 0x01; // service type
138 TMPacket->dataFieldHeader[2] = 0x08; // service subtype
139 TMPacket->dataFieldHeader[3] = CCSDS_DESTINATION_ID;
140 TMPacket->dataFieldHeader[4] = 0x00; // time
141 TMPacket->dataFieldHeader[5] = 0x00; // time
142 TMPacket->dataFieldHeader[6] = 0x00; // time
143 TMPacket->dataFieldHeader[7] = 0x00; // time
144 TMPacket->dataFieldHeader[8] = 0x00; // time
145 TMPacket->dataFieldHeader[9] = 0x00; // time
146 //
147 TMPacket->data[0] = 0x9c; // failure code
148 TMPacket->data[1] = 0x45; // failure code
149 TMPacket->data[2] = TCPacket->packetID[0];
150 TMPacket->data[3] = TCPacket->packetID[1];
151 TMPacket->data[4] = TCPacket->packetSequenceControl[0];
152 TMPacket->data[5] = TCPacket->packetSequenceControl[1];
153 TMPacket->data[6] = TCPacket->dataFieldHeader[1]; // type
154 TMPacket->data[7] = TCPacket->dataFieldHeader[2]; // subtype
155 TMPacket->data[8] = currentTC_LEN_RCV[0];
156 TMPacket->data[9] = currentTC_LEN_RCV[1];
157 TMPacket->data[10] = TCPacket->packetLength[0];
158 TMPacket->data[11] = TCPacket->packetLength[1];
159 TMPacket->data[12] = TCPacket->dataAndCRC[packetLength];
160 TMPacket->data[13] = TCPacket->dataAndCRC[packetLength+1];
161 TMPacket->data[14] = currentTC_COMPUTED_CRC[0];
162 TMPacket->data[15] = currentTC_COMPUTED_CRC[1];
163 //
164 currentTM_length = SIZE_TM_LFR_TC_EXE_CORRUPTED-CCSDS_TC_TM_PACKET_OFFSET;
165 }
166 else return 0;
171 TMHeader->targetLogicalAddress = CCSDS_DESTINATION_ID;
172 TMHeader->protocolIdentifier = 0x02;
173 TMHeader->reserved = 0x00;
174 TMHeader->userApplication = 0x00;
175 TMHeader->packetID[0] = 0x0c;
176 TMHeader->packetSequenceControl[0] = 0xc0;
177 TMHeader->packetSequenceControl[1] = 0x00;
178 TMHeader->packetLength[0] = (unsigned char) (packetLength>>8);
179 TMHeader->packetLength[1] = (unsigned char) packetLength;
180 TMHeader->dataFieldHeader[0] = 0x10;
181 TMHeader->dataFieldHeader[3] = CCSDS_DESTINATION_ID;
182 switch (tm_type){
183 case(TM_LFR_TC_EXE_OK):
184 TMHeader->packetID[1] = 0xc1;
185 TMHeader->dataFieldHeader[1] = 1; // type
186 TMHeader->dataFieldHeader[2] = 7; // subtype
187 break;
188 case(TM_LFR_TC_EXE_ERR):
189 TMHeader->packetID[1] = 0xc1;
190 TMHeader->dataFieldHeader[1] = 1; // type
191 TMHeader->dataFieldHeader[2] = 8; // subtype
192 break;
193 case(TM_LFR_HK):
194 TMHeader->packetID[1] = 0xc4;
195 TMHeader->dataFieldHeader[1] = 3; // type
196 TMHeader->dataFieldHeader[2] = 25; // subtype
197 break;
198 case(TM_LFR_SCI):
199 TMHeader->packetID[1] = 0xcc;
200 TMHeader->dataFieldHeader[1] = 21; // type
201 TMHeader->dataFieldHeader[2] = 3; // subtype
202 break;
203 case(TM_LFR_SCI_SBM):
204 TMHeader->packetID[1] = 0xfc;
205 TMHeader->dataFieldHeader[1] = 21; // type
206 TMHeader->dataFieldHeader[2] = 3; // subtype
207 break;
208 case(TM_LFR_PAR_DUMP):
209 TMHeader->packetID[1] = 0xc9;
210 TMHeader->dataFieldHeader[1] = 181; // type
211 TMHeader->dataFieldHeader[2] = 31; // subtype
212 break;
213 default:
214 return 0;
215 }
216 /*TMHeader->dataFieldHeader[4] = (unsigned char) (coarseTime>>24);
217 TMHeader->dataFieldHeader[5] = (unsigned char) (coarseTime>>16);
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 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;
173 TMPacket->protocolIdentifier = 0x02;
174 TMPacket->reserved = 0x00;
175 TMPacket->userApplication = 0x00;
176 //
177 TMPacket->packetID[0] = 0x0c;
178 TMPacket->packetID[1] = 0xc1;
179 TMPacket->packetSequenceControl[0] = 0xc0;
180 TMPacket->packetSequenceControl[1] = 0x00;
181 TMPacket->packetLength[0] = 0x00;
182 TMPacket->packetLength[1] = SIZE_TM_LFR_TC_EXE_NOT_IMPLEMENTED-CCSDS_TC_TM_PACKET_OFFSET;
183 //
184 TMPacket->dataFieldHeader[0] = 0x10;
185 TMPacket->dataFieldHeader[1] = 0x01; // service type
186 TMPacket->dataFieldHeader[2] = 0x08; // service subtype
187 TMPacket->dataFieldHeader[3] = CCSDS_DESTINATION_ID;
188 TMPacket->dataFieldHeader[4] = 0x00; // time
189 TMPacket->dataFieldHeader[5] = 0x00; // time
190 TMPacket->dataFieldHeader[6] = 0x00; // time
191 TMPacket->dataFieldHeader[7] = 0x00; // time
192 TMPacket->dataFieldHeader[8] = 0x00; // time
193 TMPacket->dataFieldHeader[9] = 0x00; // time
194 //
195 TMPacket->data[0] = 0x9c; // failure code
196 TMPacket->data[1] = 0x42; // failure code
197 TMPacket->data[2] = TCPacket->packetID[0];
198 TMPacket->data[3] = TCPacket->packetID[1];
199 TMPacket->data[4] = TCPacket->packetSequenceControl[0];
200 TMPacket->data[5] = TCPacket->packetSequenceControl[1];
201 TMPacket->data[6] = TCPacket->dataFieldHeader[1]; // type
202 TMPacket->data[7] = TCPacket->dataFieldHeader[2]; // subtype
203 //
204 currentTM_length = SIZE_TM_LFR_TC_EXE_NOT_IMPLEMENTED-CCSDS_TC_TM_PACKET_OFFSET;
205
233 unsigned int packetLength;
234 packetLength = TC->packetLength[0] * 256 + TC->packetLength[1];
235 switch (SID){
236 case (SID_NOT_EXE):
237 break;
238 case (SID_NOT_IMP):
239 data[0] = 0x9c;
240 data[1] = 0x42;
241 data[2] = TC->packetID[0];
242 data[3] = TC->packetID[1];
243 data[4] = TC->packetSequenceControl[0];
244 data[5] = TC->packetSequenceControl[1];
245 data[6] = TC->dataFieldHeader[1]; // type
246 data[7] = TC->dataFieldHeader[2]; // subtype
247 break;
248 case (SID_EXE_ERR):
249 break;
250 case (SID_EXE_CORR):
251 data[0] = 0x9c;
252 data[1] = 0x45;
253 data[2] = TC->packetID[0];
254 data[3] = TC->packetID[1];
255 data[4] = TC->packetSequenceControl[0];
256 data[5] = TC->packetSequenceControl[1];
257 data[6] = TC->dataFieldHeader[1]; // type
258 data[7] = TC->dataFieldHeader[2]; // subtype
259 data[8] = currentTC_LEN_RCV[0];
260 data[9] = currentTC_LEN_RCV[1];
261 data[10] = TC->packetLength[0];
262 data[11] = TC->packetLength[1];
263 data[12] = TC->dataAndCRC[packetLength];
264 data[13] = TC->dataAndCRC[packetLength+1];
265 data[14] = computed_CRC[0];
266 data[15] = computed_CRC[1];
267 break;
268 default:
269 return 0;
270 }
206 271 return 1;
207 272 }
273
274
275
276
General Comments 0
You need to be logged in to leave comments. Login now