##// END OF EJS Templates
HG_REPOSITORIES » LPP » INSTRUMENTATION » USERS » JEANDET » LFR_FSW
RSS

Fork of HG_REPOSITORIES/LPP/INSTRUMENTATION/SOLO_LFR/DEV_PLE

Clone from https://hephaistos.lpp.polytechnique.fr/rhodecode/HG_REPOSITORIES/LPP/INSTRUMENTATION/SOLO_LFR/DEV_PLE

fifo occupation reported in the HK packet
paul -
r197:d5570d7e57e2 R3
parent child
Show More
Show file before | Show file after | Hide comments
@@ -0,0 +1,75
1 TEMPLATE = app
2 # CONFIG += console v8 sim
3 # CONFIG options = verbose *** boot_messages *** debug_messages *** cpu_usage_report *** stack_report *** vhdl_dev *** debug_tch
4 # lpp_dpu_destid
5 CONFIG += console verbose lpp_dpu_destid cpu_usage_report
6 CONFIG -= qt
7
8 include(./sparc.pri)
9
10 # eqm debug software version
11 SWVERSION=-1-0
12 DEFINES += SW_VERSION_N1=0 # major
13 DEFINES += SW_VERSION_N2=0 # minor
14 DEFINES += SW_VERSION_N3=0 # patch
15 DEFINES += SW_VERSION_N4=0 # internal
16
17 # <GCOV>
18 #QMAKE_CFLAGS_RELEASE += -fprofile-arcs -ftest-coverage
19 #LIBS += -lgcov /opt/GCOV/01A/lib/overload.o -lc
20 # </GCOV>
21
22 # <CHANGE BEFORE FLIGHT>
23 contains( CONFIG, lpp_dpu_destid ) {
24 DEFINES += LPP_DPU_DESTID
25 }
26 # </CHANGE BEFORE FLIGHT>
27
28 contains( CONFIG, debug_tch ) {
29 DEFINES += DEBUG_TCH
30 }
31 DEFINES += MSB_FIRST_TCH
32
33 contains( CONFIG, vhdl_dev ) {
34 DEFINES += VHDL_DEV
35 }
36
37 contains( CONFIG, verbose ) {
38 DEFINES += PRINT_MESSAGES_ON_CONSOLE
39 }
40
41 contains( CONFIG, debug_messages ) {
42 DEFINES += DEBUG_MESSAGES
43 }
44
45 contains( CONFIG, cpu_usage_report ) {
46 DEFINES += PRINT_TASK_STATISTICS
47 }
48
49 contains( CONFIG, stack_report ) {
50 DEFINES += PRINT_STACK_REPORT
51 }
52
53 contains( CONFIG, boot_messages ) {
54 DEFINES += BOOT_MESSAGES
55 }
56
57 #doxygen.target = doxygen
58 #doxygen.commands = doxygen ../doc/Doxyfile
59 #QMAKE_EXTRA_TARGETS += doxygen
60
61 TARGET = eqm
62
63 INCLUDEPATH += ./header \
64 ../header/lfr_common_headers
65
66 SOURCES += \
67 src/main.c \
68 src/grspw.c
69
70 HEADERS += \
71 ../header/lfr_common_headers/fsw_params.h \
72 header/grspw.h
73
74
75
Show file before | Show file after | Hide comments
@@ -0,0 +1,8
1 #ifndef GRSPW_H_INCLUDED
2 #define GRSPW_H_INCLUDED
3
4 int grspw_set_ie( unsigned char value, unsigned int *ctrlReg );
5 int grspw_set_tq( unsigned char value, unsigned int *ctrlReg );
6 int grspw_set_tr( unsigned char value, unsigned int *ctrlReg );
7
8 #endif // GRSPW_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -0,0 +1,97
1 CONFIG += console
2 CONFIG -= qt
3 QMAKE_CC=sparc-elf-gcc
4 message(C compiler forced to: $$QMAKE_CC)
5 QMAKE_CXX=sparc-elf-g++
6 message(C++ compiler forced to: $$QMAKE_CXX)
7 QMAKE_AR=sparc-elf-ar rcs
8 message(Archiver forced to: $$QMAKE_AR)
9 QMAKE_LINK=sparc-elf-g++
10 message(Linker forced to: $$QMAKE_LINK)
11 QMAKE_LINK_SHLIB=sparc-rtems-g++
12 QMAKE_OBJCOPY= sparc-elf-objcopy
13 QMAKE_STRIP=sparc-elf-strip
14 QMAKE_GDB=sparc-elf-gdb
15
16 INCLUDEPATH += /opt/sparc-elf-4.4.2
17
18 QMAKE_CFLAGS_DEBUG= -g
19 QMAKE_CFLAGS_RELEASE=""
20 QMAKE_CXXFLAGS_DEBUG= -g
21 QMAKE_CXXFLAGS_RELEASE=""
22 QMAKE_LFLAGS_RELEASE=""
23 QMAKE_LFLAGS_DEBUG= -g
24 QMAKE_CXXFLAGS_DEPS =
25 QMAKE_CXXFLAGS_WARN_ON = -Wall
26 QMAKE_CXXFLAGS_WARN_OFF = -w
27 QMAKE_CXXFLAGS_RELEASE =
28 QMAKE_CXXFLAGS_DEBUG =
29 QMAKE_CXXFLAGS_YACC =
30 QMAKE_CXXFLAGS_THREAD =
31 QMAKE_CXXFLAGS_RTTI_ON =
32 QMAKE_CXXFLAGS_RTTI_OFF =
33 QMAKE_CXXFLAGS_EXCEPTIONS_ON =
34 QMAKE_CXXFLAGS_EXCEPTIONS_OFF =
35 QMAKE_CFLAGS_WARN_ON = -Wall
36 QMAKE_CFLAGS_WARN_OFF = -w
37 QMAKE_CFLAGS_RELEASE =
38 QMAKE_CFLAGS_YACC =
39 QMAKE_LFLAGS_EXCEPTIONS_ON =
40 QMAKE_LFLAGS_EXCEPTIONS_OFF =
41 QMAKE_LFLAGS_RELEASE =
42 QMAKE_LFLAGS_CONSOLE =
43 QMAKE_LFLAGS_WINDOWS =
44 QMAKE_LFLAGS_DLL =
45 QMAKE_INCDIR_QT =
46 QMAKE_INCDIR =
47 QMAKE_CFLAGS_SHLIB =
48 QMAKE_CFLAGS_STATIC_LIB =
49 QMAKE_CXXFLAGS_SHLIB =
50 QMAKE_CXXFLAGS_STATIC_LIB =
51 QMAKE_LIBS=""
52 INCLUDEPATH=""
53 DEFINES=""
54
55 contains( TEMPLATE, app ) {
56 OBJECTS_DIR=obj
57 DESTDIR=bin
58 }
59
60 #QMAKE_CFLAGS_RELEASE += -O0
61 #QMAKE_CFLAGS_DEBUG += -O0
62 #QMAKE_CXXFLAGS_RELEASE += -O0
63 #QMAKE_CXXFLAGS_DEBUG += -O0
64 QMAKE_CFLAGS_RELEASE += -O3
65 QMAKE_CFLAGS_DEBUG += -O3
66 QMAKE_CXXFLAGS_RELEASE += -O3
67 QMAKE_CXXFLAGS_DEBUG += -O3
68
69 #QMAKE_CFLAGS_RELEASE+= -O3 -std=c99
70 #QMAKE_CFLAGS_DEBUG+= -O3 -std=c99
71 #QMAKE_CXXFLAGS_RELEASE+= -O3 -std=c99
72 #QMAKE_CXXFLAGS_DEBUG+= -O3 -std=c99
73
74 contains( TEMPLATE, app ) {
75 grmon.target = grmon
76 grmon.commands = cd $$DESTDIR && C:/opt/grmon-eval-2.0.29b/win32/bin/grmon.exe -uart COM4 -u
77 QMAKE_EXTRA_TARGETS += grmon
78 }
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
Show file before | Show file after | Hide comments
@@ -0,0 +1,70
1 #include "grspw.h"
2
3 int grspw_set_ie( unsigned char value, unsigned int *ctrlReg )
4 {
5 // IE = bit 3
6 // Interrupt Enable
7
8 int ret = 0;
9
10 if (value == 0)
11 {
12 *ctrlReg = *ctrlReg & 0xfffffff7;
13 }
14 else if (value == 1)
15 {
16 *ctrlReg = *ctrlReg | 0x00000008;
17 }
18 else
19 {
20 ret = -1;
21 }
22
23 return ret;
24 }
25
26 int grspw_set_tq( unsigned char value, unsigned int *ctrlReg )
27 {
28 // TQ = bit 8
29 // Tick-out IRQ
30
31 int ret = 0;
32
33 if (value == 0)
34 {
35 *ctrlReg = *ctrlReg & 0xfffffeff;
36 }
37 else if (value == 1)
38 {
39 *ctrlReg = *ctrlReg | 0x00000100;
40 }
41 else
42 {
43 ret = -1;
44 }
45
46 return ret;
47 }
48
49 int grspw_set_tr( unsigned char value, unsigned int *ctrlReg )
50 {
51 // TR = bit 11
52 // Enable timecode reception
53
54 int ret = 0;
55
56 if (value == 0)
57 {
58 *ctrlReg = *ctrlReg & 0xfffff7ff;
59 }
60 else if (value == 1)
61 {
62 *ctrlReg = *ctrlReg | 0x00000800;
63 }
64 else
65 {
66 ret = -1;
67 }
68
69 return ret;
70 }
Show file before | Show file after | Hide comments
@@ -0,0 +1,81
1 #include <stdio.h>
2
3 #include "grspw.h"
4 #include "fsw_params.h"
5
6 #define DSU_TIME_TAG_COUNTER 0x90000008
7
8 //**********
9 // IRQ LINES
10 #define IRQ_GRSPW 11
11 #define IRQ_SPARC_GRSPW 0x1b // see sparcv8.pdf p.76 for interrupt levels
12
13 extern void *catch_interrupt(void func(), int irq);
14 int *lreg = (int *) 0x80000000;
15
16 #define ICLEAR 0x20c
17 #define IMASK 0x240
18 #define IFORCE 0x208
19
20 void enable_irq (int irq)
21 {
22 lreg[ICLEAR/4] = (1 << irq); // clear any pending irq
23 lreg[IMASK/4] |= (1 << irq); // unmaks irq
24 }
25
26 void disable_irq (int irq) { lreg[IMASK/4] &= ~(1 << irq); } // mask irq
27
28 void force_irq (int irq) { lreg[IFORCE/4] = (1 << irq); } // force irq
29
30 /* NOTE: NEVER put printf() or other stdio routines in interrupt handlers,
31 they are not re-entrant. This (bad) example is just a demo */
32
33 unsigned char processTimecode = 0;
34
35 void irqhandler(int irq)
36 {
37 processTimecode = 1;
38 }
39
40 int main( void )
41 {
42 unsigned int *grspwCtrlReg;
43 unsigned int k;
44 volatile unsigned int *reg;
45 float aux;
46 unsigned int counter = 0;
47
48 printf("hello world!\n");
49
50 grspwCtrlReg = (unsigned int*) REGS_ADDR_GRSPW;
51 grspw_set_ie( 1, grspwCtrlReg );
52 grspw_set_tq( 1, grspwCtrlReg );
53 grspw_set_tr( 1, grspwCtrlReg );
54
55 catch_interrupt(irqhandler, IRQ_GRSPW);
56 enable_irq( IRQ_GRSPW );
57 force_irq( IRQ_GRSPW );
58
59 reg = (volatile unsigned int *) DSU_TIME_TAG_COUNTER;
60
61 while(1)
62 {
63 if (processTimecode == 1)
64 {
65 counter ++;
66 printf("timecode counter = %d\n", counter);
67 processTimecode = 0;
68 }
69 else
70 {
71 printf(".");
72 }
73
74 // for (k=0; k<100000;k++)
75 // {
76 // aux = aux + *reg ;
77 // }
78 }
79
80 return 0;
81 }
Show file before | Show file after | Hide comments
@@ -0,0 +1,17
1 # LOAD FSW USING LINK 1
2 SpwPlugin0.StarDundeeSelectLinkNumber( 1 )
3
4 APB_UART_PLUGIN0.setFifoDebugEnabled( 0 )
5
6 dsu3plugin0.openFile("/opt/DEV_PLE/EQM/bin/eqm")
7 dsu3plugin0.loadFile()
8
9 dsu3plugin0.run()
10
11 APB_UART_PLUGIN0.setFifoDebugEnabled( 1 )
12
13 # START SENDING TIMECODES AT 1 Hz
14 SpwPlugin0.StarDundeeStartTimecodes( 1 )
15
16 # it is possible to change the time code frequency
17 #RMAPPlugin0.changeTimecodeFrequency(2)
Show file before | Show file after | Hide comments
@@ -0,0 +1,34
1 import time
2
3 proxy.loadSysDriver("SpwPlugin","SpwPlugin0")
4 SpwPlugin0.selectBridge("STAR-Dundee Spw USB Brick")
5
6 proxy.loadSysDriverToParent("dsu3plugin","SpwPlugin0")
7 proxy.loadSysDriverToParent("LFRControlPlugin","SpwPlugin0")
8
9 proxy.loadSysDriverToParent("APB_UART_PLUGIN","SpwPlugin0")
10 APB_UART_PLUGIN0.setFifoDebugEnabled( 1 )
11
12 availableBrickCount = SpwPlugin0.StarDundeeGetAvailableBrickCount()
13 print str(availableBrickCount) + " SpaceWire brick(s) found"
14
15 SpwPlugin0.StarDundeeSelectBrick(1)
16 SpwPlugin0.StarDundeeSetBrickAsARouter(1)
17 SpwPlugin0.StarDundeeSelectLinkNumber( 1 )
18 SpwPlugin0.connectBridge()
19
20 #SpwPlugin0.TCPServerSetIP("127.0.0.1")
21 SpwPlugin0.TCPServerConnect()
22
23 # OPEN SPACEWIRE SERVER
24 #LFRControlPlugin0.SetSpwServerIP(129,104,27,164)
25 LFRControlPlugin0.TCPServerConnect()
26
27 # OPEN TM ECHO BRIDGE SERVER
28 LFRControlPlugin0.TMEchoBridgeOpenPort()
29
30 # START SENDING TIMECODES AT 1 Hz
31 SpwPlugin0.StarDundeeStartTimecodes( 1 )
32
33 # it is possible to change the time code frequency
34 #RMAPPlugin0.changeTimecodeFrequency(2)
Show file before | Show file after | Hide comments
@@ -0,0 +1,34
1 #!/usr/bin/lppmon -e
2
3 import time
4
5 proxy.loadSysDriver("SpwPlugin","SpwPlugin0")
6 SpwPlugin0.selectBridge("STAR-Dundee Spw USB Brick")
7
8 proxy.loadSysDriverToParent("dsu3plugin","SpwPlugin0")
9 proxy.loadSysDriverToParent("LFRControlPlugin","SpwPlugin0")
10
11 proxy.loadSysDriverToParent("APB_UART_PLUGIN","SpwPlugin0")
12 APB_UART_PLUGIN0.setFifoDebugEnabled( 0 )
13
14 availableBrickCount = SpwPlugin0.StarDundeeGetAvailableBrickCount()
15 print "availableBrickCount = ", availableBrickCount
16
17 SpwPlugin0.StarDundeeSelectBrick(1)
18 SpwPlugin0.StarDundeeSetBrickAsARouter(1)
19 SpwPlugin0.connectBridge()
20
21 #SpwPlugin0.TCPServerSetIP("127.0.0.1")
22 SpwPlugin0.TCPServerConnect()
23
24 #LFRControlPlugin0.SetSpwServerIP(129,104,27,164)
25 LFRControlPlugin0.TCPServerConnect()
26
27 dsu3plugin0.openFile("/opt/DEV_PLE/EQM/bin/eqm")
28 dsu3plugin0.loadFile()
29 dsu3plugin0.run()
30
31 APB_UART_PLUGIN0.setFifoDebugEnabled( 1 )
32
33 LFRControlPlugin0.TMEchoBridgeOpenPort()
34
Show file before | Show file after | Hide comments
@@ -0,0 +1,13
1 # LOAD FSW USING LINK 1
2 SpwPlugin0.StarDundeeSelectLinkNumber( 1 )
3
4 dsu3plugin0.openFile("/opt/LFR/LFR-FSW/2.0.2.3/fsw")
5 dsu3plugin0.loadFile()
6
7 dsu3plugin0.run()
8
9 # START SENDING TIMECODES AT 1 Hz
10 SpwPlugin0.StarDundeeStartTimecodes( 1 )
11
12 # it is possible to change the time code frequency
13 #RMAPPlugin0.changeTimecodeFrequency(2)
Show file before | Show file after | Hide comments
@@ -0,0 +1,5
1 dsu3plugin0.openFile("/opt/DEV_PLE/FSW-qt/bin/fsw")
2 dsu3plugin0.loadFile()
3
4 dsu3plugin0.run()
5
Show file before | Show file after | Hide comments
@@ -0,0 +1,83
1 #from PyQt4 import QtGui
2 #from PyQt4 import QtCore
3 #from PyQt4 import Qt
4 import sys
5 def elfSize(FileName,section):
6 bashCommand = "/usr/bin/size "+ FileName
7 import subprocess
8 process = subprocess.Popen(bashCommand.split(), stdout=subprocess.PIPE)
9 result = process.communicate()[0].split("\n")
10 header = result[0].lstrip()
11 line1 = result[1].lstrip()
12 hcolumns=header.split()
13 columns=line1.split()
14 for i in range(0,len(hcolumns)):
15 if(hcolumns[i].find(section) != -1):
16 return int(columns[i])
17 return 0;
18
19
20 def elfAddress(FileName,section):
21 bashCommand = "readelf -S " + FileName
22 import subprocess
23 process = subprocess.Popen(bashCommand.split(), stdout=subprocess.PIPE)
24 result = process.communicate()[0].split()
25 for i in range(0,len(result)):
26 if(result[i].find('.'+section) != -1):
27 return int("0x"+result[i+2],16)
28 return 0;
29
30 def compare(data1,data2):
31 if len(data1)!=len(data2):
32 return [-1,-1]
33 for i in range(len(data1)):
34 if data1[i]!=data2[i]:
35 return [-1,i]
36 return [1,-1]
37
38 def cycles(rootPlugin,textAddress,textSize,orgiData,count):
39 for i in range(count):
40 rootPlugin.dumpMemory(textAddress,textSize/4,"/opt/dump"+str(i)+".srec","srec")
41 data = rootPlugin.Read(textAddress,textSize/4)
42 result = compare(data,orgiData)
43 if(result[0]==1):
44 print("test number "+str(i)+" = success")
45 else:
46 print("test number "+str(i)+" = error @0x" + hex(textAddress + result[1]))
47
48
49 #app = QtGui.QApplication(sys.argv)
50 fileName = QtGui.QFileDialog.getOpenFileName()
51
52 if PySocExplorer.ElfFile.isElf(fileName):
53 proxy.loadSysDriver("SpwPlugin","SpwPlugin0")
54 SpwPlugin0.selectBridge("STAR-Dundee Spw USB Brick")
55
56 proxy.loadSysDriverToParent("dsu3plugin","SpwPlugin0")
57 proxy.loadSysDriverToParent("LFRControlPlugin","SpwPlugin0")
58 SpwPlugin0.TCPServerConnect()
59 LFRControlPlugin0.TCPServerConnect()
60 proxy.loadSysDriverToParent("APB_UART_PLUGIN","SpwPlugin0")
61 availableBrickCount = SpwPlugin0.StarDundeeGetAvailableBrickCount()
62 print(str(availableBrickCount) + " SpaceWire brick(s) found")
63
64 SpwPlugin0.StarDundeeSelectBrick(1)
65 SpwPlugin0.StarDundeeSetBrickAsARouter(1)
66 SpwPlugin0.connectBridge()
67 APB_UART_PLUGIN0.setUARTPortNane("/dev/ttyUSB1")
68 APB_UART_PLUGIN0.setUARTPortSpeed(38400)
69 APB_UART_PLUGIN0.openUart()
70 textSize= elfSize(fileName,"text")
71 textAddress= elfAddress(fileName,"text")
72 print "Found text section@" + hex(textAddress)+ " of " + str(textSize) +" bytes"
73 print "loading software"
74 dsu3plugin0.openFile(fileName)
75 dsu3plugin0.loadFile()
76 SpwPlugin0.dumpMemory(textAddress,textSize/4,"/opt/dumpOrig.srec","srec")
77 dsu3plugin0.run()
78 orgiData = SpwPlugin0.Read(textAddress,textSize/4)
79 orgiData = SpwPlugin0.Read(textAddress,textSize/4)
80
81
82
83
Show file before | Show file after | Hide comments
@@ -1,50 +1,50
1 1 #ifndef FSW_INIT_H_INCLUDED
2 2 #define FSW_INIT_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <leon.h>
6 6
7 7 #include "fsw_params.h"
8 8 #include "fsw_misc.h"
9 9 #include "fsw_processing.h"
10 10
11 11 #include "tc_handler.h"
12 12 #include "wf_handler.h"
13 13 #include "fsw_spacewire.h"
14 14
15 15 #include "avf0_prc0.h"
16 16 #include "avf1_prc1.h"
17 17 #include "avf2_prc2.h"
18 18
19 #include "GscMemoryLPP.hpp"
20
21 19 extern rtems_name Task_name[20]; /* array of task names */
22 20 extern rtems_id Task_id[20]; /* array of task ids */
23 21
24 22 // RTEMS TASKS
25 23 rtems_task Init( rtems_task_argument argument);
26 24
27 25 // OTHER functions
28 26 void create_names( void );
29 27 int create_all_tasks( void );
30 28 int start_all_tasks( void );
31 29 //
32 30 rtems_status_code create_message_queues( void );
33 31 rtems_status_code get_message_queue_id_send( rtems_id *queue_id );
34 32 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id );
35 33 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id );
36 34 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
37 35 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
36 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max );
37 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize );
38 38 //
39 39 int start_recv_send_tasks( void );
40 40 //
41 41 void init_local_mode_parameters( void );
42 42 void reset_local_time( void );
43 43
44 44 extern void rtems_cpu_usage_report( void );
45 45 extern void rtems_cpu_usage_reset( void );
46 46 extern void rtems_stack_checker_report_usage( void );
47 47
48 48 extern int sched_yield( void );
49 49
50 50 #endif // FSW_INIT_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,48 +1,54
1 1 #ifndef FSW_MISC_H_INCLUDED
2 2 #define FSW_MISC_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <stdio.h>
6 6 #include <grspw.h>
7 7 #include <grlib_regs.h>
8 8
9 9 #include "fsw_params.h"
10 10 #include "fsw_spacewire.h"
11 11 #include "lfr_cpu_usage_report.h"
12 12
13 13 rtems_name name_hk_rate_monotonic; // name of the HK rate monotonic
14 14 rtems_id HK_id; // id of the HK rate monotonic period
15 15
16 16 void configure_timer(gptimer_regs_t *gptimer_regs, unsigned char timer, unsigned int clock_divider,
17 17 unsigned char interrupt_level, rtems_isr (*timer_isr)() );
18 18 void timer_start( gptimer_regs_t *gptimer_regs, unsigned char timer );
19 19 void timer_stop( gptimer_regs_t *gptimer_regs, unsigned char timer );
20 20 void timer_set_clock_divider(gptimer_regs_t *gptimer_regs, unsigned char timer, unsigned int clock_divider);
21 21
22 22 // SERIAL LINK
23 23 int send_console_outputs_on_apbuart_port( void );
24 24 int enable_apbuart_transmitter( void );
25 25 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value);
26 26
27 27 // RTEMS TASKS
28 28 rtems_task stat_task( rtems_task_argument argument );
29 29 rtems_task hous_task( rtems_task_argument argument );
30 30 rtems_task dumb_task( rtems_task_argument unused );
31 31
32 32 void init_housekeeping_parameters( void );
33 33 void increment_seq_counter(unsigned short *packetSequenceControl);
34 34 void getTime( unsigned char *time);
35 35 unsigned long long int getTimeAsUnsignedLongLongInt( );
36 36 void send_dumb_hk( void );
37 37 void get_temperatures( unsigned char *temperatures );
38 38 void get_v_e1_e2_f3( unsigned char *spacecraft_potential );
39 39 void get_cpu_load( unsigned char *resource_statistics );
40 40
41 41 extern int sched_yield( void );
42 42 extern void rtems_cpu_usage_reset();
43 43 extern ring_node *current_ring_node_f3;
44 44 extern ring_node *ring_node_to_send_cwf_f3;
45 45 extern ring_node waveform_ring_f3[];
46 46 extern unsigned short sequenceCounterHK;
47 47
48 extern unsigned char hk_lfr_q_sd_fifo_size_max;
49 extern unsigned char hk_lfr_q_rv_fifo_size_max;
50 extern unsigned char hk_lfr_q_p0_fifo_size_max;
51 extern unsigned char hk_lfr_q_p1_fifo_size_max;
52 extern unsigned char hk_lfr_q_p2_fifo_size_max;
53
48 54 #endif // FSW_MISC_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,50 +1,51
1 1 #ifndef FSW_SPACEWIRE_H_INCLUDED
2 2 #define FSW_SPACEWIRE_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <grspw.h>
6 6
7 7 #include <fcntl.h> // for O_RDWR
8 8 #include <unistd.h> // for the read call
9 9 #include <sys/ioctl.h> // for the ioctl call
10 10 #include <errno.h>
11 11
12 12 #include "fsw_params.h"
13 13 #include "tc_handler.h"
14 #include "fsw_init.h"
14 15
15 16 extern spw_stats spacewire_stats;
16 17 extern spw_stats spacewire_stats_backup;
17 18
18 19 // RTEMS TASK
19 20 rtems_task spiq_task( rtems_task_argument argument );
20 21 rtems_task recv_task( rtems_task_argument unused );
21 22 rtems_task send_task( rtems_task_argument argument );
22 23 rtems_task wtdg_task( rtems_task_argument argument );
23 24
24 25 int spacewire_open_link( void );
25 26 int spacewire_start_link( int fd );
26 27 int spacewire_stop_and_start_link( int fd );
27 28 int spacewire_configure_link(int fd );
28 29 int spacewire_reset_link( void );
29 30 void spacewire_set_NP( unsigned char val, unsigned int regAddr ); // No Port force
30 31 void spacewire_set_RE( unsigned char val, unsigned int regAddr ); // RMAP Enable
31 32 void spacewire_compute_stats_offsets( void );
32 33 void spacewire_update_statistics( void );
33 34
34 35 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header );
35 36 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header );
36 37 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header );
37 38 int spw_send_waveform_CWF( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_CWF_t *header );
38 39 int spw_send_waveform_SWF( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_SWF_t *header );
39 40 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_CWF_t *header );
40 41 void spw_send_asm_f0( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_ASM_t *header );
41 42 void spw_send_asm_f1( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_ASM_t *header );
42 43 void spw_send_asm_f2( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_ASM_t *header );
43 44 void spw_send_k_dump( ring_node *ring_node_to_send );
44 45
45 46 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc );
46 47 rtems_timer_service_routine user_routine( rtems_id timer_id, void *user_data );
47 48
48 49 void (*grspw_timecode_callback) ( void *pDev, void *regs, int minor, unsigned int tc );
49 50
50 51 #endif // FSW_SPACEWIRE_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,37 +1,38
1 1 #ifndef AVF0_PRC0_H_INCLUDED
2 2 #define AVF0_PRC0_H_INCLUDED
3 3
4 4 #include "fsw_processing.h"
5 5 #include "basic_parameters.h"
6 #include "fsw_init.h"
6 7
7 8 typedef struct {
8 9 unsigned int norm_bp1;
9 10 unsigned int norm_bp2;
10 11 unsigned int norm_asm;
11 12 unsigned int burst_sbm_bp1;
12 13 unsigned int burst_sbm_bp2;
13 14 unsigned int burst_bp1;
14 15 unsigned int burst_bp2;
15 16 unsigned int sbm1_bp1;
16 17 unsigned int sbm1_bp2;
17 18 unsigned int sbm2_bp1;
18 19 unsigned int sbm2_bp2;
19 20 } nb_sm_before_bp_asm_f0;
20 21
21 22 //************
22 23 // RTEMS TASKS
23 24 rtems_task avf0_task( rtems_task_argument lfrRequestedMode );
24 25 rtems_task prc0_task( rtems_task_argument lfrRequestedMode );
25 26
26 27 //**********
27 28 // FUNCTIONS
28 29
29 30 void reset_nb_sm_f0( unsigned char lfrMode );
30 31 void init_k_coefficients_f0( void );
31 32 void test_TCH( void );
32 33
33 34 //*******
34 35 // EXTERN
35 36 extern rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id );
36 37
37 38 #endif // AVF0_PRC0_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,34 +1,35
1 1 #ifndef AVF1_PRC1_H
2 2 #define AVF1_PRC1_H
3 3
4 4 #include "fsw_processing.h"
5 5 #include "basic_parameters.h"
6 #include "fsw_init.h"
6 7
7 8 typedef struct {
8 9 unsigned int norm_bp1;
9 10 unsigned int norm_bp2;
10 11 unsigned int norm_asm;
11 12 unsigned int burst_sbm_bp1;
12 13 unsigned int burst_sbm_bp2;
13 14 unsigned int burst_bp1;
14 15 unsigned int burst_bp2;
15 16 unsigned int sbm2_bp1;
16 17 unsigned int sbm2_bp2;
17 18 } nb_sm_before_bp_asm_f1;
18 19
19 20 //************
20 21 // RTEMS TASKS
21 22 rtems_task avf1_task( rtems_task_argument lfrRequestedMode );
22 23 rtems_task prc1_task( rtems_task_argument lfrRequestedMode );
23 24
24 25 //**********
25 26 // FUNCTIONS
26 27
27 28 void reset_nb_sm_f1( unsigned char lfrMode );
28 29 void init_k_coefficients_f1( void );
29 30
30 31 //*******
31 32 // EXTERN
32 33 extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
33 34
34 35 #endif // AVF1_PRC1_H
Show file before | Show file after | Hide comments
@@ -1,29 +1,30
1 1 #ifndef AVF2_PRC2_H
2 2 #define AVF2_PRC2_H
3 3
4 4 #include "fsw_processing.h"
5 5 #include "basic_parameters.h"
6 #include "fsw_init.h"
6 7
7 8 typedef struct {
8 9 unsigned int norm_bp1;
9 10 unsigned int norm_bp2;
10 11 unsigned int norm_asm;
11 12 } nb_sm_before_bp_asm_f2;
12 13
13 14 //************
14 15 // RTEMS TASKS
15 16 rtems_task avf2_task( rtems_task_argument lfrRequestedMode );
16 17 rtems_task prc2_task( rtems_task_argument lfrRequestedMode );
17 18
18 19 //**********
19 20 // FUNCTIONS
20 21
21 22 void reset_nb_sm_f2( void );
22 23 void SM_average_f2(float *averaged_spec_mat_f2, ring_node *ring_node, unsigned int nbAverageNormF2 , asm_msg *msgForMATR);
23 24 void init_k_coefficients_f2( void );
24 25
25 26 //*******
26 27 // EXTERN
27 28 extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
28 29
29 30 #endif // AVF2_PRC2_H
Show file before | Show file after | Hide comments
@@ -1,324 +1,323
1 1 #ifndef FSW_PROCESSING_H_INCLUDED
2 2 #define FSW_PROCESSING_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <grspw.h>
6 6 #include <math.h>
7 7 #include <stdlib.h> // abs() is in the stdlib
8 8 #include <stdio.h> // printf()
9 9 #include <math.h>
10 10 #include <grlib_regs.h>
11 11
12 12 #include "fsw_params.h"
13 #include "fsw_spacewire.h"
14 13
15 14 typedef struct ring_node_asm
16 15 {
17 16 struct ring_node_asm *next;
18 17 float matrix[ TOTAL_SIZE_SM ];
19 18 unsigned int status;
20 19 } ring_node_asm;
21 20
22 21 typedef struct
23 22 {
24 23 unsigned char targetLogicalAddress;
25 24 unsigned char protocolIdentifier;
26 25 unsigned char reserved;
27 26 unsigned char userApplication;
28 27 unsigned char packetID[2];
29 28 unsigned char packetSequenceControl[2];
30 29 unsigned char packetLength[2];
31 30 // DATA FIELD HEADER
32 31 unsigned char spare1_pusVersion_spare2;
33 32 unsigned char serviceType;
34 33 unsigned char serviceSubType;
35 34 unsigned char destinationID;
36 35 unsigned char time[6];
37 36 // AUXILIARY HEADER
38 37 unsigned char sid;
39 38 unsigned char biaStatusInfo;
40 39 unsigned char sy_lfr_common_parameters_spare;
41 40 unsigned char sy_lfr_common_parameters;
42 41 unsigned char acquisitionTime[6];
43 42 unsigned char pa_lfr_bp_blk_nr[2];
44 43 // SOURCE DATA
45 44 unsigned char data[ 780 ]; // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1]
46 45 } bp_packet;
47 46
48 47 typedef struct
49 48 {
50 49 unsigned char targetLogicalAddress;
51 50 unsigned char protocolIdentifier;
52 51 unsigned char reserved;
53 52 unsigned char userApplication;
54 53 unsigned char packetID[2];
55 54 unsigned char packetSequenceControl[2];
56 55 unsigned char packetLength[2];
57 56 // DATA FIELD HEADER
58 57 unsigned char spare1_pusVersion_spare2;
59 58 unsigned char serviceType;
60 59 unsigned char serviceSubType;
61 60 unsigned char destinationID;
62 61 unsigned char time[6];
63 62 // AUXILIARY HEADER
64 63 unsigned char sid;
65 64 unsigned char biaStatusInfo;
66 65 unsigned char sy_lfr_common_parameters_spare;
67 66 unsigned char sy_lfr_common_parameters;
68 67 unsigned char acquisitionTime[6];
69 68 unsigned char source_data_spare;
70 69 unsigned char pa_lfr_bp_blk_nr[2];
71 70 // SOURCE DATA
72 71 unsigned char data[ 117 ]; // 13 bins * 9 Bytes only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1
73 72 } bp_packet_with_spare;
74 73
75 typedef struct
74 typedef struct asm_msg
76 75 {
77 76 ring_node_asm *norm;
78 77 ring_node_asm *burst_sbm;
79 78 rtems_event_set event;
80 79 unsigned int coarseTimeNORM;
81 80 unsigned int fineTimeNORM;
82 81 unsigned int coarseTimeSBM;
83 82 unsigned int fineTimeSBM;
84 83 } asm_msg;
85 84
86 85 extern volatile int sm_f0[ ];
87 86 extern volatile int sm_f1[ ];
88 87 extern volatile int sm_f2[ ];
89 88
90 89 // parameters
91 90 extern struct param_local_str param_local;
92 91 extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
93 92
94 93 // registers
95 94 extern time_management_regs_t *time_management_regs;
96 95 extern volatile spectral_matrix_regs_t *spectral_matrix_regs;
97 96
98 97 extern rtems_name misc_name[5];
99 98 extern rtems_id Task_id[20]; /* array of task ids */
100 99
101 100 //
102 101 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel);
103 102 // ISR
104 103 rtems_isr spectral_matrices_isr( rtems_vector_number vector );
105 104 rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector );
106 105
107 106 //******************
108 107 // Spectral Matrices
109 108 void reset_nb_sm( void );
110 109 // SM
111 110 void SM_init_rings( void );
112 111 void SM_reset_current_ring_nodes( void );
113 112 // ASM
114 113 void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes );
115 114
116 115 //*****************
117 116 // Basic Parameters
118 117
119 118 void BP_reset_current_ring_nodes( void );
120 119 void BP_init_header(bp_packet *packet,
121 120 unsigned int apid, unsigned char sid,
122 121 unsigned int packetLength , unsigned char blkNr);
123 122 void BP_init_header_with_spare(bp_packet_with_spare *packet,
124 123 unsigned int apid, unsigned char sid,
125 124 unsigned int packetLength, unsigned char blkNr );
126 125 void BP_send( char *data,
127 126 rtems_id queue_id ,
128 127 unsigned int nbBytesToSend , unsigned int sid );
129 128
130 129 //******************
131 130 // general functions
132 131 void reset_sm_status( void );
133 132 void reset_spectral_matrix_regs( void );
134 133 void set_time(unsigned char *time, unsigned char *timeInBuffer );
135 134 unsigned long long int get_acquisition_time( unsigned char *timePtr );
136 135 unsigned char getSID( rtems_event_set event );
137 136
138 137 extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
139 138 extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
140 139
141 140 //***************************************
142 141 // DEFINITIONS OF STATIC INLINE FUNCTIONS
143 142 static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
144 143 ring_node *ring_node_tab[],
145 144 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
146 145 asm_msg *msgForMATR );
147 146
148 147 static inline void SM_average_debug(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
149 148 ring_node *ring_node_tab[],
150 149 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
151 150 asm_msg *msgForMATR );
152 151
153 152 void ASM_patch( float *inputASM, float *outputASM );
154 153
155 154 void extractReImVectors(float *inputASM, float *outputASM, unsigned int asmComponent );
156 155
157 156 static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized,
158 157 float divider );
159 158
160 159 static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat,
161 160 float divider,
162 161 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
163 162
164 163 static inline void ASM_convert(volatile float *input_matrix, char *output_matrix);
165 164
166 165 void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
167 166 ring_node *ring_node_tab[],
168 167 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
169 168 asm_msg *msgForMATR )
170 169 {
171 170 float sum;
172 171 unsigned int i;
173 172
174 173 for(i=0; i<TOTAL_SIZE_SM; i++)
175 174 {
176 175 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ]
177 176 + ( (int *) (ring_node_tab[1]->buffer_address) ) [ i ]
178 177 + ( (int *) (ring_node_tab[2]->buffer_address) ) [ i ]
179 178 + ( (int *) (ring_node_tab[3]->buffer_address) ) [ i ]
180 179 + ( (int *) (ring_node_tab[4]->buffer_address) ) [ i ]
181 180 + ( (int *) (ring_node_tab[5]->buffer_address) ) [ i ]
182 181 + ( (int *) (ring_node_tab[6]->buffer_address) ) [ i ]
183 182 + ( (int *) (ring_node_tab[7]->buffer_address) ) [ i ];
184 183
185 184 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
186 185 {
187 186 averaged_spec_mat_NORM[ i ] = sum;
188 187 averaged_spec_mat_SBM[ i ] = sum;
189 188 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
190 189 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
191 190 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
192 191 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
193 192 }
194 193 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
195 194 {
196 195 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
197 196 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
198 197 }
199 198 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
200 199 {
201 200 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
202 201 averaged_spec_mat_SBM[ i ] = sum;
203 202 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
204 203 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
205 204 }
206 205 else
207 206 {
208 207 PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
209 208 }
210 209 }
211 210 }
212 211
213 212 void SM_average_debug( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
214 213 ring_node *ring_node_tab[],
215 214 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
216 215 asm_msg *msgForMATR )
217 216 {
218 217 float sum;
219 218 unsigned int i;
220 219
221 220 for(i=0; i<TOTAL_SIZE_SM; i++)
222 221 {
223 222 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ];
224 223 averaged_spec_mat_NORM[ i ] = sum;
225 224 averaged_spec_mat_SBM[ i ] = sum;
226 225 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
227 226 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
228 227 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
229 228 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
230 229 }
231 230 }
232 231
233 232 void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
234 233 {
235 234 int frequencyBin;
236 235 int asmComponent;
237 236 unsigned int offsetASM;
238 237 unsigned int offsetASMReorganized;
239 238
240 239 // BUILD DATA
241 240 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
242 241 {
243 242 for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
244 243 {
245 244 offsetASMReorganized =
246 245 frequencyBin * NB_VALUES_PER_SM
247 246 + asmComponent;
248 247 offsetASM =
249 248 asmComponent * NB_BINS_PER_SM
250 249 + frequencyBin;
251 250 averaged_spec_mat_reorganized[offsetASMReorganized ] =
252 251 averaged_spec_mat[ offsetASM ] / divider;
253 252 }
254 253 }
255 254 }
256 255
257 256 void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
258 257 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
259 258 {
260 259 int frequencyBin;
261 260 int asmComponent;
262 261 int offsetASM;
263 262 int offsetCompressed;
264 263 int k;
265 264
266 265 // BUILD DATA
267 266 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
268 267 {
269 268 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
270 269 {
271 270 offsetCompressed = // NO TIME OFFSET
272 271 frequencyBin * NB_VALUES_PER_SM
273 272 + asmComponent;
274 273 offsetASM = // NO TIME OFFSET
275 274 asmComponent * NB_BINS_PER_SM
276 275 + ASMIndexStart
277 276 + frequencyBin * nbBinsToAverage;
278 277 compressed_spec_mat[ offsetCompressed ] = 0;
279 278 for ( k = 0; k < nbBinsToAverage; k++ )
280 279 {
281 280 compressed_spec_mat[offsetCompressed ] =
282 281 ( compressed_spec_mat[ offsetCompressed ]
283 282 + averaged_spec_mat[ offsetASM + k ] );
284 283 }
285 284 compressed_spec_mat[ offsetCompressed ] =
286 285 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
287 286 }
288 287 }
289 288 }
290 289
291 290 void ASM_convert( volatile float *input_matrix, char *output_matrix)
292 291 {
293 292 unsigned int frequencyBin;
294 293 unsigned int asmComponent;
295 294 char * pt_char_input;
296 295 char * pt_char_output;
297 296 unsigned int offsetInput;
298 297 unsigned int offsetOutput;
299 298
300 299 pt_char_input = (char*) &input_matrix;
301 300 pt_char_output = (char*) &output_matrix;
302 301
303 302 // convert all other data
304 303 for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++)
305 304 {
306 305 for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++)
307 306 {
308 307 offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ;
309 308 offsetOutput = 2 * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ;
310 309 pt_char_input = (char*) &input_matrix [ offsetInput ];
311 310 pt_char_output = (char*) &output_matrix[ offsetOutput ];
312 311 pt_char_output[0] = pt_char_input[0]; // bits 31 downto 24 of the float
313 312 pt_char_output[1] = pt_char_input[1]; // bits 23 downto 16 of the float
314 313 }
315 314 }
316 315 }
317 316
318 317 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat,
319 318 float divider,
320 319 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
321 320
322 321 int getFBinMask(int k);
323 322
324 323 #endif // FSW_PROCESSING_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,75 +1,72
1 1 #ifndef TC_HANDLER_H_INCLUDED
2 2 #define TC_HANDLER_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <leon.h>
6 6
7 7 #include "tc_load_dump_parameters.h"
8 8 #include "tc_acceptance.h"
9 9 #include "tm_lfr_tc_exe.h"
10 10 #include "wf_handler.h"
11 11 #include "fsw_processing.h"
12 12
13 13 #include "lfr_cpu_usage_report.h"
14 14
15 // MODE PARAMETERS
16 extern unsigned int maxCount;
17
18 15 //****
19 16 // ISR
20 17 rtems_isr commutation_isr1( rtems_vector_number vector );
21 18 rtems_isr commutation_isr2( rtems_vector_number vector );
22 19
23 20 //***********
24 21 // RTEMS TASK
25 22 rtems_task actn_task( rtems_task_argument unused );
26 23
27 24 //***********
28 25 // TC ACTIONS
29 26 int action_reset( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time );
30 27 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id);
31 28 int action_update_info( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
32 29 int action_enable_calibration( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time );
33 30 int action_disable_calibration( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time );
34 31 int action_update_time( ccsdsTelecommandPacket_t *TC);
35 32
36 33 // mode transition
37 34 int check_mode_value( unsigned char requestedMode );
38 35 int check_mode_transition( unsigned char requestedMode );
39 36 int check_transition_date( unsigned int transitionCoarseTime );
40 37 int stop_current_mode( void );
41 38 int enter_mode( unsigned char mode , unsigned int transitionCoarseTime );
42 39 int restart_science_tasks( unsigned char lfrRequestedMode );
43 40 int suspend_science_tasks();
44 41 void launch_waveform_picker( unsigned char mode , unsigned int transitionCoarseTime );
45 42 void launch_spectral_matrix( void );
46 43 void launch_spectral_matrix_simu( void );
47 44 void set_sm_irq_onNewMatrix( unsigned char value );
48 45 void set_sm_irq_onError( unsigned char value );
49 46
50 47 // other functions
51 48 void updateLFRCurrentMode();
52 49 void set_lfr_soft_reset( unsigned char value );
53 50 void reset_lfr( void );
54 51 // CALIBRATION
55 52 void setCalibrationPrescaler( unsigned int prescaler );
56 53 void setCalibrationDivisor( unsigned int divisionFactor );
57 54 void setCalibrationData( void );
58 55 void setCalibrationReload( bool state);
59 56 void setCalibrationEnable( bool state );
60 57 void setCalibrationInterleaved( bool state );
61 58 void startCalibration( void );
62 59 void stopCalibration( void );
63 60 void configureCalibration( bool interleaved );
64 61 //
65 62 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC , unsigned char *time );
66 63 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC , unsigned char *time );
67 64 void close_action( ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id );
68 65
69 66 extern rtems_status_code get_message_queue_id_send( rtems_id *queue_id );
70 67 extern rtems_status_code get_message_queue_id_recv( rtems_id *queue_id );
71 68
72 69 #endif // TC_HANDLER_H_INCLUDED
73 70
74 71
75 72
Show file before | Show file after | Hide comments
@@ -1,87 +1,86
1 1 #ifndef WF_HANDLER_H_INCLUDED
2 2 #define WF_HANDLER_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <grspw.h>
6 6 #include <stdio.h>
7 7 #include <math.h>
8 8 #include <fsw_params.h>
9 9
10 #include "fsw_spacewire.h"
11 #include "fsw_misc.h"
10 #include "fsw_init.h"
12 11 #include "fsw_params_wf_handler.h"
13 12
14 13 #define pi 3.14159265359
15 14
16 15 extern int fdSPW;
17 16
18 17 //*****************
19 18 // waveform buffers
20 19 extern volatile int wf_buffer_f0[ ];
21 20 extern volatile int wf_buffer_f1[ ];
22 21 extern volatile int wf_buffer_f2[ ];
23 22 extern volatile int wf_buffer_f3[ ];
24 23
25 24 extern waveform_picker_regs_0_1_18_t *waveform_picker_regs;
26 25 extern time_management_regs_t *time_management_regs;
27 26 extern Packet_TM_LFR_HK_t housekeeping_packet;
28 27 extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
29 28 extern struct param_local_str param_local;
30 29
31 30 extern unsigned short sequenceCounters_SCIENCE_NORMAL_BURST;
32 31 extern unsigned short sequenceCounters_SCIENCE_SBM1_SBM2;
33 32
34 33 extern rtems_id Task_id[20]; /* array of task ids */
35 34
36 35 extern unsigned char lfrCurrentMode;
37 36
38 37 //**********
39 38 // RTEMS_ISR
40 39 void reset_extractSWF( void );
41 40 rtems_isr waveforms_isr( rtems_vector_number vector );
42 41
43 42 //***********
44 43 // RTEMS_TASK
45 44 rtems_task wfrm_task( rtems_task_argument argument );
46 45 rtems_task cwf3_task( rtems_task_argument argument );
47 46 rtems_task cwf2_task( rtems_task_argument argument );
48 47 rtems_task cwf1_task( rtems_task_argument argument );
49 48 rtems_task swbd_task( rtems_task_argument argument );
50 49
51 50 //******************
52 51 // general functions
53 52 void WFP_init_rings( void );
54 53 void init_ring( ring_node ring[], unsigned char nbNodes, volatile int buffer[] , unsigned int bufferSize );
55 54 void WFP_reset_current_ring_nodes( void );
56 55 //
57 56 int init_header_continuous_cwf3_light_table( Header_TM_LFR_SCIENCE_CWF_t *headerCWF );
58 57 //
59 58 int send_waveform_CWF3_light(ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id );
60 59 //
61 60 void compute_acquisition_time(unsigned int coarseTime, unsigned int fineTime,
62 61 unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char *acquisitionTime );
63 62 void build_snapshot_from_ring(ring_node *ring_node_to_send, unsigned char frequencyChannel , unsigned long long acquisitionTimeF0_asLong);
64 63 void snapshot_resynchronization( unsigned char *timePtr );
65 64 //
66 65 rtems_id get_pkts_queue_id( void );
67 66
68 67 //**************
69 68 // wfp registers
70 69 // RESET
71 70 void reset_wfp_burst_enable( void );
72 71 void reset_wfp_status( void );
73 72 void reset_wfp_buffer_addresses( void );
74 73 void reset_waveform_picker_regs( void );
75 74 // SET
76 75 void set_wfp_data_shaping(void);
77 76 void set_wfp_burst_enable_register( unsigned char mode );
78 77 void set_wfp_delta_snapshot( void );
79 78 void set_wfp_delta_f0_f0_2( void );
80 79 void set_wfp_delta_f1( void );
81 80 void set_wfp_delta_f2( void );
82 81
83 82 //*****************
84 83 // local parameters
85 84 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid );
86 85
87 86 #endif // WF_HANDLER_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,31 +1,31
1 1 import time
2 2
3 3 proxy.loadSysDriver("SpwPlugin","SpwPlugin0")
4 4 SpwPlugin0.selectBridge("STAR-Dundee Spw USB Brick")
5 5
6 6 proxy.loadSysDriverToParent("dsu3plugin","SpwPlugin0")
7 7 proxy.loadSysDriverToParent("LFRControlPlugin","SpwPlugin0")
8 8
9 9 availableBrickCount = SpwPlugin0.StarDundeeGetAvailableBrickCount()
10 10 print str(availableBrickCount) + " SpaceWire brick(s) found"
11 11
12 12 SpwPlugin0.StarDundeeSelectBrick(1)
13 13 SpwPlugin0.StarDundeeSetBrickAsARouter(1)
14 SpwPlugin0.StarDundeeSelectLinkNumber( 2 )
14 SpwPlugin0.StarDundeeSelectLinkNumber( 1 )
15 15 SpwPlugin0.connectBridge()
16 16
17 17 #SpwPlugin0.TCPServerSetIP("127.0.0.1")
18 18 SpwPlugin0.TCPServerConnect()
19 19
20 20 # OPEN SPACEWIRE SERVER
21 21 #LFRControlPlugin0.SetSpwServerIP(129,104,27,164)
22 22 LFRControlPlugin0.TCPServerConnect()
23 23
24 24 # OPEN TM ECHO BRIDGE SERVER
25 25 LFRControlPlugin0.TMEchoBridgeOpenPort()
26 26
27 27 # START SENDING TIMECODES AT 1 Hz
28 28 SpwPlugin0.StarDundeeStartTimecodes( 1 )
29 29
30 30 # it is possible to change the time code frequency
31 31 #RMAPPlugin0.changeTimecodeFrequency(2)
Show file before | Show file after | Hide comments
@@ -1,73 +1,78
1 1 /** Global variables of the LFR flight software.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * Among global variables, there are:
7 7 * - RTEMS names and id.
8 8 * - APB configuration registers.
9 9 * - waveforms global buffers, used by the waveform picker hardware module to store data.
10 10 * - spectral matrices buffesr, used by the hardware module to store data.
11 11 * - variable related to LFR modes parameters.
12 12 * - the global HK packet buffer.
13 13 * - the global dump parameter buffer.
14 14 *
15 15 */
16 16
17 17 #include <rtems.h>
18 18 #include <grspw.h>
19 19
20 20 #include "ccsds_types.h"
21 21 #include "grlib_regs.h"
22 22 #include "fsw_params.h"
23 23 #include "fsw_params_wf_handler.h"
24 24
25 25 // RTEMS GLOBAL VARIABLES
26 26 rtems_name misc_name[5];
27 27 rtems_name Task_name[20]; /* array of task names */
28 28 rtems_id Task_id[20]; /* array of task ids */
29 unsigned int maxCount;
30 29 int fdSPW = 0;
31 30 int fdUART = 0;
32 31 unsigned char lfrCurrentMode;
33 32
34 33 // WAVEFORMS GLOBAL VARIABLES // 2048 * 3 * 4 + 2 * 4 = 24576 + 8 bytes = 24584
35 34 // 97 * 256 = 24832 => delta = 248 bytes = 62 words
36 35 // WAVEFORMS GLOBAL VARIABLES // 2688 * 3 * 4 + 2 * 4 = 32256 + 8 bytes = 32264
37 36 // 127 * 256 = 32512 => delta = 248 bytes = 62 words
38 37 // F0 F1 F2 F3
39 38 volatile int wf_buffer_f0[ NB_RING_NODES_F0 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
40 39 volatile int wf_buffer_f1[ NB_RING_NODES_F1 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
41 40 volatile int wf_buffer_f2[ NB_RING_NODES_F2 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
42 41 volatile int wf_buffer_f3[ NB_RING_NODES_F3 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
43 42
44 43 //***********************************
45 44 // SPECTRAL MATRICES GLOBAL VARIABLES
46 45
47 46 // alignment constraints for the spectral matrices buffers => the first data after the time (8 bytes) shall be aligned on 0x00
48 47 volatile int sm_f0[ NB_RING_NODES_SM_F0 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
49 48 volatile int sm_f1[ NB_RING_NODES_SM_F1 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
50 49 volatile int sm_f2[ NB_RING_NODES_SM_F2 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
51 50
52 51 // APB CONFIGURATION REGISTERS
53 52 time_management_regs_t *time_management_regs = (time_management_regs_t*) REGS_ADDR_TIME_MANAGEMENT;
54 53 gptimer_regs_t *gptimer_regs = (gptimer_regs_t *) REGS_ADDR_GPTIMER;
55 54 waveform_picker_regs_0_1_18_t *waveform_picker_regs = (waveform_picker_regs_0_1_18_t*) REGS_ADDR_WAVEFORM_PICKER;
56 55 spectral_matrix_regs_t *spectral_matrix_regs = (spectral_matrix_regs_t*) REGS_ADDR_SPECTRAL_MATRIX;
57 56
58 57 // MODE PARAMETERS
59 58 Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
60 59 struct param_local_str param_local;
61 60
62 61 // HK PACKETS
63 62 Packet_TM_LFR_HK_t housekeeping_packet;
63 // message queues occupancy
64 unsigned char hk_lfr_q_sd_fifo_size_max;
65 unsigned char hk_lfr_q_rv_fifo_size_max;
66 unsigned char hk_lfr_q_p0_fifo_size_max;
67 unsigned char hk_lfr_q_p1_fifo_size_max;
68 unsigned char hk_lfr_q_p2_fifo_size_max;
64 69 // sequence counters are incremented by APID (PID + CAT) and destination ID
65 70 unsigned short sequenceCounters_SCIENCE_NORMAL_BURST;
66 71 unsigned short sequenceCounters_SCIENCE_SBM1_SBM2;
67 72 unsigned short sequenceCounters_TC_EXE[SEQ_CNT_NB_DEST_ID];
68 73 unsigned short sequenceCounterHK;
69 74 unsigned short sequenceCounterParameterDump;
70 75 spw_stats spacewire_stats;
71 76 spw_stats spacewire_stats_backup;
72 77
73 78
Show file before | Show file after | Hide comments
@@ -1,814 +1,869
1 1 /** This is the RTEMS initialization module.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * This module contains two very different information:
7 7 * - specific instructions to configure the compilation of the RTEMS executive
8 8 * - functions related to the fligth softwre initialization, especially the INIT RTEMS task
9 9 *
10 10 */
11 11
12 12 //*************************
13 13 // GPL reminder to be added
14 14 //*************************
15 15
16 16 #include <rtems.h>
17 17
18 18 /* configuration information */
19 19
20 20 #define CONFIGURE_INIT
21 21
22 22 #include <bsp.h> /* for device driver prototypes */
23 23
24 24 /* configuration information */
25 25
26 26 #define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
27 27 #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
28 28
29 29 #define CONFIGURE_MAXIMUM_TASKS 20
30 30 #define CONFIGURE_RTEMS_INIT_TASKS_TABLE
31 31 #define CONFIGURE_EXTRA_TASK_STACKS (3 * RTEMS_MINIMUM_STACK_SIZE)
32 32 #define CONFIGURE_LIBIO_MAXIMUM_FILE_DESCRIPTORS 32
33 33 #define CONFIGURE_INIT_TASK_PRIORITY 1 // instead of 100
34 34 #define CONFIGURE_INIT_TASK_MODE (RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT)
35 35 #define CONFIGURE_INIT_TASK_ATTRIBUTES (RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT)
36 36 #define CONFIGURE_MAXIMUM_DRIVERS 16
37 37 #define CONFIGURE_MAXIMUM_PERIODS 5
38 38 #define CONFIGURE_MAXIMUM_TIMERS 5 // STAT (1s), send SWF (0.3s), send CWF3 (1s)
39 39 #define CONFIGURE_MAXIMUM_MESSAGE_QUEUES 5
40 40 #ifdef PRINT_STACK_REPORT
41 41 #define CONFIGURE_STACK_CHECKER_ENABLED
42 42 #endif
43 43
44 44 #include <rtems/confdefs.h>
45 45
46 46 /* If --drvmgr was enabled during the configuration of the RTEMS kernel */
47 47 #ifdef RTEMS_DRVMGR_STARTUP
48 48 #ifdef LEON3
49 49 /* Add Timer and UART Driver */
50 50 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
51 51 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
52 52 #endif
53 53 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
54 54 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
55 55 #endif
56 56 #endif
57 57 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
58 58 #include <drvmgr/drvmgr_confdefs.h>
59 59 #endif
60 60
61 61 #include "fsw_init.h"
62 62 #include "fsw_config.c"
63 #include "GscMemoryLPP.hpp"
63 64
64 65 void initCache()
65 66 {
66 67 unsigned int cacheControlRegister;
67 68
68 69 cacheControlRegister = getCacheControlRegister();
69 70 printf("(0) cacheControlRegister = %x\n", cacheControlRegister);
70 71
71 72 resetCacheControlRegister();
72 73
73 74 enableInstructionCache();
74 75 enableDataCache();
75 76 enableInstructionBurstFetch();
76 77
77 78 cacheControlRegister = getCacheControlRegister();
78 79 printf("(1) cacheControlRegister = %x\n", cacheControlRegister);
79 80 }
80 81
81 82 rtems_task Init( rtems_task_argument ignored )
82 83 {
83 84 /** This is the RTEMS INIT taks, it is the first task launched by the system.
84 85 *
85 86 * @param unused is the starting argument of the RTEMS task
86 87 *
87 88 * The INIT task create and run all other RTEMS tasks.
88 89 *
89 90 */
90 91
91 92 //***********
92 93 // INIT CACHE
93 94
94 95 unsigned char *vhdlVersion;
95 96
96 97 reset_lfr();
97 98
98 99 reset_local_time();
99 100
100 101 rtems_cpu_usage_reset();
101 102
102 103 rtems_status_code status;
103 104 rtems_status_code status_spw;
104 105 rtems_isr_entry old_isr_handler;
105 106
106 107 // UART settings
107 108 send_console_outputs_on_apbuart_port();
108 109 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
109 110 enable_apbuart_transmitter();
110 111
111 112 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
112 113
113 114
114 115 PRINTF("\n\n\n\n\n")
115 116
116 117 initCache();
117 118
118 119 PRINTF("*************************\n")
119 120 PRINTF("** LFR Flight Software **\n")
120 121 PRINTF1("** %d.", SW_VERSION_N1)
121 122 PRINTF1("%d." , SW_VERSION_N2)
122 123 PRINTF1("%d." , SW_VERSION_N3)
123 124 PRINTF1("%d **\n", SW_VERSION_N4)
124 125
125 126 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
126 127 PRINTF("** VHDL **\n")
127 128 PRINTF1("** %d.", vhdlVersion[1])
128 129 PRINTF1("%d." , vhdlVersion[2])
129 130 PRINTF1("%d **\n", vhdlVersion[3])
130 131 PRINTF("*************************\n")
131 132 PRINTF("\n\n")
132 133
133 134 init_parameter_dump();
134 135 init_kcoefficients_dump();
135 136 init_local_mode_parameters();
136 137 init_housekeeping_parameters();
137 138 init_k_coefficients_f0();
138 139 init_k_coefficients_f1();
139 140 init_k_coefficients_f2();
140 141
141 142 // waveform picker initialization
142 143 WFP_init_rings(); // initialize the waveform rings
143 144 WFP_reset_current_ring_nodes();
144 145 reset_waveform_picker_regs();
145 146
146 147 // spectral matrices initialization
147 148 SM_init_rings(); // initialize spectral matrices rings
148 149 SM_reset_current_ring_nodes();
149 150 reset_spectral_matrix_regs();
150 151
151 152 // configure calibration
152 153 configureCalibration( false ); // true means interleaved mode, false is for normal mode
153 154
154 155 updateLFRCurrentMode();
155 156
156 157 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
157 158
158 159 create_names(); // create all names
159 160
160 161 status = create_message_queues(); // create message queues
161 162 if (status != RTEMS_SUCCESSFUL)
162 163 {
163 164 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
164 165 }
165 166
166 167 status = create_all_tasks(); // create all tasks
167 168 if (status != RTEMS_SUCCESSFUL)
168 169 {
169 170 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
170 171 }
171 172
172 173 // **************************
173 174 // <SPACEWIRE INITIALIZATION>
174 175 grspw_timecode_callback = &timecode_irq_handler;
175 176
176 177 status_spw = spacewire_open_link(); // (1) open the link
177 178 if ( status_spw != RTEMS_SUCCESSFUL )
178 179 {
179 180 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
180 181 }
181 182
182 183 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
183 184 {
184 185 status_spw = spacewire_configure_link( fdSPW );
185 186 if ( status_spw != RTEMS_SUCCESSFUL )
186 187 {
187 188 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
188 189 }
189 190 }
190 191
191 192 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
192 193 {
193 194 status_spw = spacewire_start_link( fdSPW );
194 195 if ( status_spw != RTEMS_SUCCESSFUL )
195 196 {
196 197 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
197 198 }
198 199 }
199 200 // </SPACEWIRE INITIALIZATION>
200 201 // ***************************
201 202
202 203 status = start_all_tasks(); // start all tasks
203 204 if (status != RTEMS_SUCCESSFUL)
204 205 {
205 206 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
206 207 }
207 208
208 209 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
209 210 status = start_recv_send_tasks();
210 211 if ( status != RTEMS_SUCCESSFUL )
211 212 {
212 213 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
213 214 }
214 215
215 216 // suspend science tasks, they will be restarted later depending on the mode
216 217 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
217 218 if (status != RTEMS_SUCCESSFUL)
218 219 {
219 220 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
220 221 }
221 222
222 223 //******************************
223 224 // <SPECTRAL MATRICES SIMULATOR>
224 225 LEON_Mask_interrupt( IRQ_SM_SIMULATOR );
225 226 configure_timer((gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR, CLKDIV_SM_SIMULATOR,
226 227 IRQ_SPARC_SM_SIMULATOR, spectral_matrices_isr_simu );
227 228 // </SPECTRAL MATRICES SIMULATOR>
228 229 //*******************************
229 230
230 231 // configure IRQ handling for the waveform picker unit
231 232 status = rtems_interrupt_catch( waveforms_isr,
232 233 IRQ_SPARC_WAVEFORM_PICKER,
233 234 &old_isr_handler) ;
234 235 // configure IRQ handling for the spectral matrices unit
235 236 status = rtems_interrupt_catch( spectral_matrices_isr,
236 237 IRQ_SPARC_SPECTRAL_MATRIX,
237 238 &old_isr_handler) ;
238 239
239 240 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
240 241 if ( status_spw != RTEMS_SUCCESSFUL )
241 242 {
242 243 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
243 244 if ( status != RTEMS_SUCCESSFUL ) {
244 245 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
245 246 }
246 247 }
247 248
248 249 BOOT_PRINTF("delete INIT\n")
249 250
250 251 status = rtems_task_delete(RTEMS_SELF);
251 252
252 253 }
253 254
254 255 void init_local_mode_parameters( void )
255 256 {
256 257 /** This function initialize the param_local global variable with default values.
257 258 *
258 259 */
259 260
260 261 unsigned int i;
261 262
262 263 // LOCAL PARAMETERS
263 264
264 265 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
265 266 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
266 267 BOOT_PRINTF1("nb_interrupt_f0_MAX = %d\n", param_local.local_nb_interrupt_f0_MAX)
267 268
268 269 // init sequence counters
269 270
270 271 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
271 272 {
272 273 sequenceCounters_TC_EXE[i] = 0x00;
273 274 }
274 275 sequenceCounters_SCIENCE_NORMAL_BURST = 0x00;
275 276 sequenceCounters_SCIENCE_SBM1_SBM2 = 0x00;
276 277 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
277 278 sequenceCounterParameterDump = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
278 279 }
279 280
280 281 void reset_local_time( void )
281 282 {
282 283 time_management_regs->ctrl = time_management_regs->ctrl | 0x02; // [0010] software reset, coarse time = 0x80000000
283 284 }
284 285
285 286 void create_names( void ) // create all names for tasks and queues
286 287 {
287 288 /** This function creates all RTEMS names used in the software for tasks and queues.
288 289 *
289 290 * @return RTEMS directive status codes:
290 291 * - RTEMS_SUCCESSFUL - successful completion
291 292 *
292 293 */
293 294
294 295 // task names
295 296 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
296 297 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
297 298 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
298 299 Task_name[TASKID_STAT] = rtems_build_name( 'S', 'T', 'A', 'T' );
299 300 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
300 301 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
301 302 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
302 303 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
303 304 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
304 305 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
305 306 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
306 307 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
307 308 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
308 309 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
309 310 Task_name[TASKID_WTDG] = rtems_build_name( 'W', 'T', 'D', 'G' );
310 311 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
311 312 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
312 313 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
313 314 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
314 315
315 316 // rate monotonic period names
316 317 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
317 318
318 319 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
319 320 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
320 321 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
321 322 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
322 323 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
323 324 }
324 325
325 326 int create_all_tasks( void ) // create all tasks which run in the software
326 327 {
327 328 /** This function creates all RTEMS tasks used in the software.
328 329 *
329 330 * @return RTEMS directive status codes:
330 331 * - RTEMS_SUCCESSFUL - task created successfully
331 332 * - RTEMS_INVALID_ADDRESS - id is NULL
332 333 * - RTEMS_INVALID_NAME - invalid task name
333 334 * - RTEMS_INVALID_PRIORITY - invalid task priority
334 335 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
335 336 * - RTEMS_TOO_MANY - too many tasks created
336 337 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
337 338 * - RTEMS_TOO_MANY - too many global objects
338 339 *
339 340 */
340 341
341 342 rtems_status_code status;
342 343
343 344 //**********
344 345 // SPACEWIRE
345 346 // RECV
346 347 status = rtems_task_create(
347 348 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
348 349 RTEMS_DEFAULT_MODES,
349 350 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
350 351 );
351 352 if (status == RTEMS_SUCCESSFUL) // SEND
352 353 {
353 354 status = rtems_task_create(
354 355 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * 2,
355 356 RTEMS_DEFAULT_MODES,
356 357 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
357 358 );
358 359 }
359 360 if (status == RTEMS_SUCCESSFUL) // WTDG
360 361 {
361 362 status = rtems_task_create(
362 363 Task_name[TASKID_WTDG], TASK_PRIORITY_WTDG, RTEMS_MINIMUM_STACK_SIZE,
363 364 RTEMS_DEFAULT_MODES,
364 365 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_WTDG]
365 366 );
366 367 }
367 368 if (status == RTEMS_SUCCESSFUL) // ACTN
368 369 {
369 370 status = rtems_task_create(
370 371 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
371 372 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
372 373 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
373 374 );
374 375 }
375 376 if (status == RTEMS_SUCCESSFUL) // SPIQ
376 377 {
377 378 status = rtems_task_create(
378 379 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
379 380 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
380 381 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
381 382 );
382 383 }
383 384
384 385 //******************
385 386 // SPECTRAL MATRICES
386 387 if (status == RTEMS_SUCCESSFUL) // AVF0
387 388 {
388 389 status = rtems_task_create(
389 390 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
390 391 RTEMS_DEFAULT_MODES,
391 392 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
392 393 );
393 394 }
394 395 if (status == RTEMS_SUCCESSFUL) // PRC0
395 396 {
396 397 status = rtems_task_create(
397 398 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * 2,
398 399 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
399 400 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
400 401 );
401 402 }
402 403 if (status == RTEMS_SUCCESSFUL) // AVF1
403 404 {
404 405 status = rtems_task_create(
405 406 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
406 407 RTEMS_DEFAULT_MODES,
407 408 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
408 409 );
409 410 }
410 411 if (status == RTEMS_SUCCESSFUL) // PRC1
411 412 {
412 413 status = rtems_task_create(
413 414 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * 2,
414 415 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
415 416 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
416 417 );
417 418 }
418 419 if (status == RTEMS_SUCCESSFUL) // AVF2
419 420 {
420 421 status = rtems_task_create(
421 422 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
422 423 RTEMS_DEFAULT_MODES,
423 424 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
424 425 );
425 426 }
426 427 if (status == RTEMS_SUCCESSFUL) // PRC2
427 428 {
428 429 status = rtems_task_create(
429 430 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * 2,
430 431 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
431 432 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
432 433 );
433 434 }
434 435
435 436 //****************
436 437 // WAVEFORM PICKER
437 438 if (status == RTEMS_SUCCESSFUL) // WFRM
438 439 {
439 440 status = rtems_task_create(
440 441 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
441 442 RTEMS_DEFAULT_MODES,
442 443 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
443 444 );
444 445 }
445 446 if (status == RTEMS_SUCCESSFUL) // CWF3
446 447 {
447 448 status = rtems_task_create(
448 449 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
449 450 RTEMS_DEFAULT_MODES,
450 451 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
451 452 );
452 453 }
453 454 if (status == RTEMS_SUCCESSFUL) // CWF2
454 455 {
455 456 status = rtems_task_create(
456 457 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
457 458 RTEMS_DEFAULT_MODES,
458 459 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
459 460 );
460 461 }
461 462 if (status == RTEMS_SUCCESSFUL) // CWF1
462 463 {
463 464 status = rtems_task_create(
464 465 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
465 466 RTEMS_DEFAULT_MODES,
466 467 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
467 468 );
468 469 }
469 470 if (status == RTEMS_SUCCESSFUL) // SWBD
470 471 {
471 472 status = rtems_task_create(
472 473 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
473 474 RTEMS_DEFAULT_MODES,
474 475 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
475 476 );
476 477 }
477 478
478 479 //*****
479 480 // MISC
480 481 if (status == RTEMS_SUCCESSFUL) // STAT
481 482 {
482 483 status = rtems_task_create(
483 484 Task_name[TASKID_STAT], TASK_PRIORITY_STAT, RTEMS_MINIMUM_STACK_SIZE,
484 485 RTEMS_DEFAULT_MODES,
485 486 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_STAT]
486 487 );
487 488 }
488 489 if (status == RTEMS_SUCCESSFUL) // DUMB
489 490 {
490 491 status = rtems_task_create(
491 492 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
492 493 RTEMS_DEFAULT_MODES,
493 494 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
494 495 );
495 496 }
496 497 if (status == RTEMS_SUCCESSFUL) // HOUS
497 498 {
498 499 status = rtems_task_create(
499 500 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
500 501 RTEMS_DEFAULT_MODES,
501 502 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
502 503 );
503 504 }
504 505
505 506 return status;
506 507 }
507 508
508 509 int start_recv_send_tasks( void )
509 510 {
510 511 rtems_status_code status;
511 512
512 513 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
513 514 if (status!=RTEMS_SUCCESSFUL) {
514 515 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
515 516 }
516 517
517 518 if (status == RTEMS_SUCCESSFUL) // SEND
518 519 {
519 520 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
520 521 if (status!=RTEMS_SUCCESSFUL) {
521 522 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
522 523 }
523 524 }
524 525
525 526 return status;
526 527 }
527 528
528 529 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
529 530 {
530 531 /** This function starts all RTEMS tasks used in the software.
531 532 *
532 533 * @return RTEMS directive status codes:
533 534 * - RTEMS_SUCCESSFUL - ask started successfully
534 535 * - RTEMS_INVALID_ADDRESS - invalid task entry point
535 536 * - RTEMS_INVALID_ID - invalid task id
536 537 * - RTEMS_INCORRECT_STATE - task not in the dormant state
537 538 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
538 539 *
539 540 */
540 541 // starts all the tasks fot eh flight software
541 542
542 543 rtems_status_code status;
543 544
544 545 //**********
545 546 // SPACEWIRE
546 547 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
547 548 if (status!=RTEMS_SUCCESSFUL) {
548 549 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
549 550 }
550 551
551 552 if (status == RTEMS_SUCCESSFUL) // WTDG
552 553 {
553 554 status = rtems_task_start( Task_id[TASKID_WTDG], wtdg_task, 1 );
554 555 if (status!=RTEMS_SUCCESSFUL) {
555 556 BOOT_PRINTF("in INIT *** Error starting TASK_WTDG\n")
556 557 }
557 558 }
558 559
559 560 if (status == RTEMS_SUCCESSFUL) // ACTN
560 561 {
561 562 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
562 563 if (status!=RTEMS_SUCCESSFUL) {
563 564 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
564 565 }
565 566 }
566 567
567 568 //******************
568 569 // SPECTRAL MATRICES
569 570 if (status == RTEMS_SUCCESSFUL) // AVF0
570 571 {
571 572 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
572 573 if (status!=RTEMS_SUCCESSFUL) {
573 574 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
574 575 }
575 576 }
576 577 if (status == RTEMS_SUCCESSFUL) // PRC0
577 578 {
578 579 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
579 580 if (status!=RTEMS_SUCCESSFUL) {
580 581 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
581 582 }
582 583 }
583 584 if (status == RTEMS_SUCCESSFUL) // AVF1
584 585 {
585 586 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
586 587 if (status!=RTEMS_SUCCESSFUL) {
587 588 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
588 589 }
589 590 }
590 591 if (status == RTEMS_SUCCESSFUL) // PRC1
591 592 {
592 593 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
593 594 if (status!=RTEMS_SUCCESSFUL) {
594 595 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
595 596 }
596 597 }
597 598 if (status == RTEMS_SUCCESSFUL) // AVF2
598 599 {
599 600 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
600 601 if (status!=RTEMS_SUCCESSFUL) {
601 602 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
602 603 }
603 604 }
604 605 if (status == RTEMS_SUCCESSFUL) // PRC2
605 606 {
606 607 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
607 608 if (status!=RTEMS_SUCCESSFUL) {
608 609 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
609 610 }
610 611 }
611 612
612 613 //****************
613 614 // WAVEFORM PICKER
614 615 if (status == RTEMS_SUCCESSFUL) // WFRM
615 616 {
616 617 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
617 618 if (status!=RTEMS_SUCCESSFUL) {
618 619 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
619 620 }
620 621 }
621 622 if (status == RTEMS_SUCCESSFUL) // CWF3
622 623 {
623 624 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
624 625 if (status!=RTEMS_SUCCESSFUL) {
625 626 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
626 627 }
627 628 }
628 629 if (status == RTEMS_SUCCESSFUL) // CWF2
629 630 {
630 631 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
631 632 if (status!=RTEMS_SUCCESSFUL) {
632 633 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
633 634 }
634 635 }
635 636 if (status == RTEMS_SUCCESSFUL) // CWF1
636 637 {
637 638 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
638 639 if (status!=RTEMS_SUCCESSFUL) {
639 640 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
640 641 }
641 642 }
642 643 if (status == RTEMS_SUCCESSFUL) // SWBD
643 644 {
644 645 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
645 646 if (status!=RTEMS_SUCCESSFUL) {
646 647 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
647 648 }
648 649 }
649 650
650 651 //*****
651 652 // MISC
652 653 if (status == RTEMS_SUCCESSFUL) // HOUS
653 654 {
654 655 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
655 656 if (status!=RTEMS_SUCCESSFUL) {
656 657 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
657 658 }
658 659 }
659 660 if (status == RTEMS_SUCCESSFUL) // DUMB
660 661 {
661 662 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
662 663 if (status!=RTEMS_SUCCESSFUL) {
663 664 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
664 665 }
665 666 }
666 667 if (status == RTEMS_SUCCESSFUL) // STAT
667 668 {
668 669 status = rtems_task_start( Task_id[TASKID_STAT], stat_task, 1 );
669 670 if (status!=RTEMS_SUCCESSFUL) {
670 671 BOOT_PRINTF("in INIT *** Error starting TASK_STAT\n")
671 672 }
672 673 }
673 674
674 675 return status;
675 676 }
676 677
677 678 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
678 679 {
679 680 rtems_status_code status_recv;
680 681 rtems_status_code status_send;
681 682 rtems_status_code status_q_p0;
682 683 rtems_status_code status_q_p1;
683 684 rtems_status_code status_q_p2;
684 685 rtems_status_code ret;
685 686 rtems_id queue_id;
686 687
687 688 //****************************************
688 689 // create the queue for handling valid TCs
689 690 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
690 691 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
691 692 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
692 693 if ( status_recv != RTEMS_SUCCESSFUL ) {
693 694 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
694 695 }
695 696
696 697 //************************************************
697 698 // create the queue for handling TM packet sending
698 699 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
699 700 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
700 701 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
701 702 if ( status_send != RTEMS_SUCCESSFUL ) {
702 703 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
703 704 }
704 705
705 706 //*****************************************************************************
706 707 // create the queue for handling averaged spectral matrices for processing @ f0
707 708 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
708 709 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
709 710 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
710 711 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
711 712 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
712 713 }
713 714
714 715 //*****************************************************************************
715 716 // create the queue for handling averaged spectral matrices for processing @ f1
716 717 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
717 718 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
718 719 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
719 720 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
720 721 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
721 722 }
722 723
723 724 //*****************************************************************************
724 725 // create the queue for handling averaged spectral matrices for processing @ f2
725 726 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
726 727 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
727 728 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
728 729 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
729 730 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
730 731 }
731 732
732 733 if ( status_recv != RTEMS_SUCCESSFUL )
733 734 {
734 735 ret = status_recv;
735 736 }
736 737 else if( status_send != RTEMS_SUCCESSFUL )
737 738 {
738 739 ret = status_send;
739 740 }
740 741 else if( status_q_p0 != RTEMS_SUCCESSFUL )
741 742 {
742 743 ret = status_q_p0;
743 744 }
744 745 else if( status_q_p1 != RTEMS_SUCCESSFUL )
745 746 {
746 747 ret = status_q_p1;
747 748 }
748 749 else
749 750 {
750 751 ret = status_q_p2;
751 752 }
752 753
753 754 return ret;
754 755 }
755 756
756 757 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
757 758 {
758 759 rtems_status_code status;
759 760 rtems_name queue_name;
760 761
761 762 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
762 763
763 764 status = rtems_message_queue_ident( queue_name, 0, queue_id );
764 765
765 766 return status;
766 767 }
767 768
768 769 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
769 770 {
770 771 rtems_status_code status;
771 772 rtems_name queue_name;
772 773
773 774 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
774 775
775 776 status = rtems_message_queue_ident( queue_name, 0, queue_id );
776 777
777 778 return status;
778 779 }
779 780
780 781 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
781 782 {
782 783 rtems_status_code status;
783 784 rtems_name queue_name;
784 785
785 786 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
786 787
787 788 status = rtems_message_queue_ident( queue_name, 0, queue_id );
788 789
789 790 return status;
790 791 }
791 792
792 793 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
793 794 {
794 795 rtems_status_code status;
795 796 rtems_name queue_name;
796 797
797 798 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
798 799
799 800 status = rtems_message_queue_ident( queue_name, 0, queue_id );
800 801
801 802 return status;
802 803 }
803 804
804 805 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
805 806 {
806 807 rtems_status_code status;
807 808 rtems_name queue_name;
808 809
809 810 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
810 811
811 812 status = rtems_message_queue_ident( queue_name, 0, queue_id );
812 813
813 814 return status;
814 815 }
816
817 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max )
818 {
819 u_int32_t count;
820 rtems_status_code status;
821
822 status = rtems_message_queue_get_number_pending( queue_id, &count );
823
824 count = count + 1;
825
826 if (status != RTEMS_SUCCESSFUL)
827 {
828 PRINTF1("in update_queue_max_count *** ERR = %d\n", status)
829 }
830 else
831 {
832 if (count > *fifo_size_max)
833 {
834 *fifo_size_max = count;
835 }
836 }
837 }
838
839 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
840 {
841 unsigned char i;
842
843 //***************
844 // BUFFER ADDRESS
845 for(i=0; i<nbNodes; i++)
846 {
847 ring[i].coarseTime = 0x00;
848 ring[i].fineTime = 0x00;
849 ring[i].sid = 0x00;
850 ring[i].status = 0x00;
851 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
852 }
853
854 //*****
855 // NEXT
856 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
857 for(i=0; i<nbNodes-1; i++)
858 {
859 ring[i].next = (ring_node*) &ring[ i + 1 ];
860 }
861
862 //*********
863 // PREVIOUS
864 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
865 for(i=1; i<nbNodes; i++)
866 {
867 ring[i].previous = (ring_node*) &ring[ i - 1 ];
868 }
869 }
Show file before | Show file after | Hide comments
@@ -1,513 +1,530
1 1 /** General usage functions and RTEMS tasks.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 */
7 7
8 8 #include "fsw_misc.h"
9 9
10 10 void configure_timer(gptimer_regs_t *gptimer_regs, unsigned char timer, unsigned int clock_divider,
11 11 unsigned char interrupt_level, rtems_isr (*timer_isr)() )
12 12 {
13 13 /** This function configures a GPTIMER timer instantiated in the VHDL design.
14 14 *
15 15 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
16 16 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
17 17 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
18 18 * @param interrupt_level is the interrupt level that the timer drives.
19 19 * @param timer_isr is the interrupt subroutine that will be attached to the IRQ driven by the timer.
20 20 *
21 21 * Interrupt levels are described in the SPARC documentation sparcv8.pdf p.76
22 22 *
23 23 */
24 24
25 25 rtems_status_code status;
26 26 rtems_isr_entry old_isr_handler;
27 27
28 28 gptimer_regs->timer[timer].ctrl = 0x00; // reset the control register
29 29
30 30 status = rtems_interrupt_catch( timer_isr, interrupt_level, &old_isr_handler) ; // see sparcv8.pdf p.76 for interrupt levels
31 31 if (status!=RTEMS_SUCCESSFUL)
32 32 {
33 33 PRINTF("in configure_timer *** ERR rtems_interrupt_catch\n")
34 34 }
35 35
36 36 timer_set_clock_divider( gptimer_regs, timer, clock_divider);
37 37 }
38 38
39 39 void timer_start(gptimer_regs_t *gptimer_regs, unsigned char timer)
40 40 {
41 41 /** This function starts a GPTIMER timer.
42 42 *
43 43 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
44 44 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
45 45 *
46 46 */
47 47
48 48 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000010; // clear pending IRQ if any
49 49 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000004; // LD load value from the reload register
50 50 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000001; // EN enable the timer
51 51 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000002; // RS restart
52 52 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000008; // IE interrupt enable
53 53 }
54 54
55 55 void timer_stop(gptimer_regs_t *gptimer_regs, unsigned char timer)
56 56 {
57 57 /** This function stops a GPTIMER timer.
58 58 *
59 59 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
60 60 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
61 61 *
62 62 */
63 63
64 64 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & 0xfffffffe; // EN enable the timer
65 65 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & 0xffffffef; // IE interrupt enable
66 66 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000010; // clear pending IRQ if any
67 67 }
68 68
69 69 void timer_set_clock_divider(gptimer_regs_t *gptimer_regs, unsigned char timer, unsigned int clock_divider)
70 70 {
71 71 /** This function sets the clock divider of a GPTIMER timer.
72 72 *
73 73 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
74 74 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
75 75 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
76 76 *
77 77 */
78 78
79 79 gptimer_regs->timer[timer].reload = clock_divider; // base clock frequency is 1 MHz
80 80 }
81 81
82 82 int send_console_outputs_on_apbuart_port( void ) // Send the console outputs on the apbuart port
83 83 {
84 84 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
85 85
86 86 apbuart_regs->ctrl = APBUART_CTRL_REG_MASK_TE;
87 87
88 88 return 0;
89 89 }
90 90
91 91 int enable_apbuart_transmitter( void ) // set the bit 1, TE Transmitter Enable to 1 in the APBUART control register
92 92 {
93 93 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
94 94
95 95 apbuart_regs->ctrl = apbuart_regs->ctrl | APBUART_CTRL_REG_MASK_TE;
96 96
97 97 return 0;
98 98 }
99 99
100 100 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value)
101 101 {
102 102 /** This function sets the scaler reload register of the apbuart module
103 103 *
104 104 * @param regs is the address of the apbuart registers in memory
105 105 * @param value is the value that will be stored in the scaler register
106 106 *
107 107 * The value shall be set by the software to get data on the serial interface.
108 108 *
109 109 */
110 110
111 111 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) regs;
112 112
113 113 apbuart_regs->scaler = value;
114 114 BOOT_PRINTF1("OK *** apbuart port scaler reload register set to 0x%x\n", value)
115 115 }
116 116
117 117 //************
118 118 // RTEMS TASKS
119 119
120 120 rtems_task stat_task(rtems_task_argument argument)
121 121 {
122 122 int i;
123 123 int j;
124 124 i = 0;
125 125 j = 0;
126 126 BOOT_PRINTF("in STAT *** \n")
127 127 while(1){
128 128 rtems_task_wake_after(1000);
129 129 PRINTF1("%d\n", j)
130 130 if (i == CPU_USAGE_REPORT_PERIOD) {
131 131 // #ifdef PRINT_TASK_STATISTICS
132 132 // rtems_cpu_usage_report();
133 133 // rtems_cpu_usage_reset();
134 134 // #endif
135 135 i = 0;
136 136 }
137 137 else i++;
138 138 j++;
139 139 }
140 140 }
141 141
142 142 rtems_task hous_task(rtems_task_argument argument)
143 143 {
144 144 rtems_status_code status;
145 145 rtems_status_code spare_status;
146 146 rtems_id queue_id;
147 147 rtems_rate_monotonic_period_status period_status;
148 148
149 149 status = get_message_queue_id_send( &queue_id );
150 150 if (status != RTEMS_SUCCESSFUL)
151 151 {
152 152 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
153 153 }
154 154
155 155 BOOT_PRINTF("in HOUS ***\n")
156 156
157 157 if (rtems_rate_monotonic_ident( name_hk_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
158 158 status = rtems_rate_monotonic_create( name_hk_rate_monotonic, &HK_id );
159 159 if( status != RTEMS_SUCCESSFUL ) {
160 160 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status )
161 161 }
162 162 }
163 163
164 housekeeping_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
165 housekeeping_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
166 housekeeping_packet.reserved = DEFAULT_RESERVED;
167 housekeeping_packet.userApplication = CCSDS_USER_APP;
168 housekeeping_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
169 housekeeping_packet.packetID[1] = (unsigned char) (APID_TM_HK);
170 housekeeping_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
171 housekeeping_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
172 housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
173 housekeeping_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
174 housekeeping_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
175 housekeeping_packet.serviceType = TM_TYPE_HK;
176 housekeeping_packet.serviceSubType = TM_SUBTYPE_HK;
177 housekeeping_packet.destinationID = TM_DESTINATION_ID_GROUND;
178 housekeeping_packet.sid = SID_HK;
179
180 164 status = rtems_rate_monotonic_cancel(HK_id);
181 165 if( status != RTEMS_SUCCESSFUL ) {
182 166 PRINTF1( "ERR *** in HOUS *** rtems_rate_monotonic_cancel(HK_id) ***code: %d\n", status )
183 167 }
184 168 else {
185 169 DEBUG_PRINTF("OK *** in HOUS *** rtems_rate_monotonic_cancel(HK_id)\n")
186 170 }
187 171
188 172 // startup phase
189 173 status = rtems_rate_monotonic_period( HK_id, SY_LFR_TIME_SYN_TIMEOUT_in_ticks );
190 174 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
191 175 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
192 176 while(period_status.state != RATE_MONOTONIC_EXPIRED ) // after SY_LFR_TIME_SYN_TIMEOUT ms, starts HK anyway
193 177 {
194 178 if ((time_management_regs->coarse_time & 0x80000000) == 0x00000000) // check time synchronization
195 179 {
196 180 break; // break if LFR is synchronized
197 181 }
198 182 else
199 183 {
200 184 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
201 185 // sched_yield();
202 186 status = rtems_task_wake_after( 10 ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 100 ms = 10 * 10 ms
203 187 }
204 188 }
205 189 status = rtems_rate_monotonic_cancel(HK_id);
206 190 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
207 191
208 192 while(1){ // launch the rate monotonic task
209 193 status = rtems_rate_monotonic_period( HK_id, HK_PERIOD );
210 194 if ( status != RTEMS_SUCCESSFUL ) {
211 195 PRINTF1( "in HOUS *** ERR period: %d\n", status);
212 196 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_6 );
213 197 }
214 198 else {
215 199 housekeeping_packet.packetSequenceControl[0] = (unsigned char) (sequenceCounterHK >> 8);
216 200 housekeeping_packet.packetSequenceControl[1] = (unsigned char) (sequenceCounterHK );
217 201 increment_seq_counter( &sequenceCounterHK );
218 202
219 203 housekeeping_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
220 204 housekeeping_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
221 205 housekeeping_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
222 206 housekeeping_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
223 207 housekeeping_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
224 208 housekeeping_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
225 209
226 210 spacewire_update_statistics();
227 211
212 housekeeping_packet.hk_lfr_q_sd_fifo_size_max = hk_lfr_q_sd_fifo_size_max;
213 housekeeping_packet.hk_lfr_q_rv_fifo_size_max = hk_lfr_q_rv_fifo_size_max;
214 housekeeping_packet.hk_lfr_q_p0_fifo_size_max = hk_lfr_q_p0_fifo_size_max;
215 housekeeping_packet.hk_lfr_q_p1_fifo_size_max = hk_lfr_q_p1_fifo_size_max;
216 housekeeping_packet.hk_lfr_q_p2_fifo_size_max = hk_lfr_q_p2_fifo_size_max;
217
228 218 housekeeping_packet.sy_lfr_common_parameters_spare = parameter_dump_packet.sy_lfr_common_parameters_spare;
229 219 housekeeping_packet.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
230 220 get_temperatures( housekeeping_packet.hk_lfr_temp_scm );
231 221 get_v_e1_e2_f3( housekeeping_packet.hk_lfr_sc_v_f3 );
232 222 get_cpu_load( (unsigned char *) &housekeeping_packet.hk_lfr_cpu_load );
233 223
234 224 // SEND PACKET
235 225 status = rtems_message_queue_send( queue_id, &housekeeping_packet,
236 226 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
237 227 if (status != RTEMS_SUCCESSFUL) {
238 228 PRINTF1("in HOUS *** ERR send: %d\n", status)
239 229 }
240 230 }
241 231 }
242 232
243 233 PRINTF("in HOUS *** deleting task\n")
244 234
245 235 status = rtems_task_delete( RTEMS_SELF ); // should not return
246 236 printf( "rtems_task_delete returned with status of %d.\n", status );
247 237 return;
248 238 }
249 239
250 240 rtems_task dumb_task( rtems_task_argument unused )
251 241 {
252 242 /** This RTEMS taks is used to print messages without affecting the general behaviour of the software.
253 243 *
254 244 * @param unused is the starting argument of the RTEMS task
255 245 *
256 246 * The DUMB taks waits for RTEMS events and print messages depending on the incoming events.
257 247 *
258 248 */
259 249
260 250 unsigned int i;
261 251 unsigned int intEventOut;
262 252 unsigned int coarse_time = 0;
263 253 unsigned int fine_time = 0;
264 254 rtems_event_set event_out;
265 255
266 256 char *DumbMessages[12] = {"in DUMB *** default", // RTEMS_EVENT_0
267 257 "in DUMB *** timecode_irq_handler", // RTEMS_EVENT_1
268 258 "in DUMB *** f3 buffer changed", // RTEMS_EVENT_2
269 259 "in DUMB *** in SMIQ *** Error sending event to AVF0", // RTEMS_EVENT_3
270 260 "in DUMB *** spectral_matrices_isr *** Error sending event to SMIQ", // RTEMS_EVENT_4
271 261 "in DUMB *** waveforms_simulator_isr", // RTEMS_EVENT_5
272 262 "VHDL SM *** two buffers f0 ready", // RTEMS_EVENT_6
273 263 "ready for dump", // RTEMS_EVENT_7
274 264 "VHDL ERR *** spectral matrix", // RTEMS_EVENT_8
275 265 "tick", // RTEMS_EVENT_9
276 266 "VHDL ERR *** waveform picker", // RTEMS_EVENT_10
277 267 "VHDL ERR *** unexpected ready matrix values" // RTEMS_EVENT_11
278 268 };
279 269
280 270 BOOT_PRINTF("in DUMB *** \n")
281 271
282 272 while(1){
283 273 rtems_event_receive(RTEMS_EVENT_0 | RTEMS_EVENT_1 | RTEMS_EVENT_2 | RTEMS_EVENT_3
284 274 | RTEMS_EVENT_4 | RTEMS_EVENT_5 | RTEMS_EVENT_6 | RTEMS_EVENT_7
285 275 | RTEMS_EVENT_8 | RTEMS_EVENT_9,
286 276 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT
287 277 intEventOut = (unsigned int) event_out;
288 278 for ( i=0; i<32; i++)
289 279 {
290 280 if ( ((intEventOut >> i) & 0x0001) != 0)
291 281 {
292 282 coarse_time = time_management_regs->coarse_time;
293 283 fine_time = time_management_regs->fine_time;
294 284 printf("in DUMB *** coarse: %x, fine: %x, %s\n", coarse_time, fine_time, DumbMessages[i]);
295 285 if (i==8)
296 286 {
297 287 }
298 288 if (i==10)
299 289 {
300 290 }
301 291 }
302 292 }
303 293 }
304 294 }
305 295
306 296 //*****************************
307 297 // init housekeeping parameters
308 298
309 299 void init_housekeeping_parameters( void )
310 300 {
311 301 /** This function initialize the housekeeping_packet global variable with default values.
312 302 *
313 303 */
314 304
315 305 unsigned int i = 0;
316 306 unsigned char *parameters;
307 unsigned char sizeOfHK;
317 308
318 parameters = (unsigned char*) &housekeeping_packet.lfr_status_word;
319 for(i = 0; i< SIZE_HK_PARAMETERS; i++)
309 sizeOfHK = sizeof( Packet_TM_LFR_HK_t );
310
311 parameters = (unsigned char*) &housekeeping_packet;
312
313 for(i = 0; i< sizeOfHK; i++)
320 314 {
321 315 parameters[i] = 0x00;
322 316 }
317
318 housekeeping_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
319 housekeeping_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
320 housekeeping_packet.reserved = DEFAULT_RESERVED;
321 housekeeping_packet.userApplication = CCSDS_USER_APP;
322 housekeeping_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
323 housekeeping_packet.packetID[1] = (unsigned char) (APID_TM_HK);
324 housekeeping_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
325 housekeeping_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
326 housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
327 housekeeping_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
328 housekeeping_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
329 housekeeping_packet.serviceType = TM_TYPE_HK;
330 housekeeping_packet.serviceSubType = TM_SUBTYPE_HK;
331 housekeeping_packet.destinationID = TM_DESTINATION_ID_GROUND;
332 housekeeping_packet.sid = SID_HK;
333
323 334 // init status word
324 335 housekeeping_packet.lfr_status_word[0] = DEFAULT_STATUS_WORD_BYTE0;
325 336 housekeeping_packet.lfr_status_word[1] = DEFAULT_STATUS_WORD_BYTE1;
326 337 // init software version
327 338 housekeeping_packet.lfr_sw_version[0] = SW_VERSION_N1;
328 339 housekeeping_packet.lfr_sw_version[1] = SW_VERSION_N2;
329 340 housekeeping_packet.lfr_sw_version[2] = SW_VERSION_N3;
330 341 housekeeping_packet.lfr_sw_version[3] = SW_VERSION_N4;
331 342 // init fpga version
332 343 parameters = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
333 344 housekeeping_packet.lfr_fpga_version[0] = parameters[1]; // n1
334 345 housekeeping_packet.lfr_fpga_version[1] = parameters[2]; // n2
335 346 housekeeping_packet.lfr_fpga_version[2] = parameters[3]; // n3
347
348 housekeeping_packet.hk_lfr_q_sd_fifo_size = MSG_QUEUE_COUNT_SEND;
349 housekeeping_packet.hk_lfr_q_rv_fifo_size = MSG_QUEUE_COUNT_RECV;
350 housekeeping_packet.hk_lfr_q_p0_fifo_size = MSG_QUEUE_COUNT_PRC0;
351 housekeeping_packet.hk_lfr_q_p1_fifo_size = MSG_QUEUE_COUNT_PRC1;
352 housekeeping_packet.hk_lfr_q_p2_fifo_size = MSG_QUEUE_COUNT_PRC2;
336 353 }
337 354
338 355 void increment_seq_counter( unsigned short *packetSequenceControl )
339 356 {
340 357 /** This function increment the sequence counter passes in argument.
341 358 *
342 359 * The increment does not affect the grouping flag. In case of an overflow, the counter is reset to 0.
343 360 *
344 361 */
345 362
346 363 unsigned short segmentation_grouping_flag;
347 364 unsigned short sequence_cnt;
348 365
349 366 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8; // keep bits 7 downto 6
350 367 sequence_cnt = (*packetSequenceControl) & 0x3fff; // [0011 1111 1111 1111]
351 368
352 369 if ( sequence_cnt < SEQ_CNT_MAX)
353 370 {
354 371 sequence_cnt = sequence_cnt + 1;
355 372 }
356 373 else
357 374 {
358 375 sequence_cnt = 0;
359 376 }
360 377
361 378 *packetSequenceControl = segmentation_grouping_flag | sequence_cnt ;
362 379 }
363 380
364 381 void getTime( unsigned char *time)
365 382 {
366 383 /** This function write the current local time in the time buffer passed in argument.
367 384 *
368 385 */
369 386
370 387 time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
371 388 time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
372 389 time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
373 390 time[3] = (unsigned char) (time_management_regs->coarse_time);
374 391 time[4] = (unsigned char) (time_management_regs->fine_time>>8);
375 392 time[5] = (unsigned char) (time_management_regs->fine_time);
376 393 }
377 394
378 395 unsigned long long int getTimeAsUnsignedLongLongInt( )
379 396 {
380 397 /** This function write the current local time in the time buffer passed in argument.
381 398 *
382 399 */
383 400 unsigned long long int time;
384 401
385 402 time = ( (unsigned long long int) (time_management_regs->coarse_time & 0x7fffffff) << 16 )
386 403 + time_management_regs->fine_time;
387 404
388 405 return time;
389 406 }
390 407
391 408 void send_dumb_hk( void )
392 409 {
393 410 Packet_TM_LFR_HK_t dummy_hk_packet;
394 411 unsigned char *parameters;
395 412 unsigned int i;
396 413 rtems_id queue_id;
397 414
398 415 dummy_hk_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
399 416 dummy_hk_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
400 417 dummy_hk_packet.reserved = DEFAULT_RESERVED;
401 418 dummy_hk_packet.userApplication = CCSDS_USER_APP;
402 419 dummy_hk_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
403 420 dummy_hk_packet.packetID[1] = (unsigned char) (APID_TM_HK);
404 421 dummy_hk_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
405 422 dummy_hk_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
406 423 dummy_hk_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
407 424 dummy_hk_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
408 425 dummy_hk_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
409 426 dummy_hk_packet.serviceType = TM_TYPE_HK;
410 427 dummy_hk_packet.serviceSubType = TM_SUBTYPE_HK;
411 428 dummy_hk_packet.destinationID = TM_DESTINATION_ID_GROUND;
412 429 dummy_hk_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
413 430 dummy_hk_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
414 431 dummy_hk_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
415 432 dummy_hk_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
416 433 dummy_hk_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
417 434 dummy_hk_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
418 435 dummy_hk_packet.sid = SID_HK;
419 436
420 437 // init status word
421 438 dummy_hk_packet.lfr_status_word[0] = 0xff;
422 439 dummy_hk_packet.lfr_status_word[1] = 0xff;
423 440 // init software version
424 441 dummy_hk_packet.lfr_sw_version[0] = SW_VERSION_N1;
425 442 dummy_hk_packet.lfr_sw_version[1] = SW_VERSION_N2;
426 443 dummy_hk_packet.lfr_sw_version[2] = SW_VERSION_N3;
427 444 dummy_hk_packet.lfr_sw_version[3] = SW_VERSION_N4;
428 445 // init fpga version
429 446 parameters = (unsigned char *) (REGS_ADDR_WAVEFORM_PICKER + 0xb0);
430 447 dummy_hk_packet.lfr_fpga_version[0] = parameters[1]; // n1
431 448 dummy_hk_packet.lfr_fpga_version[1] = parameters[2]; // n2
432 449 dummy_hk_packet.lfr_fpga_version[2] = parameters[3]; // n3
433 450
434 451 parameters = (unsigned char *) &dummy_hk_packet.hk_lfr_cpu_load;
435 452
436 453 for (i=0; i<100; i++)
437 454 {
438 455 parameters[i] = 0xff;
439 456 }
440 457
441 458 get_message_queue_id_send( &queue_id );
442 459
443 460 rtems_message_queue_send( queue_id, &dummy_hk_packet,
444 461 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
445 462 }
446 463
447 464 void get_temperatures( unsigned char *temperatures )
448 465 {
449 466 unsigned char* temp_scm_ptr;
450 467 unsigned char* temp_pcb_ptr;
451 468 unsigned char* temp_fpga_ptr;
452 469
453 470 // SEL1 SEL0
454 471 // 0 0 => PCB
455 472 // 0 1 => FPGA
456 473 // 1 0 => SCM
457 474
458 475 temp_scm_ptr = (unsigned char *) &time_management_regs->temp_scm;
459 476 temp_pcb_ptr = (unsigned char *) &time_management_regs->temp_pcb;
460 477 temp_fpga_ptr = (unsigned char *) &time_management_regs->temp_fpga;
461 478
462 479 temperatures[0] = temp_scm_ptr[2];
463 480 temperatures[1] = temp_scm_ptr[3];
464 481 temperatures[2] = temp_pcb_ptr[2];
465 482 temperatures[3] = temp_pcb_ptr[3];
466 483 temperatures[4] = temp_fpga_ptr[2];
467 484 temperatures[5] = temp_fpga_ptr[3];
468 485 }
469 486
470 487 void get_v_e1_e2_f3( unsigned char *spacecraft_potential )
471 488 {
472 489 unsigned char* v_ptr;
473 490 unsigned char* e1_ptr;
474 491 unsigned char* e2_ptr;
475 492
476 493 v_ptr = (unsigned char *) &waveform_picker_regs->v;
477 494 e1_ptr = (unsigned char *) &waveform_picker_regs->e1;
478 495 e2_ptr = (unsigned char *) &waveform_picker_regs->e2;
479 496
480 497 spacecraft_potential[0] = v_ptr[2];
481 498 spacecraft_potential[1] = v_ptr[3];
482 499 spacecraft_potential[2] = e1_ptr[2];
483 500 spacecraft_potential[3] = e1_ptr[3];
484 501 spacecraft_potential[4] = e2_ptr[2];
485 502 spacecraft_potential[5] = e2_ptr[3];
486 503 }
487 504
488 505 void get_cpu_load( unsigned char *resource_statistics )
489 506 {
490 507 unsigned char cpu_load;
491 508
492 509 cpu_load = lfr_rtems_cpu_usage_report();
493 510
494 511 // HK_LFR_CPU_LOAD
495 512 resource_statistics[0] = cpu_load;
496 513
497 514 // HK_LFR_CPU_LOAD_MAX
498 515 if (cpu_load > resource_statistics[1])
499 516 {
500 517 resource_statistics[1] = cpu_load;
501 518 }
502 519
503 520 // CPU_LOAD_AVE
504 521 resource_statistics[2] = 0;
505 522
506 523 #ifndef PRINT_TASK_STATISTICS
507 524 rtems_cpu_usage_reset();
508 525 #endif
509 526
510 527 }
511 528
512 529
513 530
Show file before | Show file after | Hide comments
@@ -1,1295 +1,1283
1 1 /** Functions related to the SpaceWire interface.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle SpaceWire transmissions:
7 7 * - configuration of the SpaceWire link
8 8 * - SpaceWire related interruption requests processing
9 9 * - transmission of TeleMetry packets by a dedicated RTEMS task
10 10 * - reception of TeleCommands by a dedicated RTEMS task
11 11 *
12 12 */
13 13
14 14 #include "fsw_spacewire.h"
15 15
16 16 rtems_name semq_name;
17 17 rtems_id semq_id;
18 18
19 19 //*****************
20 20 // waveform headers
21 21 Header_TM_LFR_SCIENCE_CWF_t headerCWF;
22 22 Header_TM_LFR_SCIENCE_SWF_t headerSWF;
23 23 Header_TM_LFR_SCIENCE_ASM_t headerASM;
24 24
25 25 //***********
26 26 // RTEMS TASK
27 27 rtems_task spiq_task(rtems_task_argument unused)
28 28 {
29 29 /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver.
30 30 *
31 31 * @param unused is the starting argument of the RTEMS task
32 32 *
33 33 */
34 34
35 35 rtems_event_set event_out;
36 36 rtems_status_code status;
37 37 int linkStatus;
38 38
39 39 BOOT_PRINTF("in SPIQ *** \n")
40 40
41 41 while(true){
42 42 rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT
43 43 PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n")
44 44
45 45 // [0] SUSPEND RECV AND SEND TASKS
46 46 status = rtems_task_suspend( Task_id[ TASKID_RECV ] );
47 47 if ( status != RTEMS_SUCCESSFUL ) {
48 48 PRINTF("in SPIQ *** ERR suspending RECV Task\n")
49 49 }
50 50 status = rtems_task_suspend( Task_id[ TASKID_SEND ] );
51 51 if ( status != RTEMS_SUCCESSFUL ) {
52 52 PRINTF("in SPIQ *** ERR suspending SEND Task\n")
53 53 }
54 54
55 55 // [1] CHECK THE LINK
56 56 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1)
57 57 if ( linkStatus != 5) {
58 58 PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus)
59 59 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
60 60 }
61 61
62 62 // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT
63 63 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2)
64 64 if ( linkStatus != 5 ) // [2.a] not in run state, reset the link
65 65 {
66 66 spacewire_compute_stats_offsets();
67 67 status = spacewire_reset_link( );
68 68 }
69 69 else // [2.b] in run state, start the link
70 70 {
71 71 status = spacewire_stop_and_start_link( fdSPW ); // start the link
72 72 if ( status != RTEMS_SUCCESSFUL)
73 73 {
74 PRINTF1("in SPIQ *** ERR spacewire_start_link %d\n", status)
74 PRINTF1("in SPIQ *** ERR spacewire_stop_and_start_link %d\n", status)
75 75 }
76 76 }
77 77
78 78 // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS
79 79 if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully
80 80 {
81 81 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
82 82 if ( status != RTEMS_SUCCESSFUL ) {
83 83 PRINTF("in SPIQ *** ERR resuming SEND Task\n")
84 84 }
85 85 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
86 86 if ( status != RTEMS_SUCCESSFUL ) {
87 87 PRINTF("in SPIQ *** ERR resuming RECV Task\n")
88 88 }
89 89 }
90 90 else // [3.b] the link is not in run state, go in STANDBY mode
91 91 {
92 status = stop_current_mode();
93 if ( status != RTEMS_SUCCESSFUL ) {
94 PRINTF1("in SPIQ *** ERR stop_current_mode *** code %d\n", status)
95 }
96 92 status = enter_mode( LFR_MODE_STANDBY, 0 );
97 93 if ( status != RTEMS_SUCCESSFUL ) {
98 94 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
99 95 }
100 96 // wake the WTDG task up to wait for the link recovery
101 97 status = rtems_event_send ( Task_id[TASKID_WTDG], RTEMS_EVENT_0 );
102 98 status = rtems_task_suspend( RTEMS_SELF );
103 99 }
104 100 }
105 101 }
106 102
107 103 rtems_task recv_task( rtems_task_argument unused )
108 104 {
109 105 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
110 106 *
111 107 * @param unused is the starting argument of the RTEMS task
112 108 *
113 109 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
114 110 * 1. It reads the incoming data.
115 111 * 2. Launches the acceptance procedure.
116 112 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
117 113 *
118 114 */
119 115
120 116 int len;
121 117 ccsdsTelecommandPacket_t currentTC;
122 118 unsigned char computed_CRC[ 2 ];
123 119 unsigned char currentTC_LEN_RCV[ 2 ];
124 120 unsigned char destinationID;
125 121 unsigned int estimatedPacketLength;
126 122 unsigned int parserCode;
127 123 rtems_status_code status;
128 124 rtems_id queue_recv_id;
129 125 rtems_id queue_send_id;
130 126
131 127 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
132 128
133 129 status = get_message_queue_id_recv( &queue_recv_id );
134 130 if (status != RTEMS_SUCCESSFUL)
135 131 {
136 132 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
137 133 }
138 134
139 135 status = get_message_queue_id_send( &queue_send_id );
140 136 if (status != RTEMS_SUCCESSFUL)
141 137 {
142 138 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
143 139 }
144 140
145 141 BOOT_PRINTF("in RECV *** \n")
146 142
147 143 while(1)
148 144 {
149 145 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
150 146 if (len == -1){ // error during the read call
151 147 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
152 148 }
153 149 else {
154 150 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
155 151 PRINTF("in RECV *** packet lenght too short\n")
156 152 }
157 153 else {
158 154 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // => -3 is for Prot ID, Reserved and User App bytes
159 155 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> 8);
160 156 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
161 157 // CHECK THE TC
162 158 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
163 159 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
164 160 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
165 161 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
166 162 || (parserCode == WRONG_SRC_ID) )
167 163 { // send TM_LFR_TC_EXE_CORRUPTED
168 164 PRINTF1("TC corrupted received, with code: %d\n", parserCode)
169 165 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
170 166 &&
171 167 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
172 168 )
173 169 {
174 170 if ( parserCode == WRONG_SRC_ID )
175 171 {
176 172 destinationID = SID_TC_GROUND;
177 173 }
178 174 else
179 175 {
180 176 destinationID = currentTC.sourceID;
181 177 }
182 178 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
183 179 computed_CRC, currentTC_LEN_RCV,
184 180 destinationID );
185 181 }
186 182 }
187 183 else
188 184 { // send valid TC to the action launcher
189 185 status = rtems_message_queue_send( queue_recv_id, &currentTC,
190 186 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + 3);
191 187 }
192 188 }
193 189 }
190
191 update_queue_max_count( queue_recv_id, &hk_lfr_q_rv_fifo_size_max );
192
194 193 }
195 194 }
196 195
197 196 rtems_task send_task( rtems_task_argument argument)
198 197 {
199 198 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
200 199 *
201 200 * @param unused is the starting argument of the RTEMS task
202 201 *
203 202 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
204 203 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
205 204 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
206 205 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
207 206 * data it contains.
208 207 *
209 208 */
210 209
211 210 rtems_status_code status; // RTEMS status code
212 211 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
213 212 ring_node *incomingRingNodePtr;
214 213 int ring_node_address;
215 214 char *charPtr;
216 215 spw_ioctl_pkt_send *spw_ioctl_send;
217 216 size_t size; // size of the incoming TC packet
218 u_int32_t count;
219 rtems_id queue_id;
217 rtems_id queue_send_id;
220 218 unsigned int sid;
221 219
222 220 incomingRingNodePtr = NULL;
223 221 ring_node_address = 0;
224 222 charPtr = (char *) &ring_node_address;
225 223 sid = 0;
226 224
227 225 init_header_cwf( &headerCWF );
228 226 init_header_swf( &headerSWF );
229 227 init_header_asm( &headerASM );
230 228
231 status = get_message_queue_id_send( &queue_id );
229 status = get_message_queue_id_send( &queue_send_id );
232 230 if (status != RTEMS_SUCCESSFUL)
233 231 {
234 232 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
235 233 }
236 234
237 235 BOOT_PRINTF("in SEND *** \n")
238 236
239 237 while(1)
240 238 {
241 status = rtems_message_queue_receive( queue_id, incomingData, &size,
239 status = rtems_message_queue_receive( queue_send_id, incomingData, &size,
242 240 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
243 241
244 242 if (status!=RTEMS_SUCCESSFUL)
245 243 {
246 244 PRINTF1("in SEND *** (1) ERR = %d\n", status)
247 245 }
248 246 else
249 247 {
250 248 if ( size == sizeof(ring_node*) )
251 249 {
252 250 charPtr[0] = incomingData[0];
253 251 charPtr[1] = incomingData[1];
254 252 charPtr[2] = incomingData[2];
255 253 charPtr[3] = incomingData[3];
256 254 incomingRingNodePtr = (ring_node*) ring_node_address;
257 255 sid = incomingRingNodePtr->sid;
258 256 if ( (sid==SID_NORM_CWF_LONG_F3)
259 257 || (sid==SID_BURST_CWF_F2 )
260 258 || (sid==SID_SBM1_CWF_F1 )
261 259 || (sid==SID_SBM2_CWF_F2 ))
262 260 {
263 261 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
264 262 }
265 263 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
266 264 {
267 265 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
268 266 }
269 267 else if ( (sid==SID_NORM_CWF_F3) )
270 268 {
271 269 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
272 270 }
273 271 else if (sid==SID_NORM_ASM_F0)
274 272 {
275 273 spw_send_asm_f0( incomingRingNodePtr, &headerASM );
276 274 }
277 275 else if (sid==SID_NORM_ASM_F1)
278 276 {
279 277 spw_send_asm_f1( incomingRingNodePtr, &headerASM );
280 278 }
281 279 else if (sid==SID_NORM_ASM_F2)
282 280 {
283 281 spw_send_asm_f2( incomingRingNodePtr, &headerASM );
284 282 }
285 283 else if ( sid==TM_CODE_K_DUMP )
286 284 {
287 285 spw_send_k_dump( incomingRingNodePtr );
288 286 }
289 287 else
290 288 {
291 289 printf("unexpected sid = %d\n", sid);
292 290 }
293 291 }
294 292 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
295 293 {
296 294 status = write( fdSPW, incomingData, size );
297 295 if (status == -1){
298 296 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
299 297 }
300 298 }
301 299 else // the incoming message is a spw_ioctl_pkt_send structure
302 300 {
303 301 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
304 302 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
305 303 if (status == -1){
306 304 printf("size = %d, %x, %x, %x, %x, %x\n",
307 305 size,
308 306 incomingData[0],
309 307 incomingData[1],
310 308 incomingData[2],
311 309 incomingData[3],
312 310 incomingData[4]);
313 311 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
314 312 }
315 313 }
316 314 }
317 315
318 status = rtems_message_queue_get_number_pending( queue_id, &count );
319 if (status != RTEMS_SUCCESSFUL)
320 {
321 PRINTF1("in SEND *** (3) ERR = %d\n", status)
322 }
323 else
324 {
325 if (count > maxCount)
326 {
327 maxCount = count;
328 }
329 }
316 update_queue_max_count( queue_send_id, &hk_lfr_q_sd_fifo_size_max );
317
330 318 }
331 319 }
332 320
333 321 rtems_task wtdg_task( rtems_task_argument argument )
334 322 {
335 323 rtems_event_set event_out;
336 324 rtems_status_code status;
337 325 int linkStatus;
338 326
339 327 BOOT_PRINTF("in WTDG ***\n")
340 328
341 329 while(1)
342 330 {
343 331 // wait for an RTEMS_EVENT
344 332 rtems_event_receive( RTEMS_EVENT_0,
345 333 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
346 334 PRINTF("in WTDG *** wait for the link\n")
347 335 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
348 336 while( linkStatus != 5) // wait for the link
349 337 {
350 338 status = rtems_task_wake_after( 10 ); // monitor the link each 100ms
351 339 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
352 340 }
353 341
354 342 status = spacewire_stop_and_start_link( fdSPW );
355 343
356 344 if (status != RTEMS_SUCCESSFUL)
357 345 {
358 346 PRINTF1("in WTDG *** ERR link not started %d\n", status)
359 347 }
360 348 else
361 349 {
362 350 PRINTF("in WTDG *** OK link started\n")
363 351 }
364 352
365 353 // restart the SPIQ task
366 354 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
367 355 if ( status != RTEMS_SUCCESSFUL ) {
368 356 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
369 357 }
370 358
371 359 // restart RECV and SEND
372 360 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
373 361 if ( status != RTEMS_SUCCESSFUL ) {
374 362 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
375 363 }
376 364 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
377 365 if ( status != RTEMS_SUCCESSFUL ) {
378 366 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
379 367 }
380 368 }
381 369 }
382 370
383 371 //****************
384 372 // OTHER FUNCTIONS
385 373 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
386 374 {
387 375 /** This function opens the SpaceWire link.
388 376 *
389 377 * @return a valid file descriptor in case of success, -1 in case of a failure
390 378 *
391 379 */
392 380 rtems_status_code status;
393 381
394 382 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
395 383 if ( fdSPW < 0 ) {
396 384 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
397 385 }
398 386 else
399 387 {
400 388 status = RTEMS_SUCCESSFUL;
401 389 }
402 390
403 391 return status;
404 392 }
405 393
406 394 int spacewire_start_link( int fd )
407 395 {
408 396 rtems_status_code status;
409 397
410 398 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
411 399 // -1 default hardcoded driver timeout
412 400
413 401 return status;
414 402 }
415 403
416 404 int spacewire_stop_and_start_link( int fd )
417 405 {
418 406 rtems_status_code status;
419 407
420 408 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
421 409 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
422 410 // -1 default hardcoded driver timeout
423 411
424 412 return status;
425 413 }
426 414
427 415 int spacewire_configure_link( int fd )
428 416 {
429 417 /** This function configures the SpaceWire link.
430 418 *
431 419 * @return GR-RTEMS-DRIVER directive status codes:
432 420 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
433 421 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
434 422 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
435 423 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
436 424 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
437 425 * - 5 EIO - Error when writing to grswp hardware registers.
438 426 * - 2 ENOENT - No such file or directory
439 427 */
440 428
441 429 rtems_status_code status;
442 430
443 431 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
444 432 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
445 433
446 434 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
447 435 if (status!=RTEMS_SUCCESSFUL) {
448 436 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
449 437 }
450 438 //
451 439 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
452 440 if (status!=RTEMS_SUCCESSFUL) {
453 441 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
454 442 }
455 443 //
456 444 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
457 445 if (status!=RTEMS_SUCCESSFUL) {
458 446 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
459 447 }
460 448 //
461 449 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
462 450 if (status!=RTEMS_SUCCESSFUL) {
463 451 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
464 452 }
465 453 //
466 454 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
467 455 if (status!=RTEMS_SUCCESSFUL) {
468 456 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
469 457 }
470 458 //
471 459 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
472 460 if (status!=RTEMS_SUCCESSFUL) {
473 461 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
474 462 }
475 463 //
476 464 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, 0x0909); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
477 465 if (status!=RTEMS_SUCCESSFUL) {
478 466 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
479 467 }
480 468
481 469 return status;
482 470 }
483 471
484 472 int spacewire_reset_link( void )
485 473 {
486 474 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
487 475 *
488 476 * @return RTEMS directive status code:
489 477 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
490 478 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
491 479 *
492 480 */
493 481
494 482 rtems_status_code status_spw;
495 483 rtems_status_code status;
496 484 int i;
497 485
498 486 for ( i=0; i<SY_LFR_DPU_CONNECT_ATTEMPT; i++ )
499 487 {
500 488 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
501 489
502 490 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
503 491
504 492 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
505 493
506 494 status_spw = spacewire_stop_and_start_link( fdSPW );
507 495 if ( status_spw != RTEMS_SUCCESSFUL )
508 496 {
509 497 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw)
510 498 }
511 499
512 500 if ( status_spw == RTEMS_SUCCESSFUL)
513 501 {
514 502 break;
515 503 }
516 504 }
517 505
518 506 return status_spw;
519 507 }
520 508
521 509 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
522 510 {
523 511 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
524 512 *
525 513 * @param val is the value, 0 or 1, used to set the value of the NP bit.
526 514 * @param regAddr is the address of the GRSPW control register.
527 515 *
528 516 * NP is the bit 20 of the GRSPW control register.
529 517 *
530 518 */
531 519
532 520 unsigned int *spwptr = (unsigned int*) regAddr;
533 521
534 522 if (val == 1) {
535 523 *spwptr = *spwptr | 0x00100000; // [NP] set the No port force bit
536 524 }
537 525 if (val== 0) {
538 526 *spwptr = *spwptr & 0xffdfffff;
539 527 }
540 528 }
541 529
542 530 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
543 531 {
544 532 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
545 533 *
546 534 * @param val is the value, 0 or 1, used to set the value of the RE bit.
547 535 * @param regAddr is the address of the GRSPW control register.
548 536 *
549 537 * RE is the bit 16 of the GRSPW control register.
550 538 *
551 539 */
552 540
553 541 unsigned int *spwptr = (unsigned int*) regAddr;
554 542
555 543 if (val == 1)
556 544 {
557 545 *spwptr = *spwptr | 0x00010000; // [RE] set the RMAP Enable bit
558 546 }
559 547 if (val== 0)
560 548 {
561 549 *spwptr = *spwptr & 0xfffdffff;
562 550 }
563 551 }
564 552
565 553 void spacewire_compute_stats_offsets( void )
566 554 {
567 555 /** This function computes the SpaceWire statistics offsets in case of a SpaceWire related interruption raising.
568 556 *
569 557 * The offsets keep a record of the statistics in case of a reset of the statistics. They are added to the current statistics
570 558 * to keep the counters consistent even after a reset of the SpaceWire driver (the counter are set to zero by the driver when it
571 559 * during the open systel call).
572 560 *
573 561 */
574 562
575 563 spw_stats spacewire_stats_grspw;
576 564 rtems_status_code status;
577 565
578 566 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
579 567
580 568 spacewire_stats_backup.packets_received = spacewire_stats_grspw.packets_received
581 569 + spacewire_stats.packets_received;
582 570 spacewire_stats_backup.packets_sent = spacewire_stats_grspw.packets_sent
583 571 + spacewire_stats.packets_sent;
584 572 spacewire_stats_backup.parity_err = spacewire_stats_grspw.parity_err
585 573 + spacewire_stats.parity_err;
586 574 spacewire_stats_backup.disconnect_err = spacewire_stats_grspw.disconnect_err
587 575 + spacewire_stats.disconnect_err;
588 576 spacewire_stats_backup.escape_err = spacewire_stats_grspw.escape_err
589 577 + spacewire_stats.escape_err;
590 578 spacewire_stats_backup.credit_err = spacewire_stats_grspw.credit_err
591 579 + spacewire_stats.credit_err;
592 580 spacewire_stats_backup.write_sync_err = spacewire_stats_grspw.write_sync_err
593 581 + spacewire_stats.write_sync_err;
594 582 spacewire_stats_backup.rx_rmap_header_crc_err = spacewire_stats_grspw.rx_rmap_header_crc_err
595 583 + spacewire_stats.rx_rmap_header_crc_err;
596 584 spacewire_stats_backup.rx_rmap_data_crc_err = spacewire_stats_grspw.rx_rmap_data_crc_err
597 585 + spacewire_stats.rx_rmap_data_crc_err;
598 586 spacewire_stats_backup.early_ep = spacewire_stats_grspw.early_ep
599 587 + spacewire_stats.early_ep;
600 588 spacewire_stats_backup.invalid_address = spacewire_stats_grspw.invalid_address
601 589 + spacewire_stats.invalid_address;
602 590 spacewire_stats_backup.rx_eep_err = spacewire_stats_grspw.rx_eep_err
603 591 + spacewire_stats.rx_eep_err;
604 592 spacewire_stats_backup.rx_truncated = spacewire_stats_grspw.rx_truncated
605 593 + spacewire_stats.rx_truncated;
606 594 }
607 595
608 596 void spacewire_update_statistics( void )
609 597 {
610 598 rtems_status_code status;
611 599 spw_stats spacewire_stats_grspw;
612 600
613 601 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
614 602
615 603 spacewire_stats.packets_received = spacewire_stats_backup.packets_received
616 604 + spacewire_stats_grspw.packets_received;
617 605 spacewire_stats.packets_sent = spacewire_stats_backup.packets_sent
618 606 + spacewire_stats_grspw.packets_sent;
619 607 spacewire_stats.parity_err = spacewire_stats_backup.parity_err
620 608 + spacewire_stats_grspw.parity_err;
621 609 spacewire_stats.disconnect_err = spacewire_stats_backup.disconnect_err
622 610 + spacewire_stats_grspw.disconnect_err;
623 611 spacewire_stats.escape_err = spacewire_stats_backup.escape_err
624 612 + spacewire_stats_grspw.escape_err;
625 613 spacewire_stats.credit_err = spacewire_stats_backup.credit_err
626 614 + spacewire_stats_grspw.credit_err;
627 615 spacewire_stats.write_sync_err = spacewire_stats_backup.write_sync_err
628 616 + spacewire_stats_grspw.write_sync_err;
629 617 spacewire_stats.rx_rmap_header_crc_err = spacewire_stats_backup.rx_rmap_header_crc_err
630 618 + spacewire_stats_grspw.rx_rmap_header_crc_err;
631 619 spacewire_stats.rx_rmap_data_crc_err = spacewire_stats_backup.rx_rmap_data_crc_err
632 620 + spacewire_stats_grspw.rx_rmap_data_crc_err;
633 621 spacewire_stats.early_ep = spacewire_stats_backup.early_ep
634 622 + spacewire_stats_grspw.early_ep;
635 623 spacewire_stats.invalid_address = spacewire_stats_backup.invalid_address
636 624 + spacewire_stats_grspw.invalid_address;
637 625 spacewire_stats.rx_eep_err = spacewire_stats_backup.rx_eep_err
638 626 + spacewire_stats_grspw.rx_eep_err;
639 627 spacewire_stats.rx_truncated = spacewire_stats_backup.rx_truncated
640 628 + spacewire_stats_grspw.rx_truncated;
641 629 //spacewire_stats.tx_link_err;
642 630
643 631 //****************************
644 632 // DPU_SPACEWIRE_IF_STATISTICS
645 633 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (spacewire_stats.packets_received >> 8);
646 634 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (spacewire_stats.packets_received);
647 635 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (spacewire_stats.packets_sent >> 8);
648 636 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (spacewire_stats.packets_sent);
649 637 //housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt;
650 638 //housekeeping_packet.hk_lfr_dpu_spw_last_timc;
651 639
652 640 //******************************************
653 641 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
654 642 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) spacewire_stats.parity_err;
655 643 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) spacewire_stats.disconnect_err;
656 644 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) spacewire_stats.escape_err;
657 645 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) spacewire_stats.credit_err;
658 646 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) spacewire_stats.write_sync_err;
659 647
660 648 //*********************************************
661 649 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
662 650 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) spacewire_stats.early_ep;
663 651 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) spacewire_stats.invalid_address;
664 652 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) spacewire_stats.rx_eep_err;
665 653 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) spacewire_stats.rx_truncated;
666 654 }
667 655
668 656 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
669 657 {
670 658 // a valid timecode has been received, write it in the HK report
671 659 unsigned int * grspwPtr;
672 660
673 661 grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER);
674 662
675 663 housekeeping_packet.hk_lfr_dpu_spw_last_timc = (unsigned char) (grspwPtr[0] & 0xff); // [11 1111]
676 664
677 665 // update the number of valid timecodes that have been received
678 666 if (housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt == 255)
679 667 {
680 668 housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt = 0;
681 669 }
682 670 else
683 671 {
684 672 housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt = housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt + 1;
685 673 }
686 674 }
687 675
688 676 rtems_timer_service_routine user_routine( rtems_id timer_id, void *user_data )
689 677 {
690 678 int linkStatus;
691 679 rtems_status_code status;
692 680
693 681 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
694 682
695 683 if ( linkStatus == 5) {
696 684 PRINTF("in spacewire_reset_link *** link is running\n")
697 685 status = RTEMS_SUCCESSFUL;
698 686 }
699 687 }
700 688
701 689 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
702 690 {
703 691 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
704 692 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
705 693 header->reserved = DEFAULT_RESERVED;
706 694 header->userApplication = CCSDS_USER_APP;
707 695 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
708 696 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
709 697 header->packetLength[0] = 0x00;
710 698 header->packetLength[1] = 0x00;
711 699 // DATA FIELD HEADER
712 700 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
713 701 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
714 702 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
715 703 header->destinationID = TM_DESTINATION_ID_GROUND;
716 704 header->time[0] = 0x00;
717 705 header->time[0] = 0x00;
718 706 header->time[0] = 0x00;
719 707 header->time[0] = 0x00;
720 708 header->time[0] = 0x00;
721 709 header->time[0] = 0x00;
722 710 // AUXILIARY DATA HEADER
723 711 header->sid = 0x00;
724 712 header->hkBIA = DEFAULT_HKBIA;
725 713 header->blkNr[0] = 0x00;
726 714 header->blkNr[1] = 0x00;
727 715 }
728 716
729 717 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
730 718 {
731 719 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
732 720 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
733 721 header->reserved = DEFAULT_RESERVED;
734 722 header->userApplication = CCSDS_USER_APP;
735 723 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
736 724 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
737 725 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
738 726 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
739 727 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
740 728 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
741 729 // DATA FIELD HEADER
742 730 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
743 731 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
744 732 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
745 733 header->destinationID = TM_DESTINATION_ID_GROUND;
746 734 header->time[0] = 0x00;
747 735 header->time[0] = 0x00;
748 736 header->time[0] = 0x00;
749 737 header->time[0] = 0x00;
750 738 header->time[0] = 0x00;
751 739 header->time[0] = 0x00;
752 740 // AUXILIARY DATA HEADER
753 741 header->sid = 0x00;
754 742 header->hkBIA = DEFAULT_HKBIA;
755 743 header->pktCnt = DEFAULT_PKTCNT; // PKT_CNT
756 744 header->pktNr = 0x00;
757 745 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
758 746 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
759 747 }
760 748
761 749 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
762 750 {
763 751 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
764 752 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
765 753 header->reserved = DEFAULT_RESERVED;
766 754 header->userApplication = CCSDS_USER_APP;
767 755 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
768 756 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
769 757 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
770 758 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
771 759 header->packetLength[0] = 0x00;
772 760 header->packetLength[1] = 0x00;
773 761 // DATA FIELD HEADER
774 762 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
775 763 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
776 764 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
777 765 header->destinationID = TM_DESTINATION_ID_GROUND;
778 766 header->time[0] = 0x00;
779 767 header->time[0] = 0x00;
780 768 header->time[0] = 0x00;
781 769 header->time[0] = 0x00;
782 770 header->time[0] = 0x00;
783 771 header->time[0] = 0x00;
784 772 // AUXILIARY DATA HEADER
785 773 header->sid = 0x00;
786 774 header->biaStatusInfo = 0x00;
787 775 header->pa_lfr_pkt_cnt_asm = 0x00;
788 776 header->pa_lfr_pkt_nr_asm = 0x00;
789 777 header->pa_lfr_asm_blk_nr[0] = 0x00;
790 778 header->pa_lfr_asm_blk_nr[1] = 0x00;
791 779 }
792 780
793 781 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
794 782 Header_TM_LFR_SCIENCE_CWF_t *header )
795 783 {
796 784 /** This function sends CWF CCSDS packets (F2, F1 or F0).
797 785 *
798 786 * @param waveform points to the buffer containing the data that will be send.
799 787 * @param sid is the source identifier of the data that will be sent.
800 788 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
801 789 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
802 790 * contain information to setup the transmission of the data packets.
803 791 *
804 792 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
805 793 *
806 794 */
807 795
808 796 unsigned int i;
809 797 int ret;
810 798 unsigned int coarseTime;
811 799 unsigned int fineTime;
812 800 rtems_status_code status;
813 801 spw_ioctl_pkt_send spw_ioctl_send_CWF;
814 802 int *dataPtr;
815 803 unsigned char sid;
816 804
817 805 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
818 806 spw_ioctl_send_CWF.options = 0;
819 807
820 808 ret = LFR_DEFAULT;
821 809 sid = (unsigned char) ring_node_to_send->sid;
822 810
823 811 coarseTime = ring_node_to_send->coarseTime;
824 812 fineTime = ring_node_to_send->fineTime;
825 813 dataPtr = (int*) ring_node_to_send->buffer_address;
826 814
827 815 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
828 816 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
829 817 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
830 818 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
831 819 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
832 820
833 821 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
834 822 {
835 823 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
836 824 spw_ioctl_send_CWF.hdr = (char*) header;
837 825 // BUILD THE DATA
838 826 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
839 827
840 828 // SET PACKET SEQUENCE CONTROL
841 829 increment_seq_counter_source_id( header->packetSequenceControl, sid );
842 830
843 831 // SET SID
844 832 header->sid = sid;
845 833
846 834 // SET PACKET TIME
847 835 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
848 836 //
849 837 header->time[0] = header->acquisitionTime[0];
850 838 header->time[1] = header->acquisitionTime[1];
851 839 header->time[2] = header->acquisitionTime[2];
852 840 header->time[3] = header->acquisitionTime[3];
853 841 header->time[4] = header->acquisitionTime[4];
854 842 header->time[5] = header->acquisitionTime[5];
855 843
856 844 // SET PACKET ID
857 845 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
858 846 {
859 847 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8);
860 848 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
861 849 }
862 850 else
863 851 {
864 852 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
865 853 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
866 854 }
867 855
868 856 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
869 857 if (status != RTEMS_SUCCESSFUL) {
870 858 printf("%d-%d, ERR %d\n", sid, i, (int) status);
871 859 ret = LFR_DEFAULT;
872 860 }
873 861 }
874 862
875 863 return ret;
876 864 }
877 865
878 866 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
879 867 Header_TM_LFR_SCIENCE_SWF_t *header )
880 868 {
881 869 /** This function sends SWF CCSDS packets (F2, F1 or F0).
882 870 *
883 871 * @param waveform points to the buffer containing the data that will be send.
884 872 * @param sid is the source identifier of the data that will be sent.
885 873 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
886 874 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
887 875 * contain information to setup the transmission of the data packets.
888 876 *
889 877 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
890 878 *
891 879 */
892 880
893 881 unsigned int i;
894 882 int ret;
895 883 unsigned int coarseTime;
896 884 unsigned int fineTime;
897 885 rtems_status_code status;
898 886 spw_ioctl_pkt_send spw_ioctl_send_SWF;
899 887 int *dataPtr;
900 888 unsigned char sid;
901 889
902 890 spw_ioctl_send_SWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_SWF;
903 891 spw_ioctl_send_SWF.options = 0;
904 892
905 893 ret = LFR_DEFAULT;
906 894
907 895 coarseTime = ring_node_to_send->coarseTime;
908 896 fineTime = ring_node_to_send->fineTime;
909 897 dataPtr = (int*) ring_node_to_send->buffer_address;
910 898 sid = ring_node_to_send->sid;
911 899
912 900 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
913 901
914 902 for (i=0; i<7; i++) // send waveform
915 903 {
916 904 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
917 905 spw_ioctl_send_SWF.hdr = (char*) header;
918 906
919 907 // SET PACKET SEQUENCE CONTROL
920 908 increment_seq_counter_source_id( header->packetSequenceControl, sid );
921 909
922 910 // SET PACKET LENGTH AND BLKNR
923 911 if (i == 6)
924 912 {
925 913 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
926 914 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8);
927 915 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
928 916 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> 8);
929 917 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
930 918 }
931 919 else
932 920 {
933 921 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
934 922 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8);
935 923 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
936 924 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> 8);
937 925 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
938 926 }
939 927
940 928 // SET PACKET TIME
941 929 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
942 930 //
943 931 header->time[0] = header->acquisitionTime[0];
944 932 header->time[1] = header->acquisitionTime[1];
945 933 header->time[2] = header->acquisitionTime[2];
946 934 header->time[3] = header->acquisitionTime[3];
947 935 header->time[4] = header->acquisitionTime[4];
948 936 header->time[5] = header->acquisitionTime[5];
949 937
950 938 // SET SID
951 939 header->sid = sid;
952 940
953 941 // SET PKTNR
954 942 header->pktNr = i+1; // PKT_NR
955 943
956 944 // SEND PACKET
957 945 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
958 946 if (status != RTEMS_SUCCESSFUL) {
959 947 printf("%d-%d, ERR %d\n", sid, i, (int) status);
960 948 ret = LFR_DEFAULT;
961 949 }
962 950 }
963 951
964 952 return ret;
965 953 }
966 954
967 955 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
968 956 Header_TM_LFR_SCIENCE_CWF_t *header )
969 957 {
970 958 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
971 959 *
972 960 * @param waveform points to the buffer containing the data that will be send.
973 961 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
974 962 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
975 963 * contain information to setup the transmission of the data packets.
976 964 *
977 965 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
978 966 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
979 967 *
980 968 */
981 969
982 970 unsigned int i;
983 971 int ret;
984 972 unsigned int coarseTime;
985 973 unsigned int fineTime;
986 974 rtems_status_code status;
987 975 spw_ioctl_pkt_send spw_ioctl_send_CWF;
988 976 char *dataPtr;
989 977 unsigned char sid;
990 978
991 979 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
992 980 spw_ioctl_send_CWF.options = 0;
993 981
994 982 ret = LFR_DEFAULT;
995 983 sid = ring_node_to_send->sid;
996 984
997 985 coarseTime = ring_node_to_send->coarseTime;
998 986 fineTime = ring_node_to_send->fineTime;
999 987 dataPtr = (char*) ring_node_to_send->buffer_address;
1000 988
1001 989 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> 8);
1002 990 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
1003 991 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1004 992 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> 8);
1005 993 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
1006 994
1007 995 //*********************
1008 996 // SEND CWF3_light DATA
1009 997 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
1010 998 {
1011 999 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
1012 1000 spw_ioctl_send_CWF.hdr = (char*) header;
1013 1001 // BUILD THE DATA
1014 1002 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
1015 1003
1016 1004 // SET PACKET SEQUENCE COUNTER
1017 1005 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1018 1006
1019 1007 // SET SID
1020 1008 header->sid = sid;
1021 1009
1022 1010 // SET PACKET TIME
1023 1011 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1024 1012 //
1025 1013 header->time[0] = header->acquisitionTime[0];
1026 1014 header->time[1] = header->acquisitionTime[1];
1027 1015 header->time[2] = header->acquisitionTime[2];
1028 1016 header->time[3] = header->acquisitionTime[3];
1029 1017 header->time[4] = header->acquisitionTime[4];
1030 1018 header->time[5] = header->acquisitionTime[5];
1031 1019
1032 1020 // SET PACKET ID
1033 1021 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1034 1022 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1035 1023
1036 1024 // SEND PACKET
1037 1025 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1038 1026 if (status != RTEMS_SUCCESSFUL) {
1039 1027 printf("%d-%d, ERR %d\n", sid, i, (int) status);
1040 1028 ret = LFR_DEFAULT;
1041 1029 }
1042 1030 }
1043 1031
1044 1032 return ret;
1045 1033 }
1046 1034
1047 1035 void spw_send_asm_f0( ring_node *ring_node_to_send,
1048 1036 Header_TM_LFR_SCIENCE_ASM_t *header )
1049 1037 {
1050 1038 unsigned int i;
1051 1039 unsigned int length = 0;
1052 1040 rtems_status_code status;
1053 1041 unsigned int sid;
1054 1042 float *spectral_matrix;
1055 1043 int coarseTime;
1056 1044 int fineTime;
1057 1045 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1058 1046
1059 1047 sid = ring_node_to_send->sid;
1060 1048 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1061 1049 coarseTime = ring_node_to_send->coarseTime;
1062 1050 fineTime = ring_node_to_send->fineTime;
1063 1051
1064 1052 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1065 1053
1066 1054 for (i=0; i<3; i++)
1067 1055 {
1068 1056 if ((i==0) || (i==1))
1069 1057 {
1070 1058 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_1;
1071 1059 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1072 1060 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1073 1061 ];
1074 1062 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_1;
1075 1063 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1076 1064 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> 8 ); // BLK_NR MSB
1077 1065 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_1); // BLK_NR LSB
1078 1066 }
1079 1067 else
1080 1068 {
1081 1069 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_2;
1082 1070 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1083 1071 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1084 1072 ];
1085 1073 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_2;
1086 1074 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1087 1075 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> 8 ); // BLK_NR MSB
1088 1076 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_2); // BLK_NR LSB
1089 1077 }
1090 1078
1091 1079 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1092 1080 spw_ioctl_send_ASM.hdr = (char *) header;
1093 1081 spw_ioctl_send_ASM.options = 0;
1094 1082
1095 1083 // (2) BUILD THE HEADER
1096 1084 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1097 1085 header->packetLength[0] = (unsigned char) (length>>8);
1098 1086 header->packetLength[1] = (unsigned char) (length);
1099 1087 header->sid = (unsigned char) sid; // SID
1100 1088 header->pa_lfr_pkt_cnt_asm = 3;
1101 1089 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1102 1090
1103 1091 // (3) SET PACKET TIME
1104 1092 header->time[0] = (unsigned char) (coarseTime>>24);
1105 1093 header->time[1] = (unsigned char) (coarseTime>>16);
1106 1094 header->time[2] = (unsigned char) (coarseTime>>8);
1107 1095 header->time[3] = (unsigned char) (coarseTime);
1108 1096 header->time[4] = (unsigned char) (fineTime>>8);
1109 1097 header->time[5] = (unsigned char) (fineTime);
1110 1098 //
1111 1099 header->acquisitionTime[0] = header->time[0];
1112 1100 header->acquisitionTime[1] = header->time[1];
1113 1101 header->acquisitionTime[2] = header->time[2];
1114 1102 header->acquisitionTime[3] = header->time[3];
1115 1103 header->acquisitionTime[4] = header->time[4];
1116 1104 header->acquisitionTime[5] = header->time[5];
1117 1105
1118 1106 // (4) SEND PACKET
1119 1107 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1120 1108 if (status != RTEMS_SUCCESSFUL) {
1121 1109 printf("in ASM_send *** ERR %d\n", (int) status);
1122 1110 }
1123 1111 }
1124 1112 }
1125 1113
1126 1114 void spw_send_asm_f1( ring_node *ring_node_to_send,
1127 1115 Header_TM_LFR_SCIENCE_ASM_t *header )
1128 1116 {
1129 1117 unsigned int i;
1130 1118 unsigned int length = 0;
1131 1119 rtems_status_code status;
1132 1120 unsigned int sid;
1133 1121 float *spectral_matrix;
1134 1122 int coarseTime;
1135 1123 int fineTime;
1136 1124 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1137 1125
1138 1126 sid = ring_node_to_send->sid;
1139 1127 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1140 1128 coarseTime = ring_node_to_send->coarseTime;
1141 1129 fineTime = ring_node_to_send->fineTime;
1142 1130
1143 1131 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1144 1132
1145 1133 for (i=0; i<3; i++)
1146 1134 {
1147 1135 if ((i==0) || (i==1))
1148 1136 {
1149 1137 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_1;
1150 1138 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1151 1139 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1152 1140 ];
1153 1141 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_1;
1154 1142 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1155 1143 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> 8 ); // BLK_NR MSB
1156 1144 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_1); // BLK_NR LSB
1157 1145 }
1158 1146 else
1159 1147 {
1160 1148 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_2;
1161 1149 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1162 1150 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1163 1151 ];
1164 1152 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_2;
1165 1153 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1166 1154 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> 8 ); // BLK_NR MSB
1167 1155 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_2); // BLK_NR LSB
1168 1156 }
1169 1157
1170 1158 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1171 1159 spw_ioctl_send_ASM.hdr = (char *) header;
1172 1160 spw_ioctl_send_ASM.options = 0;
1173 1161
1174 1162 // (2) BUILD THE HEADER
1175 1163 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1176 1164 header->packetLength[0] = (unsigned char) (length>>8);
1177 1165 header->packetLength[1] = (unsigned char) (length);
1178 1166 header->sid = (unsigned char) sid; // SID
1179 1167 header->pa_lfr_pkt_cnt_asm = 3;
1180 1168 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1181 1169
1182 1170 // (3) SET PACKET TIME
1183 1171 header->time[0] = (unsigned char) (coarseTime>>24);
1184 1172 header->time[1] = (unsigned char) (coarseTime>>16);
1185 1173 header->time[2] = (unsigned char) (coarseTime>>8);
1186 1174 header->time[3] = (unsigned char) (coarseTime);
1187 1175 header->time[4] = (unsigned char) (fineTime>>8);
1188 1176 header->time[5] = (unsigned char) (fineTime);
1189 1177 //
1190 1178 header->acquisitionTime[0] = header->time[0];
1191 1179 header->acquisitionTime[1] = header->time[1];
1192 1180 header->acquisitionTime[2] = header->time[2];
1193 1181 header->acquisitionTime[3] = header->time[3];
1194 1182 header->acquisitionTime[4] = header->time[4];
1195 1183 header->acquisitionTime[5] = header->time[5];
1196 1184
1197 1185 // (4) SEND PACKET
1198 1186 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1199 1187 if (status != RTEMS_SUCCESSFUL) {
1200 1188 printf("in ASM_send *** ERR %d\n", (int) status);
1201 1189 }
1202 1190 }
1203 1191 }
1204 1192
1205 1193 void spw_send_asm_f2( ring_node *ring_node_to_send,
1206 1194 Header_TM_LFR_SCIENCE_ASM_t *header )
1207 1195 {
1208 1196 unsigned int i;
1209 1197 unsigned int length = 0;
1210 1198 rtems_status_code status;
1211 1199 unsigned int sid;
1212 1200 float *spectral_matrix;
1213 1201 int coarseTime;
1214 1202 int fineTime;
1215 1203 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1216 1204
1217 1205 sid = ring_node_to_send->sid;
1218 1206 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1219 1207 coarseTime = ring_node_to_send->coarseTime;
1220 1208 fineTime = ring_node_to_send->fineTime;
1221 1209
1222 1210 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1223 1211
1224 1212 for (i=0; i<3; i++)
1225 1213 {
1226 1214
1227 1215 spw_ioctl_send_ASM.dlen = DLEN_ASM_F2_PKT;
1228 1216 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1229 1217 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM )
1230 1218 ];
1231 1219 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1232 1220 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1233 1221 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB
1234 1222 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1235 1223
1236 1224 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1237 1225 spw_ioctl_send_ASM.hdr = (char *) header;
1238 1226 spw_ioctl_send_ASM.options = 0;
1239 1227
1240 1228 // (2) BUILD THE HEADER
1241 1229 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1242 1230 header->packetLength[0] = (unsigned char) (length>>8);
1243 1231 header->packetLength[1] = (unsigned char) (length);
1244 1232 header->sid = (unsigned char) sid; // SID
1245 1233 header->pa_lfr_pkt_cnt_asm = 3;
1246 1234 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1247 1235
1248 1236 // (3) SET PACKET TIME
1249 1237 header->time[0] = (unsigned char) (coarseTime>>24);
1250 1238 header->time[1] = (unsigned char) (coarseTime>>16);
1251 1239 header->time[2] = (unsigned char) (coarseTime>>8);
1252 1240 header->time[3] = (unsigned char) (coarseTime);
1253 1241 header->time[4] = (unsigned char) (fineTime>>8);
1254 1242 header->time[5] = (unsigned char) (fineTime);
1255 1243 //
1256 1244 header->acquisitionTime[0] = header->time[0];
1257 1245 header->acquisitionTime[1] = header->time[1];
1258 1246 header->acquisitionTime[2] = header->time[2];
1259 1247 header->acquisitionTime[3] = header->time[3];
1260 1248 header->acquisitionTime[4] = header->time[4];
1261 1249 header->acquisitionTime[5] = header->time[5];
1262 1250
1263 1251 // (4) SEND PACKET
1264 1252 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1265 1253 if (status != RTEMS_SUCCESSFUL) {
1266 1254 printf("in ASM_send *** ERR %d\n", (int) status);
1267 1255 }
1268 1256 }
1269 1257 }
1270 1258
1271 1259 void spw_send_k_dump( ring_node *ring_node_to_send )
1272 1260 {
1273 1261 rtems_status_code status;
1274 1262 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump;
1275 1263 unsigned int packetLength;
1276 1264 unsigned int size;
1277 1265
1278 1266 printf("spw_send_k_dump\n");
1279 1267
1280 1268 kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address;
1281 1269
1282 1270 packetLength = kcoefficients_dump->packetLength[0] * 256 + kcoefficients_dump->packetLength[1];
1283 1271
1284 1272 size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
1285 1273
1286 1274 printf("packetLength %d, size %d\n", packetLength, size );
1287 1275
1288 1276 status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size );
1289 1277
1290 1278 if (status == -1){
1291 1279 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
1292 1280 }
1293 1281
1294 1282 ring_node_to_send->status = 0x00;
1295 1283 }
Show file before | Show file after | Hide comments
@@ -1,399 +1,402
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf0_prc0.h"
11 11 #include "fsw_processing.h"
12 12
13 13 nb_sm_before_bp_asm_f0 nb_sm_before_f0;
14 14
15 15 //***
16 16 // F0
17 17 ring_node_asm asm_ring_norm_f0 [ NB_RING_NODES_ASM_NORM_F0 ];
18 18 ring_node_asm asm_ring_burst_sbm_f0 [ NB_RING_NODES_ASM_BURST_SBM_F0 ];
19 19
20 20 ring_node ring_to_send_asm_f0 [ NB_RING_NODES_ASM_F0 ];
21 21 int buffer_asm_f0 [ NB_RING_NODES_ASM_F0 * TOTAL_SIZE_SM ];
22 22
23 23 float asm_f0_patched_norm [ TOTAL_SIZE_SM ];
24 24 float asm_f0_patched_burst_sbm [ TOTAL_SIZE_SM ];
25 25 float asm_f0_reorganized [ TOTAL_SIZE_SM ];
26 26
27 27 char asm_f0_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ];
28 28 float compressed_sm_norm_f0[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F0];
29 29 float compressed_sm_sbm_f0 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F0 ];
30 30
31 31 float k_coeff_intercalib_f0_norm[ NB_BINS_COMPRESSED_SM_F0 * NB_K_COEFF_PER_BIN ]; // 11 * 32 = 352
32 32 float k_coeff_intercalib_f0_sbm[ NB_BINS_COMPRESSED_SM_SBM_F0 * NB_K_COEFF_PER_BIN ]; // 22 * 32 = 704
33 33
34 34 //************
35 35 // RTEMS TASKS
36 36
37 37 rtems_task avf0_task( rtems_task_argument lfrRequestedMode )
38 38 {
39 39 int i;
40 40
41 41 rtems_event_set event_out;
42 42 rtems_status_code status;
43 43 rtems_id queue_id_prc0;
44 44 asm_msg msgForMATR;
45 45 ring_node *nodeForAveraging;
46 46 ring_node *ring_node_tab[8];
47 47 ring_node_asm *current_ring_node_asm_burst_sbm_f0;
48 48 ring_node_asm *current_ring_node_asm_norm_f0;
49 49
50 50 unsigned int nb_norm_bp1;
51 51 unsigned int nb_norm_bp2;
52 52 unsigned int nb_norm_asm;
53 53 unsigned int nb_sbm_bp1;
54 54 unsigned int nb_sbm_bp2;
55 55
56 56 nb_norm_bp1 = 0;
57 57 nb_norm_bp2 = 0;
58 58 nb_norm_asm = 0;
59 59 nb_sbm_bp1 = 0;
60 60 nb_sbm_bp2 = 0;
61 61
62 62 reset_nb_sm_f0( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
63 63 ASM_generic_init_ring( asm_ring_norm_f0, NB_RING_NODES_ASM_NORM_F0 );
64 64 ASM_generic_init_ring( asm_ring_burst_sbm_f0, NB_RING_NODES_ASM_BURST_SBM_F0 );
65 65 current_ring_node_asm_norm_f0 = asm_ring_norm_f0;
66 66 current_ring_node_asm_burst_sbm_f0 = asm_ring_burst_sbm_f0;
67 67
68 68 BOOT_PRINTF1("in AVFO *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
69 69
70 70 status = get_message_queue_id_prc0( &queue_id_prc0 );
71 71 if (status != RTEMS_SUCCESSFUL)
72 72 {
73 73 PRINTF1("in MATR *** ERR get_message_queue_id_prc0 %d\n", status)
74 74 }
75 75
76 76 while(1){
77 77 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
78 78
79 79 //****************************************
80 80 // initialize the mesage for the MATR task
81 81 msgForMATR.norm = current_ring_node_asm_norm_f0;
82 82 msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f0;
83 83 msgForMATR.event = 0x00; // this composite event will be sent to the PRC0 task
84 84 //
85 85 //****************************************
86 86
87 87 nodeForAveraging = getRingNodeForAveraging( 0 );
88 88
89 89 ring_node_tab[NB_SM_BEFORE_AVF0-1] = nodeForAveraging;
90 90 for ( i = 2; i < (NB_SM_BEFORE_AVF0+1); i++ )
91 91 {
92 92 nodeForAveraging = nodeForAveraging->previous;
93 93 ring_node_tab[NB_SM_BEFORE_AVF0-i] = nodeForAveraging;
94 94 }
95 95
96 96 // compute the average and store it in the averaged_sm_f1 buffer
97 97 SM_average( current_ring_node_asm_norm_f0->matrix,
98 98 current_ring_node_asm_burst_sbm_f0->matrix,
99 99 ring_node_tab,
100 100 nb_norm_bp1, nb_sbm_bp1,
101 101 &msgForMATR );
102 102
103 103 // update nb_average
104 104 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0;
105 105 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0;
106 106 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0;
107 107 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0;
108 108 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0;
109 109
110 110 if (nb_sbm_bp1 == nb_sm_before_f0.burst_sbm_bp1)
111 111 {
112 112 nb_sbm_bp1 = 0;
113 113 // set another ring for the ASM storage
114 114 current_ring_node_asm_burst_sbm_f0 = current_ring_node_asm_burst_sbm_f0->next;
115 115 if ( lfrCurrentMode == LFR_MODE_BURST )
116 116 {
117 117 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP1_F0;
118 118 }
119 119 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
120 120 {
121 121 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP1_F0;
122 122 }
123 123 }
124 124
125 125 if (nb_sbm_bp2 == nb_sm_before_f0.burst_sbm_bp2)
126 126 {
127 127 nb_sbm_bp2 = 0;
128 128 if ( lfrCurrentMode == LFR_MODE_BURST )
129 129 {
130 130 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP2_F0;
131 131 }
132 132 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
133 133 {
134 134 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP2_F0;
135 135 }
136 136 }
137 137
138 138 if (nb_norm_bp1 == nb_sm_before_f0.norm_bp1)
139 139 {
140 140 nb_norm_bp1 = 0;
141 141 // set another ring for the ASM storage
142 142 current_ring_node_asm_norm_f0 = current_ring_node_asm_norm_f0->next;
143 143 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
144 144 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
145 145 {
146 146 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F0;
147 147 }
148 148 }
149 149
150 150 if (nb_norm_bp2 == nb_sm_before_f0.norm_bp2)
151 151 {
152 152 nb_norm_bp2 = 0;
153 153 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
154 154 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
155 155 {
156 156 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F0;
157 157 }
158 158 }
159 159
160 160 if (nb_norm_asm == nb_sm_before_f0.norm_asm)
161 161 {
162 162 nb_norm_asm = 0;
163 163 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
164 164 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
165 165 {
166 166 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F0;
167 167 }
168 168 }
169 169
170 170 //*************************
171 171 // send the message to MATR
172 172 if (msgForMATR.event != 0x00)
173 173 {
174 174 status = rtems_message_queue_send( queue_id_prc0, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC0);
175 175 }
176 176
177 177 if (status != RTEMS_SUCCESSFUL) {
178 178 printf("in AVF0 *** Error sending message to MATR, code %d\n", status);
179 179 }
180 180 }
181 181 }
182 182
183 183 rtems_task prc0_task( rtems_task_argument lfrRequestedMode )
184 184 {
185 185 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
186 186 size_t size; // size of the incoming TC packet
187 187 asm_msg *incomingMsg;
188 188 //
189 189 unsigned char sid;
190 190 rtems_status_code status;
191 191 rtems_id queue_id;
192 192 rtems_id queue_id_q_p0;
193 193 bp_packet_with_spare packet_norm_bp1;
194 194 bp_packet packet_norm_bp2;
195 195 bp_packet packet_sbm_bp1;
196 196 bp_packet packet_sbm_bp2;
197 197 ring_node *current_ring_node_to_send_asm_f0;
198 198
199 199 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
200 200 init_ring( ring_to_send_asm_f0, NB_RING_NODES_ASM_F0, (volatile int*) buffer_asm_f0, TOTAL_SIZE_SM );
201 201 current_ring_node_to_send_asm_f0 = ring_to_send_asm_f0;
202 202
203 203 //*************
204 204 // NORM headers
205 205 BP_init_header_with_spare( &packet_norm_bp1,
206 206 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F0,
207 207 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0, NB_BINS_COMPRESSED_SM_F0 );
208 208 BP_init_header( &packet_norm_bp2,
209 209 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F0,
210 210 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0, NB_BINS_COMPRESSED_SM_F0);
211 211
212 212 //****************************
213 213 // BURST SBM1 and SBM2 headers
214 214 if ( lfrRequestedMode == LFR_MODE_BURST )
215 215 {
216 216 BP_init_header( &packet_sbm_bp1,
217 217 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F0,
218 218 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
219 219 BP_init_header( &packet_sbm_bp2,
220 220 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F0,
221 221 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
222 222 }
223 223 else if ( lfrRequestedMode == LFR_MODE_SBM1 )
224 224 {
225 225 BP_init_header( &packet_sbm_bp1,
226 226 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP1_F0,
227 227 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
228 228 BP_init_header( &packet_sbm_bp2,
229 229 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP2_F0,
230 230 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
231 231 }
232 232 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
233 233 {
234 234 BP_init_header( &packet_sbm_bp1,
235 235 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F0,
236 236 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
237 237 BP_init_header( &packet_sbm_bp2,
238 238 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F0,
239 239 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
240 240 }
241 241 else
242 242 {
243 243 PRINTF1("in PRC0 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
244 244 }
245 245
246 246 status = get_message_queue_id_send( &queue_id );
247 247 if (status != RTEMS_SUCCESSFUL)
248 248 {
249 249 PRINTF1("in PRC0 *** ERR get_message_queue_id_send %d\n", status)
250 250 }
251 251 status = get_message_queue_id_prc0( &queue_id_q_p0);
252 252 if (status != RTEMS_SUCCESSFUL)
253 253 {
254 254 PRINTF1("in PRC0 *** ERR get_message_queue_id_prc0 %d\n", status)
255 255 }
256 256
257 257 BOOT_PRINTF1("in PRC0 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
258 258
259 259 while(1){
260 260 status = rtems_message_queue_receive( queue_id_q_p0, incomingData, &size, //************************************
261 261 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
262 262
263 263 incomingMsg = (asm_msg*) incomingData;
264 264
265 265 ASM_patch( incomingMsg->norm->matrix, asm_f0_patched_norm );
266 266 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f0_patched_burst_sbm );
267 267
268 268 //****************
269 269 //****************
270 270 // BURST SBM1 SBM2
271 271 //****************
272 272 //****************
273 273 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F0 ) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F0 ) )
274 274 {
275 275 sid = getSID( incomingMsg->event );
276 276 // 1) compress the matrix for Basic Parameters calculation
277 277 ASM_compress_reorganize_and_divide( asm_f0_patched_burst_sbm, compressed_sm_sbm_f0,
278 278 nb_sm_before_f0.burst_sbm_bp1,
279 279 NB_BINS_COMPRESSED_SM_SBM_F0, NB_BINS_TO_AVERAGE_ASM_SBM_F0,
280 280 ASM_F0_INDICE_START);
281 281 // 2) compute the BP1 set
282 282 BP1_set( compressed_sm_sbm_f0, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp1.data );
283 283 // 3) send the BP1 set
284 284 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
285 285 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
286 286 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
287 287 BP_send( (char *) &packet_sbm_bp1, queue_id,
288 288 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0 + PACKET_LENGTH_DELTA,
289 289 sid);
290 290 // 4) compute the BP2 set if needed
291 291 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F0) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F0) )
292 292 {
293 293 // 1) compute the BP2 set
294 294 BP2_set( compressed_sm_sbm_f0, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp2.data );
295 295 // 2) send the BP2 set
296 296 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
297 297 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
298 298 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
299 299 BP_send( (char *) &packet_sbm_bp2, queue_id,
300 300 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0 + PACKET_LENGTH_DELTA,
301 301 sid);
302 302 }
303 303 }
304 304
305 305 //*****
306 306 //*****
307 307 // NORM
308 308 //*****
309 309 //*****
310 310 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F0)
311 311 {
312 312 // 1) compress the matrix for Basic Parameters calculation
313 313 ASM_compress_reorganize_and_divide( asm_f0_patched_norm, compressed_sm_norm_f0,
314 314 nb_sm_before_f0.norm_bp1,
315 315 NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0,
316 316 ASM_F0_INDICE_START );
317 317 // 2) compute the BP1 set
318 318 BP1_set( compressed_sm_norm_f0, k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp1.data );
319 319 // 3) send the BP1 set
320 320 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
321 321 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
322 322 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
323 323 BP_send( (char *) &packet_norm_bp1, queue_id,
324 324 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0 + PACKET_LENGTH_DELTA,
325 325 SID_NORM_BP1_F0 );
326 326 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F0)
327 327 {
328 328 // 1) compute the BP2 set using the same ASM as the one used for BP1
329 329 BP2_set( compressed_sm_norm_f0, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp2.data );
330 330 // 2) send the BP2 set
331 331 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
332 332 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
333 333 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
334 334 BP_send( (char *) &packet_norm_bp2, queue_id,
335 335 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0 + PACKET_LENGTH_DELTA,
336 336 SID_NORM_BP2_F0);
337 337 }
338 338 }
339 339
340 340 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F0)
341 341 {
342 342 // 1) reorganize the ASM and divide
343 343 ASM_reorganize_and_divide( asm_f0_patched_norm,
344 344 (float*) current_ring_node_to_send_asm_f0->buffer_address,
345 345 nb_sm_before_f0.norm_bp1 );
346 346 current_ring_node_to_send_asm_f0->coarseTime = incomingMsg->coarseTimeNORM;
347 347 current_ring_node_to_send_asm_f0->fineTime = incomingMsg->fineTimeNORM;
348 348 current_ring_node_to_send_asm_f0->sid = SID_NORM_ASM_F0;
349 349
350 350 // 3) send the spectral matrix packets
351 351 status = rtems_message_queue_send( queue_id, &current_ring_node_to_send_asm_f0, sizeof( ring_node* ) );
352 352 // change asm ring node
353 353 current_ring_node_to_send_asm_f0 = current_ring_node_to_send_asm_f0->next;
354 354 }
355
356 update_queue_max_count( queue_id_q_p0, &hk_lfr_q_p0_fifo_size_max );
357
355 358 }
356 359 }
357 360
358 361 //**********
359 362 // FUNCTIONS
360 363
361 364 void reset_nb_sm_f0( unsigned char lfrMode )
362 365 {
363 366 nb_sm_before_f0.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 96;
364 367 nb_sm_before_f0.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 96;
365 368 nb_sm_before_f0.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1]) * 96;
366 369 nb_sm_before_f0.sbm1_bp1 = parameter_dump_packet.sy_lfr_s1_bp_p0 * 24; // 0.25 s per digit
367 370 nb_sm_before_f0.sbm1_bp2 = parameter_dump_packet.sy_lfr_s1_bp_p1 * 96;
368 371 nb_sm_before_f0.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 96;
369 372 nb_sm_before_f0.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 96;
370 373 nb_sm_before_f0.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 96;
371 374 nb_sm_before_f0.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 96;
372 375
373 376 if (lfrMode == LFR_MODE_SBM1)
374 377 {
375 378 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm1_bp1;
376 379 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm1_bp2;
377 380 }
378 381 else if (lfrMode == LFR_MODE_SBM2)
379 382 {
380 383 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm2_bp1;
381 384 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm2_bp2;
382 385 }
383 386 else if (lfrMode == LFR_MODE_BURST)
384 387 {
385 388 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
386 389 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
387 390 }
388 391 else
389 392 {
390 393 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
391 394 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
392 395 }
393 396 }
394 397
395 398 void init_k_coefficients_f0( void )
396 399 {
397 400 init_k_coefficients( k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0 );
398 401 init_k_coefficients( k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_SBM_F0);
399 402 }
Show file before | Show file after | Hide comments
@@ -1,387 +1,389
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf1_prc1.h"
11 11
12 12 nb_sm_before_bp_asm_f1 nb_sm_before_f1;
13 13
14 14 extern ring_node sm_ring_f1[ ];
15 15
16 16 //***
17 17 // F1
18 18 ring_node_asm asm_ring_norm_f1 [ NB_RING_NODES_ASM_NORM_F1 ];
19 19 ring_node_asm asm_ring_burst_sbm_f1 [ NB_RING_NODES_ASM_BURST_SBM_F1 ];
20 20
21 21 ring_node ring_to_send_asm_f1 [ NB_RING_NODES_ASM_F1 ];
22 22 int buffer_asm_f1 [ NB_RING_NODES_ASM_F1 * TOTAL_SIZE_SM ];
23 23
24 24 float asm_f1_patched_norm [ TOTAL_SIZE_SM ];
25 25 float asm_f1_patched_burst_sbm [ TOTAL_SIZE_SM ];
26 26 float asm_f1_reorganized [ TOTAL_SIZE_SM ];
27 27
28 28 char asm_f1_char [ TOTAL_SIZE_SM * 2 ];
29 29 float compressed_sm_norm_f1[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F1];
30 30 float compressed_sm_sbm_f1 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 ];
31 31
32 32 float k_coeff_intercalib_f1_norm[ NB_BINS_COMPRESSED_SM_F1 * NB_K_COEFF_PER_BIN ]; // 13 * 32 = 416
33 33 float k_coeff_intercalib_f1_sbm[ NB_BINS_COMPRESSED_SM_SBM_F1 * NB_K_COEFF_PER_BIN ]; // 26 * 32 = 832
34 34
35 35 //************
36 36 // RTEMS TASKS
37 37
38 38 rtems_task avf1_task( rtems_task_argument lfrRequestedMode )
39 39 {
40 40 int i;
41 41
42 42 rtems_event_set event_out;
43 43 rtems_status_code status;
44 44 rtems_id queue_id_prc1;
45 45 asm_msg msgForMATR;
46 46 ring_node *nodeForAveraging;
47 47 ring_node *ring_node_tab[NB_SM_BEFORE_AVF0];
48 48 ring_node_asm *current_ring_node_asm_burst_sbm_f1;
49 49 ring_node_asm *current_ring_node_asm_norm_f1;
50 50
51 51 unsigned int nb_norm_bp1;
52 52 unsigned int nb_norm_bp2;
53 53 unsigned int nb_norm_asm;
54 54 unsigned int nb_sbm_bp1;
55 55 unsigned int nb_sbm_bp2;
56 56
57 57 nb_norm_bp1 = 0;
58 58 nb_norm_bp2 = 0;
59 59 nb_norm_asm = 0;
60 60 nb_sbm_bp1 = 0;
61 61 nb_sbm_bp2 = 0;
62 62
63 63 reset_nb_sm_f1( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
64 64 ASM_generic_init_ring( asm_ring_norm_f1, NB_RING_NODES_ASM_NORM_F1 );
65 65 ASM_generic_init_ring( asm_ring_burst_sbm_f1, NB_RING_NODES_ASM_BURST_SBM_F1 );
66 66 current_ring_node_asm_norm_f1 = asm_ring_norm_f1;
67 67 current_ring_node_asm_burst_sbm_f1 = asm_ring_burst_sbm_f1;
68 68
69 69 BOOT_PRINTF1("in AVF1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
70 70
71 71 status = get_message_queue_id_prc1( &queue_id_prc1 );
72 72 if (status != RTEMS_SUCCESSFUL)
73 73 {
74 74 PRINTF1("in AVF1 *** ERR get_message_queue_id_prc1 %d\n", status)
75 75 }
76 76
77 77 while(1){
78 78 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
79 79
80 80 //****************************************
81 81 // initialize the mesage for the MATR task
82 82 msgForMATR.norm = current_ring_node_asm_norm_f1;
83 83 msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f1;
84 84 msgForMATR.event = 0x00; // this composite event will be sent to the PRC1 task
85 85 //
86 86 //****************************************
87 87
88 88 nodeForAveraging = getRingNodeForAveraging( 1 );
89 89
90 90 ring_node_tab[NB_SM_BEFORE_AVF1-1] = nodeForAveraging;
91 91 for ( i = 2; i < (NB_SM_BEFORE_AVF1+1); i++ )
92 92 {
93 93 nodeForAveraging = nodeForAveraging->previous;
94 94 ring_node_tab[NB_SM_BEFORE_AVF1-i] = nodeForAveraging;
95 95 }
96 96
97 97 // compute the average and store it in the averaged_sm_f1 buffer
98 98 SM_average( current_ring_node_asm_norm_f1->matrix,
99 99 current_ring_node_asm_burst_sbm_f1->matrix,
100 100 ring_node_tab,
101 101 nb_norm_bp1, nb_sbm_bp1,
102 102 &msgForMATR );
103 103
104 104 // update nb_average
105 105 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF1;
106 106 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF1;
107 107 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF1;
108 108 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF1;
109 109 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF1;
110 110
111 111 if (nb_sbm_bp1 == nb_sm_before_f1.burst_sbm_bp1)
112 112 {
113 113 nb_sbm_bp1 = 0;
114 114 // set another ring for the ASM storage
115 115 current_ring_node_asm_burst_sbm_f1 = current_ring_node_asm_burst_sbm_f1->next;
116 116 if ( lfrCurrentMode == LFR_MODE_BURST )
117 117 {
118 118 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP1_F1;
119 119 }
120 120 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
121 121 {
122 122 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP1_F1;
123 123 }
124 124 }
125 125
126 126 if (nb_sbm_bp2 == nb_sm_before_f1.burst_sbm_bp2)
127 127 {
128 128 nb_sbm_bp2 = 0;
129 129 if ( lfrCurrentMode == LFR_MODE_BURST )
130 130 {
131 131 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP2_F1;
132 132 }
133 133 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
134 134 {
135 135 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP2_F1;
136 136 }
137 137 }
138 138
139 139 if (nb_norm_bp1 == nb_sm_before_f1.norm_bp1)
140 140 {
141 141 nb_norm_bp1 = 0;
142 142 // set another ring for the ASM storage
143 143 current_ring_node_asm_norm_f1 = current_ring_node_asm_norm_f1->next;
144 144 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
145 145 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
146 146 {
147 147 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F1;
148 148 }
149 149 }
150 150
151 151 if (nb_norm_bp2 == nb_sm_before_f1.norm_bp2)
152 152 {
153 153 nb_norm_bp2 = 0;
154 154 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
155 155 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
156 156 {
157 157 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F1;
158 158 }
159 159 }
160 160
161 161 if (nb_norm_asm == nb_sm_before_f1.norm_asm)
162 162 {
163 163 nb_norm_asm = 0;
164 164 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
165 165 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
166 166 {
167 167 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F1;
168 168 }
169 169 }
170 170
171 171 //*************************
172 172 // send the message to MATR
173 173 if (msgForMATR.event != 0x00)
174 174 {
175 175 status = rtems_message_queue_send( queue_id_prc1, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC1);
176 176 }
177 177
178 178 if (status != RTEMS_SUCCESSFUL) {
179 179 printf("in AVF1 *** Error sending message to PRC1, code %d\n", status);
180 180 }
181 181 }
182 182 }
183 183
184 184 rtems_task prc1_task( rtems_task_argument lfrRequestedMode )
185 185 {
186 186 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
187 187 size_t size; // size of the incoming TC packet
188 188 asm_msg *incomingMsg;
189 189 //
190 190 unsigned char sid;
191 191 rtems_status_code status;
192 192 rtems_id queue_id_send;
193 193 rtems_id queue_id_q_p1;
194 194 bp_packet_with_spare packet_norm_bp1;
195 195 bp_packet packet_norm_bp2;
196 196 bp_packet packet_sbm_bp1;
197 197 bp_packet packet_sbm_bp2;
198 198 ring_node *current_ring_node_to_send_asm_f1;
199 199
200 200 unsigned long long int localTime;
201 201
202 202 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
203 203 init_ring( ring_to_send_asm_f1, NB_RING_NODES_ASM_F1, (volatile int*) buffer_asm_f1, TOTAL_SIZE_SM );
204 204 current_ring_node_to_send_asm_f1 = ring_to_send_asm_f1;
205 205
206 206 //*************
207 207 // NORM headers
208 208 BP_init_header_with_spare( &packet_norm_bp1,
209 209 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F1,
210 210 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1, NB_BINS_COMPRESSED_SM_F1 );
211 211 BP_init_header( &packet_norm_bp2,
212 212 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F1,
213 213 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1, NB_BINS_COMPRESSED_SM_F1);
214 214
215 215 //***********************
216 216 // BURST and SBM2 headers
217 217 if ( lfrRequestedMode == LFR_MODE_BURST )
218 218 {
219 219 BP_init_header( &packet_sbm_bp1,
220 220 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F1,
221 221 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
222 222 BP_init_header( &packet_sbm_bp2,
223 223 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F1,
224 224 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
225 225 }
226 226 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
227 227 {
228 228 BP_init_header( &packet_sbm_bp1,
229 229 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F1,
230 230 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
231 231 BP_init_header( &packet_sbm_bp2,
232 232 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F1,
233 233 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
234 234 }
235 235 else
236 236 {
237 237 PRINTF1("in PRC1 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
238 238 }
239 239
240 240 status = get_message_queue_id_send( &queue_id_send );
241 241 if (status != RTEMS_SUCCESSFUL)
242 242 {
243 243 PRINTF1("in PRC1 *** ERR get_message_queue_id_send %d\n", status)
244 244 }
245 245 status = get_message_queue_id_prc1( &queue_id_q_p1);
246 246 if (status != RTEMS_SUCCESSFUL)
247 247 {
248 248 PRINTF1("in PRC1 *** ERR get_message_queue_id_prc1 %d\n", status)
249 249 }
250 250
251 251 BOOT_PRINTF1("in PRC1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
252 252
253 253 while(1){
254 254 status = rtems_message_queue_receive( queue_id_q_p1, incomingData, &size, //************************************
255 255 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
256 256
257 257 incomingMsg = (asm_msg*) incomingData;
258 258
259 259 ASM_patch( incomingMsg->norm->matrix, asm_f1_patched_norm );
260 260 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f1_patched_burst_sbm );
261 261
262 262 localTime = getTimeAsUnsignedLongLongInt( );
263 263 //***********
264 264 //***********
265 265 // BURST SBM2
266 266 //***********
267 267 //***********
268 268 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F1) )
269 269 {
270 270 sid = getSID( incomingMsg->event );
271 271 // 1) compress the matrix for Basic Parameters calculation
272 272 ASM_compress_reorganize_and_divide( asm_f1_patched_burst_sbm, compressed_sm_sbm_f1,
273 273 nb_sm_before_f1.burst_sbm_bp1,
274 274 NB_BINS_COMPRESSED_SM_SBM_F1, NB_BINS_TO_AVERAGE_ASM_SBM_F1,
275 275 ASM_F1_INDICE_START);
276 276 // 2) compute the BP1 set
277 277 BP1_set( compressed_sm_sbm_f1, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp1.data );
278 278 // 3) send the BP1 set
279 279 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
280 280 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
281 281 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
282 282 BP_send( (char *) &packet_sbm_bp1, queue_id_send,
283 283 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 + PACKET_LENGTH_DELTA,
284 284 sid );
285 285 // 4) compute the BP2 set if needed
286 286 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F1) )
287 287 {
288 288 // 1) compute the BP2 set
289 289 BP2_set( compressed_sm_sbm_f1, NB_BINS_COMPRESSED_SM_SBM_F1, packet_norm_bp2.data );
290 290 // 2) send the BP2 set
291 291 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
292 292 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
293 293 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
294 294 BP_send( (char *) &packet_sbm_bp2, queue_id_send,
295 295 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1 + PACKET_LENGTH_DELTA,
296 296 sid );
297 297 }
298 298 }
299 299
300 300 //*****
301 301 //*****
302 302 // NORM
303 303 //*****
304 304 //*****
305 305 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F1)
306 306 {
307 307 // 1) compress the matrix for Basic Parameters calculation
308 308 ASM_compress_reorganize_and_divide( asm_f1_patched_norm, compressed_sm_norm_f1,
309 309 nb_sm_before_f1.norm_bp1,
310 310 NB_BINS_COMPRESSED_SM_F1, NB_BINS_TO_AVERAGE_ASM_F1,
311 311 ASM_F1_INDICE_START );
312 312 // 2) compute the BP1 set
313 313 BP1_set( compressed_sm_norm_f1, k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp1.data );
314 314 // 3) send the BP1 set
315 315 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
316 316 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
317 317 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
318 318 BP_send( (char *) &packet_norm_bp1, queue_id_send,
319 319 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 + PACKET_LENGTH_DELTA,
320 320 SID_NORM_BP1_F1 );
321 321 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F1)
322 322 {
323 323 // 1) compute the BP2 set
324 324 BP2_set( compressed_sm_norm_f1, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp2.data );
325 325 // 2) send the BP2 set
326 326 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
327 327 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
328 328 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
329 329 BP_send( (char *) &packet_norm_bp2, queue_id_send,
330 330 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 + PACKET_LENGTH_DELTA,
331 331 SID_NORM_BP2_F1 );
332 332 }
333 333 }
334 334
335 335 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F1)
336 336 {
337 337 // 1) reorganize the ASM and divide
338 338 ASM_reorganize_and_divide( asm_f1_patched_norm,
339 339 (float*) current_ring_node_to_send_asm_f1->buffer_address,
340 340 nb_sm_before_f1.norm_bp1 );
341 341 current_ring_node_to_send_asm_f1->coarseTime = incomingMsg->coarseTimeNORM;
342 342 current_ring_node_to_send_asm_f1->fineTime = incomingMsg->fineTimeNORM;
343 343 current_ring_node_to_send_asm_f1->sid = SID_NORM_ASM_F1;
344 344 // 3) send the spectral matrix packets
345 345 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f1, sizeof( ring_node* ) );
346 346 // change asm ring node
347 347 current_ring_node_to_send_asm_f1 = current_ring_node_to_send_asm_f1->next;
348 348 }
349 349
350 update_queue_max_count( queue_id_q_p1, &hk_lfr_q_p1_fifo_size_max );
351
350 352 }
351 353 }
352 354
353 355 //**********
354 356 // FUNCTIONS
355 357
356 358 void reset_nb_sm_f1( unsigned char lfrMode )
357 359 {
358 360 nb_sm_before_f1.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 16;
359 361 nb_sm_before_f1.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 16;
360 362 nb_sm_before_f1.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1]) * 16;
361 363 nb_sm_before_f1.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 16;
362 364 nb_sm_before_f1.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 16;
363 365 nb_sm_before_f1.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 16;
364 366 nb_sm_before_f1.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 16;
365 367
366 368 if (lfrMode == LFR_MODE_SBM2)
367 369 {
368 370 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.sbm2_bp1;
369 371 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.sbm2_bp2;
370 372 }
371 373 else if (lfrMode == LFR_MODE_BURST)
372 374 {
373 375 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
374 376 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
375 377 }
376 378 else
377 379 {
378 380 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
379 381 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
380 382 }
381 383 }
382 384
383 385 void init_k_coefficients_f1( void )
384 386 {
385 387 init_k_coefficients( k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1 );
386 388 init_k_coefficients( k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1);
387 389 }
Show file before | Show file after | Hide comments
@@ -1,289 +1,291
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf2_prc2.h"
11 11
12 12 nb_sm_before_bp_asm_f2 nb_sm_before_f2;
13 13
14 14 extern ring_node sm_ring_f2[ ];
15 15
16 16 //***
17 17 // F2
18 18 ring_node_asm asm_ring_norm_f2 [ NB_RING_NODES_ASM_NORM_F2 ];
19 19
20 20 ring_node ring_to_send_asm_f2 [ NB_RING_NODES_ASM_F2 ];
21 21 int buffer_asm_f2 [ NB_RING_NODES_ASM_F2 * TOTAL_SIZE_SM ];
22 22
23 23 float asm_f2_patched_norm [ TOTAL_SIZE_SM ];
24 24 float asm_f2_reorganized [ TOTAL_SIZE_SM ];
25 25
26 26 char asm_f2_char [ TOTAL_SIZE_SM * 2 ];
27 27 float compressed_sm_norm_f2[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F2];
28 28
29 29 float k_coeff_intercalib_f2[ NB_BINS_COMPRESSED_SM_F2 * NB_K_COEFF_PER_BIN ]; // 12 * 32 = 384
30 30
31 31 //************
32 32 // RTEMS TASKS
33 33
34 34 //***
35 35 // F2
36 36 rtems_task avf2_task( rtems_task_argument argument )
37 37 {
38 38 rtems_event_set event_out;
39 39 rtems_status_code status;
40 40 rtems_id queue_id_prc2;
41 41 asm_msg msgForMATR;
42 42 ring_node *nodeForAveraging;
43 43 ring_node_asm *current_ring_node_asm_norm_f2;
44 44
45 45 unsigned int nb_norm_bp1;
46 46 unsigned int nb_norm_bp2;
47 47 unsigned int nb_norm_asm;
48 48
49 49 nb_norm_bp1 = 0;
50 50 nb_norm_bp2 = 0;
51 51 nb_norm_asm = 0;
52 52
53 53 reset_nb_sm_f2( ); // reset the sm counters that drive the BP and ASM computations / transmissions
54 54 ASM_generic_init_ring( asm_ring_norm_f2, NB_RING_NODES_ASM_NORM_F2 );
55 55 current_ring_node_asm_norm_f2 = asm_ring_norm_f2;
56 56
57 57 BOOT_PRINTF("in AVF2 ***\n")
58 58
59 59 status = get_message_queue_id_prc2( &queue_id_prc2 );
60 60 if (status != RTEMS_SUCCESSFUL)
61 61 {
62 62 PRINTF1("in AVF2 *** ERR get_message_queue_id_prc2 %d\n", status)
63 63 }
64 64
65 65 while(1){
66 66 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
67 67
68 68 //****************************************
69 69 // initialize the mesage for the MATR task
70 70 msgForMATR.norm = current_ring_node_asm_norm_f2;
71 71 msgForMATR.burst_sbm = NULL;
72 72 msgForMATR.event = 0x00; // this composite event will be sent to the PRC2 task
73 73 //
74 74 //****************************************
75 75
76 76 nodeForAveraging = getRingNodeForAveraging( 2 );
77 77
78 78 // printf(" **0** %x . %x", sm_ring_f2[0].coarseTime, sm_ring_f2[0].fineTime);
79 79 // printf(" **1** %x . %x", sm_ring_f2[1].coarseTime, sm_ring_f2[1].fineTime);
80 80 // printf(" **2** %x . %x", sm_ring_f2[2].coarseTime, sm_ring_f2[2].fineTime);
81 81 // printf(" **3** %x . %x", sm_ring_f2[3].coarseTime, sm_ring_f2[3].fineTime);
82 82 // printf(" **4** %x . %x", sm_ring_f2[4].coarseTime, sm_ring_f2[4].fineTime);
83 83 // printf(" **5** %x . %x", sm_ring_f2[5].coarseTime, sm_ring_f2[5].fineTime);
84 84 // printf(" **6** %x . %x", sm_ring_f2[6].coarseTime, sm_ring_f2[6].fineTime);
85 85 // printf(" **7** %x . %x", sm_ring_f2[7].coarseTime, sm_ring_f2[7].fineTime);
86 86 // printf(" **8** %x . %x", sm_ring_f2[8].coarseTime, sm_ring_f2[8].fineTime);
87 87 // printf(" **9** %x . %x", sm_ring_f2[9].coarseTime, sm_ring_f2[9].fineTime);
88 88 // printf(" **10** %x . %x\n", sm_ring_f2[10].coarseTime, sm_ring_f2[10].fineTime);
89 89
90 90 // compute the average and store it in the averaged_sm_f2 buffer
91 91 SM_average_f2( current_ring_node_asm_norm_f2->matrix,
92 92 nodeForAveraging,
93 93 nb_norm_bp1,
94 94 &msgForMATR );
95 95
96 96 // update nb_average
97 97 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF2;
98 98 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF2;
99 99 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF2;
100 100
101 101 if (nb_norm_bp1 == nb_sm_before_f2.norm_bp1)
102 102 {
103 103 nb_norm_bp1 = 0;
104 104 // set another ring for the ASM storage
105 105 current_ring_node_asm_norm_f2 = current_ring_node_asm_norm_f2->next;
106 106 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
107 107 || (lfrCurrentMode == LFR_MODE_SBM2) )
108 108 {
109 109 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F2;
110 110 }
111 111 }
112 112
113 113 if (nb_norm_bp2 == nb_sm_before_f2.norm_bp2)
114 114 {
115 115 nb_norm_bp2 = 0;
116 116 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
117 117 || (lfrCurrentMode == LFR_MODE_SBM2) )
118 118 {
119 119 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F2;
120 120 }
121 121 }
122 122
123 123 if (nb_norm_asm == nb_sm_before_f2.norm_asm)
124 124 {
125 125 nb_norm_asm = 0;
126 126 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
127 127 || (lfrCurrentMode == LFR_MODE_SBM2) )
128 128 {
129 129 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F2;
130 130 }
131 131 }
132 132
133 133 //*************************
134 134 // send the message to MATR
135 135 if (msgForMATR.event != 0x00)
136 136 {
137 137 status = rtems_message_queue_send( queue_id_prc2, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC2);
138 138 }
139 139
140 140 if (status != RTEMS_SUCCESSFUL) {
141 141 printf("in AVF2 *** Error sending message to MATR, code %d\n", status);
142 142 }
143 143 }
144 144 }
145 145
146 146 rtems_task prc2_task( rtems_task_argument argument )
147 147 {
148 148 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
149 149 size_t size; // size of the incoming TC packet
150 150 asm_msg *incomingMsg;
151 151 //
152 152 rtems_status_code status;
153 153 rtems_id queue_id_send;
154 154 rtems_id queue_id_q_p2;
155 155 bp_packet packet_norm_bp1;
156 156 bp_packet packet_norm_bp2;
157 157 ring_node *current_ring_node_to_send_asm_f2;
158 158
159 159 unsigned long long int localTime;
160 160
161 161 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
162 162 init_ring( ring_to_send_asm_f2, NB_RING_NODES_ASM_F2, (volatile int*) buffer_asm_f2, TOTAL_SIZE_SM );
163 163 current_ring_node_to_send_asm_f2 = ring_to_send_asm_f2;
164 164
165 165 //*************
166 166 // NORM headers
167 167 BP_init_header( &packet_norm_bp1,
168 168 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F2,
169 169 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2, NB_BINS_COMPRESSED_SM_F2 );
170 170 BP_init_header( &packet_norm_bp2,
171 171 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F2,
172 172 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2, NB_BINS_COMPRESSED_SM_F2 );
173 173
174 174 status = get_message_queue_id_send( &queue_id_send );
175 175 if (status != RTEMS_SUCCESSFUL)
176 176 {
177 177 PRINTF1("in PRC2 *** ERR get_message_queue_id_send %d\n", status)
178 178 }
179 179 status = get_message_queue_id_prc2( &queue_id_q_p2);
180 180 if (status != RTEMS_SUCCESSFUL)
181 181 {
182 182 PRINTF1("in PRC2 *** ERR get_message_queue_id_prc2 %d\n", status)
183 183 }
184 184
185 185 BOOT_PRINTF("in PRC2 ***\n")
186 186
187 187 while(1){
188 188 status = rtems_message_queue_receive( queue_id_q_p2, incomingData, &size, //************************************
189 189 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF2
190 190
191 191 incomingMsg = (asm_msg*) incomingData;
192 192
193 193 ASM_patch( incomingMsg->norm->matrix, asm_f2_patched_norm );
194 194
195 195 localTime = getTimeAsUnsignedLongLongInt( );
196 196
197 197 //*****
198 198 //*****
199 199 // NORM
200 200 //*****
201 201 //*****
202 202 // 1) compress the matrix for Basic Parameters calculation
203 203 ASM_compress_reorganize_and_divide( asm_f2_patched_norm, compressed_sm_norm_f2,
204 204 nb_sm_before_f2.norm_bp1,
205 205 NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2,
206 206 ASM_F2_INDICE_START );
207 207 // BP1_F2
208 208 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F2)
209 209 {
210 210 // 1) compute the BP1 set
211 211 BP1_set( compressed_sm_norm_f2, k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp1.data );
212 212 // 2) send the BP1 set
213 213 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
214 214 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
215 215 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
216 216 BP_send( (char *) &packet_norm_bp1, queue_id_send,
217 217 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2 + PACKET_LENGTH_DELTA,
218 218 SID_NORM_BP1_F2 );
219 219 }
220 220 // BP2_F2
221 221 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F2)
222 222 {
223 223 // 1) compute the BP2 set
224 224 BP2_set( compressed_sm_norm_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp2.data );
225 225 // 2) send the BP2 set
226 226 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
227 227 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
228 228 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
229 229 BP_send( (char *) &packet_norm_bp2, queue_id_send,
230 230 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2 + PACKET_LENGTH_DELTA,
231 231 SID_NORM_BP2_F2 );
232 232 }
233 233
234 234 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F2)
235 235 {
236 236 // 1) reorganize the ASM and divide
237 237 ASM_reorganize_and_divide( asm_f2_patched_norm,
238 238 (float*) current_ring_node_to_send_asm_f2->buffer_address,
239 239 nb_sm_before_f2.norm_bp1 );
240 240 current_ring_node_to_send_asm_f2->coarseTime = incomingMsg->coarseTimeNORM;
241 241 current_ring_node_to_send_asm_f2->fineTime = incomingMsg->fineTimeNORM;
242 242 current_ring_node_to_send_asm_f2->sid = SID_NORM_ASM_F2;
243 243 // 3) send the spectral matrix packets
244 244 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f2, sizeof( ring_node* ) );
245 245 // change asm ring node
246 246 current_ring_node_to_send_asm_f2 = current_ring_node_to_send_asm_f2->next;
247 247 }
248 248
249 update_queue_max_count( queue_id_q_p2, &hk_lfr_q_p2_fifo_size_max );
250
249 251 }
250 252 }
251 253
252 254 //**********
253 255 // FUNCTIONS
254 256
255 257 void reset_nb_sm_f2( void )
256 258 {
257 259 nb_sm_before_f2.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0;
258 260 nb_sm_before_f2.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1;
259 261 nb_sm_before_f2.norm_asm = parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1];
260 262 }
261 263
262 264 void SM_average_f2( float *averaged_spec_mat_f2,
263 265 ring_node *ring_node,
264 266 unsigned int nbAverageNormF2,
265 267 asm_msg *msgForMATR )
266 268 {
267 269 float sum;
268 270 unsigned int i;
269 271
270 272 for(i=0; i<TOTAL_SIZE_SM; i++)
271 273 {
272 274 sum = ( (int *) (ring_node->buffer_address) ) [ i ];
273 275 if ( (nbAverageNormF2 == 0) )
274 276 {
275 277 averaged_spec_mat_f2[ i ] = sum;
276 278 msgForMATR->coarseTimeNORM = ring_node->coarseTime;
277 279 msgForMATR->fineTimeNORM = ring_node->fineTime;
278 280 }
279 281 else
280 282 {
281 283 averaged_spec_mat_f2[ i ] = ( averaged_spec_mat_f2[ i ] + sum );
282 284 }
283 285 }
284 286 }
285 287
286 288 void init_k_coefficients_f2( void )
287 289 {
288 290 init_k_coefficients( k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2);
289 291 }
Show file before | Show file after | Hide comments
@@ -1,651 +1,652
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "fsw_processing.h"
11 11 #include "fsw_processing_globals.c"
12 #include "fsw_init.h"
12 13
13 14 unsigned int nb_sm_f0;
14 15 unsigned int nb_sm_f0_aux_f1;
15 16 unsigned int nb_sm_f1;
16 17 unsigned int nb_sm_f0_aux_f2;
17 18
18 19 //************************
19 20 // spectral matrices rings
20 21 ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ];
21 22 ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ];
22 23 ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ];
23 24 ring_node *current_ring_node_sm_f0;
24 25 ring_node *current_ring_node_sm_f1;
25 26 ring_node *current_ring_node_sm_f2;
26 27 ring_node *ring_node_for_averaging_sm_f0;
27 28 ring_node *ring_node_for_averaging_sm_f1;
28 29 ring_node *ring_node_for_averaging_sm_f2;
29 30
30 31 //
31 32 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
32 33 {
33 34 ring_node *node;
34 35
35 36 node = NULL;
36 37 switch ( frequencyChannel ) {
37 38 case 0:
38 39 node = ring_node_for_averaging_sm_f0;
39 40 break;
40 41 case 1:
41 42 node = ring_node_for_averaging_sm_f1;
42 43 break;
43 44 case 2:
44 45 node = ring_node_for_averaging_sm_f2;
45 46 break;
46 47 default:
47 48 break;
48 49 }
49 50
50 51 return node;
51 52 }
52 53
53 54 //***********************************************************
54 55 // Interrupt Service Routine for spectral matrices processing
55 56
56 57 void spectral_matrices_isr_f0( unsigned char statusReg )
57 58 {
58 59 unsigned char status;
59 60 rtems_status_code status_code;
60 61 ring_node *full_ring_node;
61 62
62 63 status = statusReg & 0x03; // [0011] get the status_ready_matrix_f0_x bits
63 64
64 65 switch(status)
65 66 {
66 67 case 0:
67 68 break;
68 69 case 3:
69 70 // UNEXPECTED VALUE
70 71 spectral_matrix_regs->status = 0x03; // [0011]
71 72 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
72 73 break;
73 74 case 1:
74 75 full_ring_node = current_ring_node_sm_f0->previous;
75 76 full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
76 77 full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time;
77 78 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
78 79 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
79 80 // if there are enough ring nodes ready, wake up an AVFx task
80 81 nb_sm_f0 = nb_sm_f0 + 1;
81 82 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
82 83 {
83 84 ring_node_for_averaging_sm_f0 = full_ring_node;
84 85 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
85 86 {
86 87 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
87 88 }
88 89 nb_sm_f0 = 0;
89 90 }
90 91 spectral_matrix_regs->status = 0x01; // [0000 0001]
91 92 break;
92 93 case 2:
93 94 full_ring_node = current_ring_node_sm_f0->previous;
94 95 full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
95 96 full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time;
96 97 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
97 98 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
98 99 // if there are enough ring nodes ready, wake up an AVFx task
99 100 nb_sm_f0 = nb_sm_f0 + 1;
100 101 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
101 102 {
102 103 ring_node_for_averaging_sm_f0 = full_ring_node;
103 104 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
104 105 {
105 106 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
106 107 }
107 108 nb_sm_f0 = 0;
108 109 }
109 110 spectral_matrix_regs->status = 0x02; // [0000 0010]
110 111 break;
111 112 }
112 113 }
113 114
114 115 void spectral_matrices_isr_f1( unsigned char statusReg )
115 116 {
116 117 rtems_status_code status_code;
117 118 unsigned char status;
118 119 ring_node *full_ring_node;
119 120
120 121 status = (statusReg & 0x0c) >> 2; // [1100] get the status_ready_matrix_f0_x bits
121 122
122 123 switch(status)
123 124 {
124 125 case 0:
125 126 break;
126 127 case 3:
127 128 // UNEXPECTED VALUE
128 129 spectral_matrix_regs->status = 0xc0; // [1100]
129 130 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
130 131 break;
131 132 case 1:
132 133 full_ring_node = current_ring_node_sm_f1->previous;
133 134 full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
134 135 full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
135 136 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
136 137 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
137 138 // if there are enough ring nodes ready, wake up an AVFx task
138 139 nb_sm_f1 = nb_sm_f1 + 1;
139 140 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
140 141 {
141 142 ring_node_for_averaging_sm_f1 = full_ring_node;
142 143 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
143 144 {
144 145 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
145 146 }
146 147 nb_sm_f1 = 0;
147 148 }
148 149 spectral_matrix_regs->status = 0x04; // [0000 0100]
149 150 break;
150 151 case 2:
151 152 full_ring_node = current_ring_node_sm_f1->previous;
152 153 full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
153 154 full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
154 155 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
155 156 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
156 157 // if there are enough ring nodes ready, wake up an AVFx task
157 158 nb_sm_f1 = nb_sm_f1 + 1;
158 159 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
159 160 {
160 161 ring_node_for_averaging_sm_f1 = full_ring_node;
161 162 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
162 163 {
163 164 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
164 165 }
165 166 nb_sm_f1 = 0;
166 167 }
167 168 spectral_matrix_regs->status = 0x08; // [1000 0000]
168 169 break;
169 170 }
170 171 }
171 172
172 173 void spectral_matrices_isr_f2( unsigned char statusReg )
173 174 {
174 175 unsigned char status;
175 176 rtems_status_code status_code;
176 177
177 178 status = (statusReg & 0x30) >> 4; // [0011 0000] get the status_ready_matrix_f0_x bits
178 179
179 180 switch(status)
180 181 {
181 182 case 0:
182 183 break;
183 184 case 3:
184 185 // UNEXPECTED VALUE
185 186 spectral_matrix_regs->status = 0x30; // [0011 0000]
186 187 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
187 188 break;
188 189 case 1:
189 190 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
190 191 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
191 192 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
192 193 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
193 194 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
194 195 spectral_matrix_regs->status = 0x10; // [0001 0000]
195 196 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
196 197 {
197 198 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
198 199 }
199 200 break;
200 201 case 2:
201 202 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
202 203 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
203 204 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
204 205 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
205 206 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
206 207 spectral_matrix_regs->status = 0x20; // [0010 0000]
207 208 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
208 209 {
209 210 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
210 211 }
211 212 break;
212 213 }
213 214 }
214 215
215 216 void spectral_matrix_isr_error_handler( unsigned char statusReg )
216 217 {
217 218 rtems_status_code status_code;
218 219
219 220 if (statusReg & 0x7c0) // [0111 1100 0000]
220 221 {
221 222 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
222 223 }
223 224
224 225 spectral_matrix_regs->status = spectral_matrix_regs->status & 0x7c0;
225 226 }
226 227
227 228 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
228 229 {
229 230 // STATUS REGISTER
230 231 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
231 232 // 10 9 8
232 233 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
233 234 // 7 6 5 4 3 2 1 0
234 235
235 236 unsigned char statusReg;
236 237
237 238 statusReg = spectral_matrix_regs->status;
238 239
239 240 spectral_matrices_isr_f0( statusReg );
240 241
241 242 spectral_matrices_isr_f1( statusReg );
242 243
243 244 spectral_matrices_isr_f2( statusReg );
244 245
245 246 spectral_matrix_isr_error_handler( statusReg );
246 247 }
247 248
248 249 rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector )
249 250 {
250 251 rtems_status_code status_code;
251 252
252 253 //***
253 254 // F0
254 255 nb_sm_f0 = nb_sm_f0 + 1;
255 256 if (nb_sm_f0 == NB_SM_BEFORE_AVF0 )
256 257 {
257 258 ring_node_for_averaging_sm_f0 = current_ring_node_sm_f0;
258 259 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
259 260 {
260 261 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
261 262 }
262 263 nb_sm_f0 = 0;
263 264 }
264 265
265 266 //***
266 267 // F1
267 268 nb_sm_f0_aux_f1 = nb_sm_f0_aux_f1 + 1;
268 269 if (nb_sm_f0_aux_f1 == 6)
269 270 {
270 271 nb_sm_f0_aux_f1 = 0;
271 272 nb_sm_f1 = nb_sm_f1 + 1;
272 273 }
273 274 if (nb_sm_f1 == NB_SM_BEFORE_AVF1 )
274 275 {
275 276 ring_node_for_averaging_sm_f1 = current_ring_node_sm_f1;
276 277 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
277 278 {
278 279 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
279 280 }
280 281 nb_sm_f1 = 0;
281 282 }
282 283
283 284 //***
284 285 // F2
285 286 nb_sm_f0_aux_f2 = nb_sm_f0_aux_f2 + 1;
286 287 if (nb_sm_f0_aux_f2 == 96)
287 288 {
288 289 nb_sm_f0_aux_f2 = 0;
289 290 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2;
290 291 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
291 292 {
292 293 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
293 294 }
294 295 }
295 296 }
296 297
297 298 //******************
298 299 // Spectral Matrices
299 300
300 301 void reset_nb_sm( void )
301 302 {
302 303 nb_sm_f0 = 0;
303 304 nb_sm_f0_aux_f1 = 0;
304 305 nb_sm_f0_aux_f2 = 0;
305 306
306 307 nb_sm_f1 = 0;
307 308 }
308 309
309 310 void SM_init_rings( void )
310 311 {
311 312 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
312 313 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
313 314 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
314 315
315 316 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
316 317 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
317 318 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
318 319 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
319 320 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
320 321 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
321 322 }
322 323
323 324 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
324 325 {
325 326 unsigned char i;
326 327
327 328 ring[ nbNodes - 1 ].next
328 329 = (ring_node_asm*) &ring[ 0 ];
329 330
330 331 for(i=0; i<nbNodes-1; i++)
331 332 {
332 333 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
333 334 }
334 335 }
335 336
336 337 void SM_reset_current_ring_nodes( void )
337 338 {
338 339 current_ring_node_sm_f0 = sm_ring_f0[0].next;
339 340 current_ring_node_sm_f1 = sm_ring_f1[0].next;
340 341 current_ring_node_sm_f2 = sm_ring_f2[0].next;
341 342
342 343 ring_node_for_averaging_sm_f0 = NULL;
343 344 ring_node_for_averaging_sm_f1 = NULL;
344 345 ring_node_for_averaging_sm_f2 = NULL;
345 346 }
346 347
347 348 //*****************
348 349 // Basic Parameters
349 350
350 351 void BP_init_header( bp_packet *packet,
351 352 unsigned int apid, unsigned char sid,
352 353 unsigned int packetLength, unsigned char blkNr )
353 354 {
354 355 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
355 356 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
356 357 packet->reserved = 0x00;
357 358 packet->userApplication = CCSDS_USER_APP;
358 359 packet->packetID[0] = (unsigned char) (apid >> 8);
359 360 packet->packetID[1] = (unsigned char) (apid);
360 361 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
361 362 packet->packetSequenceControl[1] = 0x00;
362 363 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
363 364 packet->packetLength[1] = (unsigned char) (packetLength);
364 365 // DATA FIELD HEADER
365 366 packet->spare1_pusVersion_spare2 = 0x10;
366 367 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
367 368 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
368 369 packet->destinationID = TM_DESTINATION_ID_GROUND;
369 370 packet->time[0] = 0x00;
370 371 packet->time[1] = 0x00;
371 372 packet->time[2] = 0x00;
372 373 packet->time[3] = 0x00;
373 374 packet->time[4] = 0x00;
374 375 packet->time[5] = 0x00;
375 376 // AUXILIARY DATA HEADER
376 377 packet->sid = sid;
377 378 packet->biaStatusInfo = 0x00;
378 379 packet->acquisitionTime[0] = 0x00;
379 380 packet->acquisitionTime[1] = 0x00;
380 381 packet->acquisitionTime[2] = 0x00;
381 382 packet->acquisitionTime[3] = 0x00;
382 383 packet->acquisitionTime[4] = 0x00;
383 384 packet->acquisitionTime[5] = 0x00;
384 385 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
385 386 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
386 387 }
387 388
388 389 void BP_init_header_with_spare( bp_packet_with_spare *packet,
389 390 unsigned int apid, unsigned char sid,
390 391 unsigned int packetLength , unsigned char blkNr)
391 392 {
392 393 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
393 394 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
394 395 packet->reserved = 0x00;
395 396 packet->userApplication = CCSDS_USER_APP;
396 397 packet->packetID[0] = (unsigned char) (apid >> 8);
397 398 packet->packetID[1] = (unsigned char) (apid);
398 399 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
399 400 packet->packetSequenceControl[1] = 0x00;
400 401 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
401 402 packet->packetLength[1] = (unsigned char) (packetLength);
402 403 // DATA FIELD HEADER
403 404 packet->spare1_pusVersion_spare2 = 0x10;
404 405 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
405 406 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
406 407 packet->destinationID = TM_DESTINATION_ID_GROUND;
407 408 // AUXILIARY DATA HEADER
408 409 packet->sid = sid;
409 410 packet->biaStatusInfo = 0x00;
410 411 packet->time[0] = 0x00;
411 412 packet->time[0] = 0x00;
412 413 packet->time[0] = 0x00;
413 414 packet->time[0] = 0x00;
414 415 packet->time[0] = 0x00;
415 416 packet->time[0] = 0x00;
416 417 packet->source_data_spare = 0x00;
417 418 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
418 419 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
419 420 }
420 421
421 422 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
422 423 {
423 424 rtems_status_code status;
424 425
425 426 // SET THE SEQUENCE_CNT PARAMETER
426 427 increment_seq_counter_source_id( (unsigned char*) &data[ PACKET_POS_SEQUENCE_CNT ], sid );
427 428 // SEND PACKET
428 429 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
429 430 if (status != RTEMS_SUCCESSFUL)
430 431 {
431 432 printf("ERR *** in BP_send *** ERR %d\n", (int) status);
432 433 }
433 434 }
434 435
435 436 //******************
436 437 // general functions
437 438
438 439 void reset_sm_status( void )
439 440 {
440 441 // error
441 442 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
442 443 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
443 444 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
444 445 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
445 446
446 447 spectral_matrix_regs->status = 0x7ff; // [0111 1111 1111]
447 448 }
448 449
449 450 void reset_spectral_matrix_regs( void )
450 451 {
451 452 /** This function resets the spectral matrices module registers.
452 453 *
453 454 * The registers affected by this function are located at the following offset addresses:
454 455 *
455 456 * - 0x00 config
456 457 * - 0x04 status
457 458 * - 0x08 matrixF0_Address0
458 459 * - 0x10 matrixFO_Address1
459 460 * - 0x14 matrixF1_Address
460 461 * - 0x18 matrixF2_Address
461 462 *
462 463 */
463 464
464 465 set_sm_irq_onError( 0 );
465 466
466 467 set_sm_irq_onNewMatrix( 0 );
467 468
468 469 reset_sm_status();
469 470
470 471 // F1
471 472 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
472 473 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
473 474 // F2
474 475 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
475 476 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
476 477 // F3
477 478 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
478 479 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
479 480
480 481 spectral_matrix_regs->matrix_length = 0xc8; // 25 * 128 / 16 = 200 = 0xc8
481 482 }
482 483
483 484 void set_time( unsigned char *time, unsigned char * timeInBuffer )
484 485 {
485 486 time[0] = timeInBuffer[0];
486 487 time[1] = timeInBuffer[1];
487 488 time[2] = timeInBuffer[2];
488 489 time[3] = timeInBuffer[3];
489 490 time[4] = timeInBuffer[6];
490 491 time[5] = timeInBuffer[7];
491 492 }
492 493
493 494 unsigned long long int get_acquisition_time( unsigned char *timePtr )
494 495 {
495 496 unsigned long long int acquisitionTimeAslong;
496 497 acquisitionTimeAslong = 0x00;
497 498 acquisitionTimeAslong = ( (unsigned long long int) (timePtr[0] & 0x7f) << 40 ) // [0111 1111] mask the synchronization bit
498 499 + ( (unsigned long long int) timePtr[1] << 32 )
499 500 + ( (unsigned long long int) timePtr[2] << 24 )
500 501 + ( (unsigned long long int) timePtr[3] << 16 )
501 502 + ( (unsigned long long int) timePtr[6] << 8 )
502 503 + ( (unsigned long long int) timePtr[7] );
503 504 return acquisitionTimeAslong;
504 505 }
505 506
506 507 unsigned char getSID( rtems_event_set event )
507 508 {
508 509 unsigned char sid;
509 510
510 511 rtems_event_set eventSetBURST;
511 512 rtems_event_set eventSetSBM;
512 513
513 514 //******
514 515 // BURST
515 516 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
516 517 | RTEMS_EVENT_BURST_BP1_F1
517 518 | RTEMS_EVENT_BURST_BP2_F0
518 519 | RTEMS_EVENT_BURST_BP2_F1;
519 520
520 521 //****
521 522 // SBM
522 523 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
523 524 | RTEMS_EVENT_SBM_BP1_F1
524 525 | RTEMS_EVENT_SBM_BP2_F0
525 526 | RTEMS_EVENT_SBM_BP2_F1;
526 527
527 528 if (event & eventSetBURST)
528 529 {
529 530 sid = SID_BURST_BP1_F0;
530 531 }
531 532 else if (event & eventSetSBM)
532 533 {
533 534 sid = SID_SBM1_BP1_F0;
534 535 }
535 536 else
536 537 {
537 538 sid = 0;
538 539 }
539 540
540 541 return sid;
541 542 }
542 543
543 544 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
544 545 {
545 546 unsigned int i;
546 547 float re;
547 548 float im;
548 549
549 550 for (i=0; i<NB_BINS_PER_SM; i++){
550 551 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 ];
551 552 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 + 1];
552 553 outputASM[ (asmComponent *NB_BINS_PER_SM) + i] = re;
553 554 outputASM[ (asmComponent+1)*NB_BINS_PER_SM + i] = im;
554 555 }
555 556 }
556 557
557 558 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
558 559 {
559 560 unsigned int i;
560 561 float re;
561 562
562 563 for (i=0; i<NB_BINS_PER_SM; i++){
563 564 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
564 565 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
565 566 }
566 567 }
567 568
568 569 void ASM_patch( float *inputASM, float *outputASM )
569 570 {
570 571 extractReImVectors( inputASM, outputASM, 1); // b1b2
571 572 extractReImVectors( inputASM, outputASM, 3 ); // b1b3
572 573 extractReImVectors( inputASM, outputASM, 5 ); // b1e1
573 574 extractReImVectors( inputASM, outputASM, 7 ); // b1e2
574 575 extractReImVectors( inputASM, outputASM, 10 ); // b2b3
575 576 extractReImVectors( inputASM, outputASM, 12 ); // b2e1
576 577 extractReImVectors( inputASM, outputASM, 14 ); // b2e2
577 578 extractReImVectors( inputASM, outputASM, 17 ); // b3e1
578 579 extractReImVectors( inputASM, outputASM, 19 ); // b3e2
579 580 extractReImVectors( inputASM, outputASM, 22 ); // e1e2
580 581
581 582 copyReVectors(inputASM, outputASM, 0 ); // b1b1
582 583 copyReVectors(inputASM, outputASM, 9 ); // b2b2
583 584 copyReVectors(inputASM, outputASM, 16); // b3b3
584 585 copyReVectors(inputASM, outputASM, 21); // e1e1
585 586 copyReVectors(inputASM, outputASM, 24); // e2e2
586 587 }
587 588
588 589 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
589 590 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
590 591 {
591 592 //*************
592 593 // input format
593 594 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
594 595 //**************
595 596 // output format
596 597 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
597 598 //************
598 599 // compression
599 600 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
600 601 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
601 602
602 603 int frequencyBin;
603 604 int asmComponent;
604 605 int offsetASM;
605 606 int offsetCompressed;
606 607 int offsetFBin;
607 608 int fBinMask;
608 609 int k;
609 610
610 611 // BUILD DATA
611 612 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
612 613 {
613 614 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
614 615 {
615 616 offsetCompressed = // NO TIME OFFSET
616 617 frequencyBin * NB_VALUES_PER_SM
617 618 + asmComponent;
618 619 offsetASM = // NO TIME OFFSET
619 620 asmComponent * NB_BINS_PER_SM
620 621 + ASMIndexStart
621 622 + frequencyBin * nbBinsToAverage;
622 623 offsetFBin = ASMIndexStart
623 624 + frequencyBin * nbBinsToAverage;
624 625 compressed_spec_mat[ offsetCompressed ] = 0;
625 626 for ( k = 0; k < nbBinsToAverage; k++ )
626 627 {
627 628 fBinMask = getFBinMask( offsetFBin + k );
628 629 compressed_spec_mat[offsetCompressed ] =
629 630 ( compressed_spec_mat[ offsetCompressed ]
630 631 + averaged_spec_mat[ offsetASM + k ] * fBinMask );
631 632 }
632 633 compressed_spec_mat[ offsetCompressed ] =
633 634 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
634 635 }
635 636 }
636 637
637 638 }
638 639
639 640 int getFBinMask( int index )
640 641 {
641 642 unsigned int indexInChar;
642 643 unsigned int indexInTheChar;
643 644 int fbin;
644 645
645 646 indexInChar = index >> 3;
646 647 indexInTheChar = index - indexInChar * 8;
647 648
648 649 fbin = (int) ((parameter_dump_packet.sy_lfr_fbins_f0_word1[ NB_BYTES_PER_FREQ_MASK - 1 - indexInChar] >> indexInTheChar) & 0x1);
649 650
650 651 return fbin;
651 652 }
Show file before | Show file after | Hide comments
@@ -1,1121 +1,1159
1 1 /** Functions and tasks related to TeleCommand handling.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TeleCommands:\n
7 7 * action launching\n
8 8 * TC parsing\n
9 9 * ...
10 10 *
11 11 */
12 12
13 13 #include "tc_handler.h"
14 14 #include "math.h"
15 15
16 16 //***********
17 17 // RTEMS TASK
18 18
19 19 rtems_task actn_task( rtems_task_argument unused )
20 20 {
21 21 /** This RTEMS task is responsible for launching actions upton the reception of valid TeleCommands.
22 22 *
23 23 * @param unused is the starting argument of the RTEMS task
24 24 *
25 25 * The ACTN task waits for data coming from an RTEMS msesage queue. When data arrives, it launches specific actions depending
26 26 * on the incoming TeleCommand.
27 27 *
28 28 */
29 29
30 30 int result;
31 31 rtems_status_code status; // RTEMS status code
32 32 ccsdsTelecommandPacket_t TC; // TC sent to the ACTN task
33 33 size_t size; // size of the incoming TC packet
34 34 unsigned char subtype; // subtype of the current TC packet
35 35 unsigned char time[6];
36 36 rtems_id queue_rcv_id;
37 37 rtems_id queue_snd_id;
38 38
39 39 status = get_message_queue_id_recv( &queue_rcv_id );
40 40 if (status != RTEMS_SUCCESSFUL)
41 41 {
42 42 PRINTF1("in ACTN *** ERR get_message_queue_id_recv %d\n", status)
43 43 }
44 44
45 45 status = get_message_queue_id_send( &queue_snd_id );
46 46 if (status != RTEMS_SUCCESSFUL)
47 47 {
48 48 PRINTF1("in ACTN *** ERR get_message_queue_id_send %d\n", status)
49 49 }
50 50
51 51 result = LFR_SUCCESSFUL;
52 52 subtype = 0; // subtype of the current TC packet
53 53
54 54 BOOT_PRINTF("in ACTN *** \n")
55 55
56 56 while(1)
57 57 {
58 58 status = rtems_message_queue_receive( queue_rcv_id, (char*) &TC, &size,
59 59 RTEMS_WAIT, RTEMS_NO_TIMEOUT);
60 60 getTime( time ); // set time to the current time
61 61 if (status!=RTEMS_SUCCESSFUL)
62 62 {
63 63 PRINTF1("ERR *** in task ACTN *** error receiving a message, code %d \n", status)
64 64 }
65 65 else
66 66 {
67 67 subtype = TC.serviceSubType;
68 68 switch(subtype)
69 69 {
70 70 case TC_SUBTYPE_RESET:
71 71 result = action_reset( &TC, queue_snd_id, time );
72 72 close_action( &TC, result, queue_snd_id );
73 73 break;
74 74 case TC_SUBTYPE_LOAD_COMM:
75 75 result = action_load_common_par( &TC );
76 76 close_action( &TC, result, queue_snd_id );
77 77 break;
78 78 case TC_SUBTYPE_LOAD_NORM:
79 79 result = action_load_normal_par( &TC, queue_snd_id, time );
80 80 close_action( &TC, result, queue_snd_id );
81 81 break;
82 82 case TC_SUBTYPE_LOAD_BURST:
83 83 result = action_load_burst_par( &TC, queue_snd_id, time );
84 84 close_action( &TC, result, queue_snd_id );
85 85 break;
86 86 case TC_SUBTYPE_LOAD_SBM1:
87 87 result = action_load_sbm1_par( &TC, queue_snd_id, time );
88 88 close_action( &TC, result, queue_snd_id );
89 89 break;
90 90 case TC_SUBTYPE_LOAD_SBM2:
91 91 result = action_load_sbm2_par( &TC, queue_snd_id, time );
92 92 close_action( &TC, result, queue_snd_id );
93 93 break;
94 94 case TC_SUBTYPE_DUMP:
95 95 result = action_dump_par( queue_snd_id );
96 96 close_action( &TC, result, queue_snd_id );
97 97 break;
98 98 case TC_SUBTYPE_ENTER:
99 99 result = action_enter_mode( &TC, queue_snd_id );
100 100 close_action( &TC, result, queue_snd_id );
101 101 break;
102 102 case TC_SUBTYPE_UPDT_INFO:
103 103 result = action_update_info( &TC, queue_snd_id );
104 104 close_action( &TC, result, queue_snd_id );
105 105 break;
106 106 case TC_SUBTYPE_EN_CAL:
107 107 result = action_enable_calibration( &TC, queue_snd_id, time );
108 108 close_action( &TC, result, queue_snd_id );
109 109 break;
110 110 case TC_SUBTYPE_DIS_CAL:
111 111 result = action_disable_calibration( &TC, queue_snd_id, time );
112 112 close_action( &TC, result, queue_snd_id );
113 113 break;
114 114 case TC_SUBTYPE_LOAD_K:
115 115 printf("TC_SUBTYPE_LOAD_K\n");
116 116 result = action_load_kcoefficients( &TC, queue_snd_id, time );
117 117 close_action( &TC, result, queue_snd_id );
118 118 break;
119 119 case TC_SUBTYPE_DUMP_K:
120 120 result = action_dump_kcoefficients( &TC, queue_snd_id, time );
121 121 close_action( &TC, result, queue_snd_id );
122 122 break;
123 123 case TC_SUBTYPE_LOAD_FBINS:
124 124 result = action_load_fbins_mask( &TC, queue_snd_id, time );
125 125 close_action( &TC, result, queue_snd_id );
126 126 break;
127 127 case TC_SUBTYPE_UPDT_TIME:
128 128 result = action_update_time( &TC );
129 129 close_action( &TC, result, queue_snd_id );
130 130 break;
131 131 default:
132 132 break;
133 133 }
134 134 }
135 135 }
136 136 }
137 137
138 138 //***********
139 139 // TC ACTIONS
140 140
141 141 int action_reset(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
142 142 {
143 143 /** This function executes specific actions when a TC_LFR_RESET TeleCommand has been received.
144 144 *
145 145 * @param TC points to the TeleCommand packet that is being processed
146 146 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
147 147 *
148 148 */
149 149
150 150 printf("this is the end!!!\n");
151 151 exit(0);
152 152 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
153 153 return LFR_DEFAULT;
154 154 }
155 155
156 156 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
157 157 {
158 158 /** This function executes specific actions when a TC_LFR_ENTER_MODE TeleCommand has been received.
159 159 *
160 160 * @param TC points to the TeleCommand packet that is being processed
161 161 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
162 162 *
163 163 */
164 164
165 165 rtems_status_code status;
166 166 unsigned char requestedMode;
167 167 unsigned int *transitionCoarseTime_ptr;
168 168 unsigned int transitionCoarseTime;
169 169 unsigned char * bytePosPtr;
170 170
171 171 bytePosPtr = (unsigned char *) &TC->packetID;
172 172
173 173 requestedMode = bytePosPtr[ BYTE_POS_CP_MODE_LFR_SET ];
174 174 transitionCoarseTime_ptr = (unsigned int *) ( &bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME ] );
175 175 transitionCoarseTime = (*transitionCoarseTime_ptr) & 0x7fffffff;
176 176
177 177 status = check_mode_value( requestedMode );
178 178
179 179 if ( status != LFR_SUCCESSFUL ) // the mode value is inconsistent
180 180 {
181 181 send_tm_lfr_tc_exe_inconsistent( TC, queue_id, BYTE_POS_CP_MODE_LFR_SET, requestedMode );
182 182 }
183 183 else // the mode value is consistent, check the transition
184 184 {
185 185 status = check_mode_transition(requestedMode);
186 186 if (status != LFR_SUCCESSFUL)
187 187 {
188 188 PRINTF("ERR *** in action_enter_mode *** check_mode_transition\n")
189 189 send_tm_lfr_tc_exe_not_executable( TC, queue_id );
190 190 }
191 191 }
192 192
193 193 if ( status == LFR_SUCCESSFUL ) // the transition is valid, enter the mode
194 194 {
195 195 status = check_transition_date( transitionCoarseTime );
196 196 if (status != LFR_SUCCESSFUL)
197 197 {
198 198 PRINTF("ERR *** in action_enter_mode *** check_transition_date\n")
199 199 send_tm_lfr_tc_exe_inconsistent( TC, queue_id,
200 200 BYTE_POS_CP_LFR_ENTER_MODE_TIME,
201 201 bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME + 3 ] );
202 202 }
203 203 }
204 204
205 205 if ( status == LFR_SUCCESSFUL ) // the date is valid, enter the mode
206 206 {
207 207 PRINTF1("OK *** in action_enter_mode *** enter mode %d\n", requestedMode);
208 208 status = enter_mode( requestedMode, transitionCoarseTime );
209 209 }
210 210
211 211 return status;
212 212 }
213 213
214 214 int action_update_info(ccsdsTelecommandPacket_t *TC, rtems_id queue_id)
215 215 {
216 216 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
217 217 *
218 218 * @param TC points to the TeleCommand packet that is being processed
219 219 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
220 220 *
221 221 * @return LFR directive status code:
222 222 * - LFR_DEFAULT
223 223 * - LFR_SUCCESSFUL
224 224 *
225 225 */
226 226
227 227 unsigned int val;
228 228 int result;
229 229 unsigned int status;
230 230 unsigned char mode;
231 231 unsigned char * bytePosPtr;
232 232
233 233 bytePosPtr = (unsigned char *) &TC->packetID;
234 234
235 235 // check LFR mode
236 236 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET5 ] & 0x1e) >> 1;
237 237 status = check_update_info_hk_lfr_mode( mode );
238 238 if (status == LFR_SUCCESSFUL) // check TDS mode
239 239 {
240 240 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0xf0) >> 4;
241 241 status = check_update_info_hk_tds_mode( mode );
242 242 }
243 243 if (status == LFR_SUCCESSFUL) // check THR mode
244 244 {
245 245 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0x0f);
246 246 status = check_update_info_hk_thr_mode( mode );
247 247 }
248 248 if (status == LFR_SUCCESSFUL) // if the parameter check is successful
249 249 {
250 250 val = housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * 256
251 251 + housekeeping_packet.hk_lfr_update_info_tc_cnt[1];
252 252 val++;
253 253 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> 8);
254 254 housekeeping_packet.hk_lfr_update_info_tc_cnt[1] = (unsigned char) (val);
255 255 }
256 256
257 257 result = status;
258 258
259 259 return result;
260 260 }
261 261
262 262 int action_enable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
263 263 {
264 264 /** This function executes specific actions when a TC_LFR_ENABLE_CALIBRATION TeleCommand has been received.
265 265 *
266 266 * @param TC points to the TeleCommand packet that is being processed
267 267 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
268 268 *
269 269 */
270 270
271 271 int result;
272 272
273 273 result = LFR_DEFAULT;
274 274
275 275 startCalibration();
276 276
277 277 result = LFR_SUCCESSFUL;
278 278
279 279 return result;
280 280 }
281 281
282 282 int action_disable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
283 283 {
284 284 /** This function executes specific actions when a TC_LFR_DISABLE_CALIBRATION TeleCommand has been received.
285 285 *
286 286 * @param TC points to the TeleCommand packet that is being processed
287 287 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
288 288 *
289 289 */
290 290
291 291 int result;
292 292
293 293 result = LFR_DEFAULT;
294 294
295 295 stopCalibration();
296 296
297 297 result = LFR_SUCCESSFUL;
298 298
299 299 return result;
300 300 }
301 301
302 302 int action_update_time(ccsdsTelecommandPacket_t *TC)
303 303 {
304 304 /** This function executes specific actions when a TC_LFR_UPDATE_TIME TeleCommand has been received.
305 305 *
306 306 * @param TC points to the TeleCommand packet that is being processed
307 307 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
308 308 *
309 309 * @return LFR_SUCCESSFUL
310 310 *
311 311 */
312 312
313 313 unsigned int val;
314 314
315 315 time_management_regs->coarse_time_load = (TC->dataAndCRC[0] << 24)
316 316 + (TC->dataAndCRC[1] << 16)
317 317 + (TC->dataAndCRC[2] << 8)
318 318 + TC->dataAndCRC[3];
319 319
320 320 val = housekeeping_packet.hk_lfr_update_time_tc_cnt[0] * 256
321 321 + housekeeping_packet.hk_lfr_update_time_tc_cnt[1];
322 322 val++;
323 323 housekeeping_packet.hk_lfr_update_time_tc_cnt[0] = (unsigned char) (val >> 8);
324 324 housekeeping_packet.hk_lfr_update_time_tc_cnt[1] = (unsigned char) (val);
325 325
326 326 return LFR_SUCCESSFUL;
327 327 }
328 328
329 329 //*******************
330 330 // ENTERING THE MODES
331 331 int check_mode_value( unsigned char requestedMode )
332 332 {
333 333 int status;
334 334
335 335 if ( (requestedMode != LFR_MODE_STANDBY)
336 336 && (requestedMode != LFR_MODE_NORMAL) && (requestedMode != LFR_MODE_BURST)
337 337 && (requestedMode != LFR_MODE_SBM1) && (requestedMode != LFR_MODE_SBM2) )
338 338 {
339 339 status = LFR_DEFAULT;
340 340 }
341 341 else
342 342 {
343 343 status = LFR_SUCCESSFUL;
344 344 }
345 345
346 346 return status;
347 347 }
348 348
349 349 int check_mode_transition( unsigned char requestedMode )
350 350 {
351 351 /** This function checks the validity of the transition requested by the TC_LFR_ENTER_MODE.
352 352 *
353 353 * @param requestedMode is the mode requested by the TC_LFR_ENTER_MODE
354 354 *
355 355 * @return LFR directive status codes:
356 356 * - LFR_SUCCESSFUL - the transition is authorized
357 357 * - LFR_DEFAULT - the transition is not authorized
358 358 *
359 359 */
360 360
361 361 int status;
362 362
363 363 switch (requestedMode)
364 364 {
365 365 case LFR_MODE_STANDBY:
366 366 if ( lfrCurrentMode == LFR_MODE_STANDBY ) {
367 367 status = LFR_DEFAULT;
368 368 }
369 369 else
370 370 {
371 371 status = LFR_SUCCESSFUL;
372 372 }
373 373 break;
374 374 case LFR_MODE_NORMAL:
375 375 if ( lfrCurrentMode == LFR_MODE_NORMAL ) {
376 376 status = LFR_DEFAULT;
377 377 }
378 378 else {
379 379 status = LFR_SUCCESSFUL;
380 380 }
381 381 break;
382 382 case LFR_MODE_BURST:
383 383 if ( lfrCurrentMode == LFR_MODE_BURST ) {
384 384 status = LFR_DEFAULT;
385 385 }
386 386 else {
387 387 status = LFR_SUCCESSFUL;
388 388 }
389 389 break;
390 390 case LFR_MODE_SBM1:
391 391 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
392 392 status = LFR_DEFAULT;
393 393 }
394 394 else {
395 395 status = LFR_SUCCESSFUL;
396 396 }
397 397 break;
398 398 case LFR_MODE_SBM2:
399 399 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
400 400 status = LFR_DEFAULT;
401 401 }
402 402 else {
403 403 status = LFR_SUCCESSFUL;
404 404 }
405 405 break;
406 406 default:
407 407 status = LFR_DEFAULT;
408 408 break;
409 409 }
410 410
411 411 return status;
412 412 }
413 413
414 414 int check_transition_date( unsigned int transitionCoarseTime )
415 415 {
416 416 int status;
417 417 unsigned int localCoarseTime;
418 418 unsigned int deltaCoarseTime;
419 419
420 420 status = LFR_SUCCESSFUL;
421 421
422 422 if (transitionCoarseTime == 0) // transition time = 0 means an instant transition
423 423 {
424 424 status = LFR_SUCCESSFUL;
425 425 }
426 426 else
427 427 {
428 428 localCoarseTime = time_management_regs->coarse_time & 0x7fffffff;
429 429
430 430 PRINTF2("localTime = %x, transitionTime = %x\n", localCoarseTime, transitionCoarseTime)
431 431
432 432 if ( transitionCoarseTime <= localCoarseTime ) // SSS-CP-EQS-322
433 433 {
434 434 status = LFR_DEFAULT;
435 435 PRINTF("ERR *** in check_transition_date *** transitionCoarseTime <= localCoarseTime\n")
436 436 }
437 437
438 438 if (status == LFR_SUCCESSFUL)
439 439 {
440 440 deltaCoarseTime = transitionCoarseTime - localCoarseTime;
441 441 if ( deltaCoarseTime > 3 ) // SSS-CP-EQS-323
442 442 {
443 443 status = LFR_DEFAULT;
444 444 PRINTF1("ERR *** in check_transition_date *** deltaCoarseTime = %x\n", deltaCoarseTime)
445 445 }
446 446 }
447 447 }
448 448
449 449 return status;
450 450 }
451 451
452 452 int stop_current_mode( void )
453 453 {
454 454 /** This function stops the current mode by masking interrupt lines and suspending science tasks.
455 455 *
456 456 * @return RTEMS directive status codes:
457 457 * - RTEMS_SUCCESSFUL - task restarted successfully
458 458 * - RTEMS_INVALID_ID - task id invalid
459 459 * - RTEMS_ALREADY_SUSPENDED - task already suspended
460 460 *
461 461 */
462 462
463 463 rtems_status_code status;
464 464
465 465 status = RTEMS_SUCCESSFUL;
466 466
467 467 // (1) mask interruptions
468 468 LEON_Mask_interrupt( IRQ_WAVEFORM_PICKER ); // mask waveform picker interrupt
469 469 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
470 470
471 471 // (2) reset waveform picker registers
472 472 reset_wfp_burst_enable(); // reset burst and enable bits
473 473 reset_wfp_status(); // reset all the status bits
474 474
475 475 // (3) reset spectral matrices registers
476 476 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
477 477 reset_sm_status();
478 478
479 479 // reset lfr VHDL module
480 480 reset_lfr();
481 481
482 482 reset_extractSWF(); // reset the extractSWF flag to false
483 483
484 484 // (4) clear interruptions
485 485 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER ); // clear waveform picker interrupt
486 486 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
487 487
488 488 // <Spectral Matrices simulator>
489 489 LEON_Mask_interrupt( IRQ_SM_SIMULATOR ); // mask spectral matrix interrupt simulator
490 490 timer_stop( (gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR );
491 491 LEON_Clear_interrupt( IRQ_SM_SIMULATOR ); // clear spectral matrix interrupt simulator
492 492 // </Spectral Matrices simulator>
493 493
494 494 // suspend several tasks
495 495 if (lfrCurrentMode != LFR_MODE_STANDBY) {
496 496 status = suspend_science_tasks();
497 497 }
498 498
499 499 if (status != RTEMS_SUCCESSFUL)
500 500 {
501 501 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
502 502 }
503 503
504 504 return status;
505 505 }
506 506
507 507 int enter_mode( unsigned char mode, unsigned int transitionCoarseTime )
508 508 {
509 509 /** This function is launched after a mode transition validation.
510 510 *
511 511 * @param mode is the mode in which LFR will be put.
512 512 *
513 513 * @return RTEMS directive status codes:
514 514 * - RTEMS_SUCCESSFUL - the mode has been entered successfully
515 515 * - RTEMS_NOT_SATISFIED - the mode has not been entered successfully
516 516 *
517 517 */
518 518
519 519 rtems_status_code status;
520 520
521 521 //**********************
522 522 // STOP THE CURRENT MODE
523 523 status = stop_current_mode();
524 524 if (status != RTEMS_SUCCESSFUL)
525 525 {
526 526 PRINTF1("ERR *** in enter_mode *** stop_current_mode with mode = %d\n", mode)
527 527 }
528 528
529 529 //*************************
530 530 // ENTER THE REQUESTED MODE
531 531 if ( (mode == LFR_MODE_NORMAL) || (mode == LFR_MODE_BURST)
532 532 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2) )
533 533 {
534 534 #ifdef PRINT_TASK_STATISTICS
535 535 rtems_cpu_usage_reset();
536 maxCount = 0;
537 536 #endif
538 537 status = restart_science_tasks( mode );
539 538 launch_spectral_matrix( );
540 539 launch_waveform_picker( mode, transitionCoarseTime );
541 540 // launch_spectral_matrix_simu( );
542 541 }
543 542 else if ( mode == LFR_MODE_STANDBY )
544 543 {
545 544 #ifdef PRINT_TASK_STATISTICS
546 545 rtems_cpu_usage_report();
547 546 #endif
548 547
549 548 #ifdef PRINT_STACK_REPORT
550 549 PRINTF("stack report selected\n")
551 550 rtems_stack_checker_report_usage();
552 551 #endif
553 PRINTF1("maxCount = %d\n", maxCount)
554 552 }
555 553 else
556 554 {
557 555 status = RTEMS_UNSATISFIED;
558 556 }
559 557
560 558 if (status != RTEMS_SUCCESSFUL)
561 559 {
562 560 PRINTF1("ERR *** in enter_mode *** status = %d\n", status)
563 561 status = RTEMS_UNSATISFIED;
564 562 }
565 563
566 564 return status;
567 565 }
568 566
569 567 int restart_science_tasks(unsigned char lfrRequestedMode )
570 568 {
571 569 /** This function is used to restart all science tasks.
572 570 *
573 571 * @return RTEMS directive status codes:
574 572 * - RTEMS_SUCCESSFUL - task restarted successfully
575 573 * - RTEMS_INVALID_ID - task id invalid
576 574 * - RTEMS_INCORRECT_STATE - task never started
577 575 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
578 576 *
579 577 * Science tasks are AVF0, PRC0, WFRM, CWF3, CW2, CWF1
580 578 *
581 579 */
582 580
583 581 rtems_status_code status[10];
584 582 rtems_status_code ret;
585 583
586 584 ret = RTEMS_SUCCESSFUL;
587 585
588 586 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
589 587 if (status[0] != RTEMS_SUCCESSFUL)
590 588 {
591 589 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
592 590 }
593 591
594 592 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
595 593 if (status[1] != RTEMS_SUCCESSFUL)
596 594 {
597 595 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
598 596 }
599 597
600 598 status[2] = rtems_task_restart( Task_id[TASKID_WFRM],1 );
601 599 if (status[2] != RTEMS_SUCCESSFUL)
602 600 {
603 601 PRINTF1("in restart_science_task *** WFRM ERR %d\n", status[2])
604 602 }
605 603
606 604 status[3] = rtems_task_restart( Task_id[TASKID_CWF3],1 );
607 605 if (status[3] != RTEMS_SUCCESSFUL)
608 606 {
609 607 PRINTF1("in restart_science_task *** CWF3 ERR %d\n", status[3])
610 608 }
611 609
612 610 status[4] = rtems_task_restart( Task_id[TASKID_CWF2],1 );
613 611 if (status[4] != RTEMS_SUCCESSFUL)
614 612 {
615 613 PRINTF1("in restart_science_task *** CWF2 ERR %d\n", status[4])
616 614 }
617 615
618 616 status[5] = rtems_task_restart( Task_id[TASKID_CWF1],1 );
619 617 if (status[5] != RTEMS_SUCCESSFUL)
620 618 {
621 619 PRINTF1("in restart_science_task *** CWF1 ERR %d\n", status[5])
622 620 }
623 621
624 622 status[6] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
625 623 if (status[6] != RTEMS_SUCCESSFUL)
626 624 {
627 625 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[6])
628 626 }
629 627
630 628 status[7] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
631 629 if (status[7] != RTEMS_SUCCESSFUL)
632 630 {
633 631 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[7])
634 632 }
635 633
636 634 status[8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
637 635 if (status[8] != RTEMS_SUCCESSFUL)
638 636 {
639 637 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[8])
640 638 }
641 639
642 640 status[9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
643 641 if (status[9] != RTEMS_SUCCESSFUL)
644 642 {
645 643 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[9])
646 644 }
647 645
648 646 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
649 647 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
650 648 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) ||
651 649 (status[6] != RTEMS_SUCCESSFUL) || (status[7] != RTEMS_SUCCESSFUL) ||
652 650 (status[8] != RTEMS_SUCCESSFUL) || (status[9] != RTEMS_SUCCESSFUL) )
653 651 {
654 652 ret = RTEMS_UNSATISFIED;
655 653 }
656 654
657 655 return ret;
658 656 }
659 657
660 658 int suspend_science_tasks()
661 659 {
662 660 /** This function suspends the science tasks.
663 661 *
664 662 * @return RTEMS directive status codes:
665 663 * - RTEMS_SUCCESSFUL - task restarted successfully
666 664 * - RTEMS_INVALID_ID - task id invalid
667 665 * - RTEMS_ALREADY_SUSPENDED - task already suspended
668 666 *
669 667 */
670 668
671 669 rtems_status_code status;
672 670
673 671 printf("in suspend_science_tasks\n");
674 672
675 673 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
676 if (status != RTEMS_SUCCESSFUL)
674 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
677 675 {
678 676 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
679 677 }
678 else
679 {
680 status = RTEMS_SUCCESSFUL;
681 }
680 682 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
681 683 {
682 684 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
683 if (status != RTEMS_SUCCESSFUL)
685 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
684 686 {
685 687 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
686 688 }
689 else
690 {
691 status = RTEMS_SUCCESSFUL;
692 }
687 693 }
688 694 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
689 695 {
690 696 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
691 if (status != RTEMS_SUCCESSFUL)
697 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
692 698 {
693 699 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
694 700 }
701 else
702 {
703 status = RTEMS_SUCCESSFUL;
704 }
695 705 }
696 706 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
697 707 {
698 708 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
699 if (status != RTEMS_SUCCESSFUL)
709 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
700 710 {
701 711 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
702 712 }
713 else
714 {
715 status = RTEMS_SUCCESSFUL;
716 }
703 717 }
704 718 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
705 719 {
706 720 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
707 if (status != RTEMS_SUCCESSFUL)
721 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
708 722 {
709 723 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
710 724 }
725 else
726 {
727 status = RTEMS_SUCCESSFUL;
728 }
711 729 }
712 730 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
713 731 {
714 732 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
715 if (status != RTEMS_SUCCESSFUL)
733 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
716 734 {
717 735 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
718 736 }
737 else
738 {
739 status = RTEMS_SUCCESSFUL;
740 }
719 741 }
720 742 if (status == RTEMS_SUCCESSFUL) // suspend WFRM
721 743 {
722 744 status = rtems_task_suspend( Task_id[TASKID_WFRM] );
723 if (status != RTEMS_SUCCESSFUL)
745 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
724 746 {
725 747 PRINTF1("in suspend_science_task *** WFRM ERR %d\n", status)
726 748 }
749 else
750 {
751 status = RTEMS_SUCCESSFUL;
752 }
727 753 }
728 754 if (status == RTEMS_SUCCESSFUL) // suspend CWF3
729 755 {
730 756 status = rtems_task_suspend( Task_id[TASKID_CWF3] );
731 if (status != RTEMS_SUCCESSFUL)
757 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
732 758 {
733 759 PRINTF1("in suspend_science_task *** CWF3 ERR %d\n", status)
734 760 }
761 else
762 {
763 status = RTEMS_SUCCESSFUL;
764 }
735 765 }
736 766 if (status == RTEMS_SUCCESSFUL) // suspend CWF2
737 767 {
738 768 status = rtems_task_suspend( Task_id[TASKID_CWF2] );
739 if (status != RTEMS_SUCCESSFUL)
769 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
740 770 {
741 771 PRINTF1("in suspend_science_task *** CWF2 ERR %d\n", status)
742 772 }
773 else
774 {
775 status = RTEMS_SUCCESSFUL;
776 }
743 777 }
744 778 if (status == RTEMS_SUCCESSFUL) // suspend CWF1
745 779 {
746 780 status = rtems_task_suspend( Task_id[TASKID_CWF1] );
747 if (status != RTEMS_SUCCESSFUL)
781 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
748 782 {
749 783 PRINTF1("in suspend_science_task *** CWF1 ERR %d\n", status)
750 784 }
785 else
786 {
787 status = RTEMS_SUCCESSFUL;
788 }
751 789 }
752 790
753 791 return status;
754 792 }
755 793
756 794 void launch_waveform_picker( unsigned char mode, unsigned int transitionCoarseTime )
757 795 {
758 796 WFP_reset_current_ring_nodes();
759 797
760 798 reset_waveform_picker_regs();
761 799
762 800 set_wfp_burst_enable_register( mode );
763 801
764 802 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER );
765 803 LEON_Unmask_interrupt( IRQ_WAVEFORM_PICKER );
766 804
767 805 if (transitionCoarseTime == 0)
768 806 {
769 807 waveform_picker_regs->start_date = time_management_regs->coarse_time;
770 808 }
771 809 else
772 810 {
773 811 waveform_picker_regs->start_date = transitionCoarseTime;
774 812 }
775 813
776 814 }
777 815
778 816 void launch_spectral_matrix( void )
779 817 {
780 818 SM_reset_current_ring_nodes();
781 819
782 820 reset_spectral_matrix_regs();
783 821
784 822 reset_nb_sm();
785 823
786 824 set_sm_irq_onNewMatrix( 1 );
787 825
788 826 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX );
789 827 LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRIX );
790 828
791 829 }
792 830
793 831 void launch_spectral_matrix_simu( void )
794 832 {
795 833 SM_reset_current_ring_nodes();
796 834 reset_spectral_matrix_regs();
797 835 reset_nb_sm();
798 836
799 837 // Spectral Matrices simulator
800 838 timer_start( (gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR );
801 839 LEON_Clear_interrupt( IRQ_SM_SIMULATOR );
802 840 LEON_Unmask_interrupt( IRQ_SM_SIMULATOR );
803 841 }
804 842
805 843 void set_sm_irq_onNewMatrix( unsigned char value )
806 844 {
807 845 if (value == 1)
808 846 {
809 847 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x01;
810 848 }
811 849 else
812 850 {
813 851 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffe; // 1110
814 852 }
815 853 }
816 854
817 855 void set_sm_irq_onError( unsigned char value )
818 856 {
819 857 if (value == 1)
820 858 {
821 859 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x02;
822 860 }
823 861 else
824 862 {
825 863 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffd; // 1101
826 864 }
827 865 }
828 866
829 867 //*****************************
830 868 // CONFIGURE CALIBRATION SIGNAL
831 869 void setCalibrationPrescaler( unsigned int prescaler )
832 870 {
833 871 // prescaling of the master clock (25 MHz)
834 872 // master clock is divided by 2^prescaler
835 873 time_management_regs->calPrescaler = prescaler;
836 874 }
837 875
838 876 void setCalibrationDivisor( unsigned int divisionFactor )
839 877 {
840 878 // division of the prescaled clock by the division factor
841 879 time_management_regs->calDivisor = divisionFactor;
842 880 }
843 881
844 882 void setCalibrationData( void ){
845 883 unsigned int k;
846 884 unsigned short data;
847 885 float val;
848 886 float f0;
849 887 float f1;
850 888 float fs;
851 889 float Ts;
852 890 float scaleFactor;
853 891
854 892 f0 = 625;
855 893 f1 = 10000;
856 894 fs = 160256.410;
857 895 Ts = 1. / fs;
858 896 scaleFactor = 0.125 / 0.000654; // 191, 500 mVpp, 2 sinus waves => 250 mVpp each, amplitude = 125 mV
859 897
860 898 time_management_regs->calDataPtr = 0x00;
861 899
862 900 // build the signal for the SCM calibration
863 901 for (k=0; k<256; k++)
864 902 {
865 903 val = sin( 2 * pi * f0 * k * Ts )
866 904 + sin( 2 * pi * f1 * k * Ts );
867 905 data = (unsigned short) ((val * scaleFactor) + 2048);
868 906 time_management_regs->calData = data & 0xfff;
869 907 }
870 908 }
871 909
872 910 void setCalibrationDataInterleaved( void ){
873 911 unsigned int k;
874 912 float val;
875 913 float f0;
876 914 float f1;
877 915 float fs;
878 916 float Ts;
879 917 unsigned short data[384];
880 918 unsigned char *dataPtr;
881 919
882 920 f0 = 625;
883 921 f1 = 10000;
884 922 fs = 240384.615;
885 923 Ts = 1. / fs;
886 924
887 925 time_management_regs->calDataPtr = 0x00;
888 926
889 927 // build the signal for the SCM calibration
890 928 for (k=0; k<384; k++)
891 929 {
892 930 val = sin( 2 * pi * f0 * k * Ts )
893 931 + sin( 2 * pi * f1 * k * Ts );
894 932 data[k] = (unsigned short) (val * 512 + 2048);
895 933 }
896 934
897 935 // write the signal in interleaved mode
898 936 for (k=0; k<128; k++)
899 937 {
900 938 dataPtr = (unsigned char*) &data[k*3 + 2];
901 939 time_management_regs->calData = (data[k*3] & 0xfff)
902 940 + ( (dataPtr[0] & 0x3f) << 12);
903 941 time_management_regs->calData = (data[k*3 + 1] & 0xfff)
904 942 + ( (dataPtr[1] & 0x3f) << 12);
905 943 }
906 944 }
907 945
908 946 void setCalibrationReload( bool state)
909 947 {
910 948 if (state == true)
911 949 {
912 950 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000010; // [0001 0000]
913 951 }
914 952 else
915 953 {
916 954 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffef; // [1110 1111]
917 955 }
918 956 }
919 957
920 958 void setCalibrationEnable( bool state )
921 959 {
922 960 // this bit drives the multiplexer
923 961 if (state == true)
924 962 {
925 963 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000040; // [0100 0000]
926 964 }
927 965 else
928 966 {
929 967 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffbf; // [1011 1111]
930 968 }
931 969 }
932 970
933 971 void setCalibrationInterleaved( bool state )
934 972 {
935 973 // this bit drives the multiplexer
936 974 if (state == true)
937 975 {
938 976 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000020; // [0010 0000]
939 977 }
940 978 else
941 979 {
942 980 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffdf; // [1101 1111]
943 981 }
944 982 }
945 983
946 984 void startCalibration( void )
947 985 {
948 986 setCalibrationEnable( true );
949 987 setCalibrationReload( false );
950 988 }
951 989
952 990 void stopCalibration( void )
953 991 {
954 992 setCalibrationEnable( false );
955 993 setCalibrationReload( true );
956 994 }
957 995
958 996 void configureCalibration( bool interleaved )
959 997 {
960 998 stopCalibration();
961 999 if ( interleaved == true )
962 1000 {
963 1001 setCalibrationInterleaved( true );
964 1002 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
965 1003 setCalibrationDivisor( 26 ); // => 240 384
966 1004 setCalibrationDataInterleaved();
967 1005 }
968 1006 else
969 1007 {
970 1008 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
971 1009 setCalibrationDivisor( 38 ); // => 160 256 (39 - 1)
972 1010 setCalibrationData();
973 1011 }
974 1012 }
975 1013
976 1014 //****************
977 1015 // CLOSING ACTIONS
978 1016 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC, unsigned char * time )
979 1017 {
980 1018 /** This function is used to update the HK packets statistics after a successful TC execution.
981 1019 *
982 1020 * @param TC points to the TC being processed
983 1021 * @param time is the time used to date the TC execution
984 1022 *
985 1023 */
986 1024
987 1025 unsigned int val;
988 1026
989 1027 housekeeping_packet.hk_lfr_last_exe_tc_id[0] = TC->packetID[0];
990 1028 housekeeping_packet.hk_lfr_last_exe_tc_id[1] = TC->packetID[1];
991 1029 housekeeping_packet.hk_lfr_last_exe_tc_type[0] = 0x00;
992 1030 housekeeping_packet.hk_lfr_last_exe_tc_type[1] = TC->serviceType;
993 1031 housekeeping_packet.hk_lfr_last_exe_tc_subtype[0] = 0x00;
994 1032 housekeeping_packet.hk_lfr_last_exe_tc_subtype[1] = TC->serviceSubType;
995 1033 housekeeping_packet.hk_lfr_last_exe_tc_time[0] = time[0];
996 1034 housekeeping_packet.hk_lfr_last_exe_tc_time[1] = time[1];
997 1035 housekeeping_packet.hk_lfr_last_exe_tc_time[2] = time[2];
998 1036 housekeeping_packet.hk_lfr_last_exe_tc_time[3] = time[3];
999 1037 housekeeping_packet.hk_lfr_last_exe_tc_time[4] = time[4];
1000 1038 housekeeping_packet.hk_lfr_last_exe_tc_time[5] = time[5];
1001 1039
1002 1040 val = housekeeping_packet.hk_lfr_exe_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_exe_tc_cnt[1];
1003 1041 val++;
1004 1042 housekeeping_packet.hk_lfr_exe_tc_cnt[0] = (unsigned char) (val >> 8);
1005 1043 housekeeping_packet.hk_lfr_exe_tc_cnt[1] = (unsigned char) (val);
1006 1044 }
1007 1045
1008 1046 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC, unsigned char * time )
1009 1047 {
1010 1048 /** This function is used to update the HK packets statistics after a TC rejection.
1011 1049 *
1012 1050 * @param TC points to the TC being processed
1013 1051 * @param time is the time used to date the TC rejection
1014 1052 *
1015 1053 */
1016 1054
1017 1055 unsigned int val;
1018 1056
1019 1057 housekeeping_packet.hk_lfr_last_rej_tc_id[0] = TC->packetID[0];
1020 1058 housekeeping_packet.hk_lfr_last_rej_tc_id[1] = TC->packetID[1];
1021 1059 housekeeping_packet.hk_lfr_last_rej_tc_type[0] = 0x00;
1022 1060 housekeeping_packet.hk_lfr_last_rej_tc_type[1] = TC->serviceType;
1023 1061 housekeeping_packet.hk_lfr_last_rej_tc_subtype[0] = 0x00;
1024 1062 housekeeping_packet.hk_lfr_last_rej_tc_subtype[1] = TC->serviceSubType;
1025 1063 housekeeping_packet.hk_lfr_last_rej_tc_time[0] = time[0];
1026 1064 housekeeping_packet.hk_lfr_last_rej_tc_time[1] = time[1];
1027 1065 housekeeping_packet.hk_lfr_last_rej_tc_time[2] = time[2];
1028 1066 housekeeping_packet.hk_lfr_last_rej_tc_time[3] = time[3];
1029 1067 housekeeping_packet.hk_lfr_last_rej_tc_time[4] = time[4];
1030 1068 housekeeping_packet.hk_lfr_last_rej_tc_time[5] = time[5];
1031 1069
1032 1070 val = housekeeping_packet.hk_lfr_rej_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_rej_tc_cnt[1];
1033 1071 val++;
1034 1072 housekeeping_packet.hk_lfr_rej_tc_cnt[0] = (unsigned char) (val >> 8);
1035 1073 housekeeping_packet.hk_lfr_rej_tc_cnt[1] = (unsigned char) (val);
1036 1074 }
1037 1075
1038 1076 void close_action(ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id )
1039 1077 {
1040 1078 /** This function is the last step of the TC execution workflow.
1041 1079 *
1042 1080 * @param TC points to the TC being processed
1043 1081 * @param result is the result of the TC execution (LFR_SUCCESSFUL / LFR_DEFAULT)
1044 1082 * @param queue_id is the id of the RTEMS message queue used to send TM packets
1045 1083 * @param time is the time used to date the TC execution
1046 1084 *
1047 1085 */
1048 1086
1049 1087 unsigned char requestedMode;
1050 1088
1051 1089 if (result == LFR_SUCCESSFUL)
1052 1090 {
1053 1091 if ( !( (TC->serviceType==TC_TYPE_TIME) & (TC->serviceSubType==TC_SUBTYPE_UPDT_TIME) )
1054 1092 &
1055 1093 !( (TC->serviceType==TC_TYPE_GEN) & (TC->serviceSubType==TC_SUBTYPE_UPDT_INFO))
1056 1094 )
1057 1095 {
1058 1096 send_tm_lfr_tc_exe_success( TC, queue_id );
1059 1097 }
1060 1098 if ( (TC->serviceType == TC_TYPE_GEN) & (TC->serviceSubType == TC_SUBTYPE_ENTER) )
1061 1099 {
1062 1100 //**********************************
1063 1101 // UPDATE THE LFRMODE LOCAL VARIABLE
1064 1102 requestedMode = TC->dataAndCRC[1];
1065 1103 housekeeping_packet.lfr_status_word[0] = (unsigned char) ((requestedMode << 4) + 0x0d);
1066 1104 updateLFRCurrentMode();
1067 1105 }
1068 1106 }
1069 1107 else if (result == LFR_EXE_ERROR)
1070 1108 {
1071 1109 send_tm_lfr_tc_exe_error( TC, queue_id );
1072 1110 }
1073 1111 }
1074 1112
1075 1113 //***************************
1076 1114 // Interrupt Service Routines
1077 1115 rtems_isr commutation_isr1( rtems_vector_number vector )
1078 1116 {
1079 1117 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1080 1118 printf("In commutation_isr1 *** Error sending event to DUMB\n");
1081 1119 }
1082 1120 }
1083 1121
1084 1122 rtems_isr commutation_isr2( rtems_vector_number vector )
1085 1123 {
1086 1124 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1087 1125 printf("In commutation_isr2 *** Error sending event to DUMB\n");
1088 1126 }
1089 1127 }
1090 1128
1091 1129 //****************
1092 1130 // OTHER FUNCTIONS
1093 1131 void updateLFRCurrentMode()
1094 1132 {
1095 1133 /** This function updates the value of the global variable lfrCurrentMode.
1096 1134 *
1097 1135 * lfrCurrentMode is a parameter used by several functions to know in which mode LFR is running.
1098 1136 *
1099 1137 */
1100 1138 // update the local value of lfrCurrentMode with the value contained in the housekeeping_packet structure
1101 1139 lfrCurrentMode = (housekeeping_packet.lfr_status_word[0] & 0xf0) >> 4;
1102 1140 }
1103 1141
1104 1142 void set_lfr_soft_reset( unsigned char value )
1105 1143 {
1106 1144 if (value == 1)
1107 1145 {
1108 1146 time_management_regs->ctrl = time_management_regs->ctrl | 0x00000004; // [0100]
1109 1147 }
1110 1148 else
1111 1149 {
1112 1150 time_management_regs->ctrl = time_management_regs->ctrl & 0xfffffffb; // [1011]
1113 1151 }
1114 1152 }
1115 1153
1116 1154 void reset_lfr( void )
1117 1155 {
1118 1156 set_lfr_soft_reset( 1 );
1119 1157
1120 1158 set_lfr_soft_reset( 0 );
1121 1159 }
Show file before | Show file after | Hide comments
@@ -1,1400 +1,1370
1 1 /** Functions and tasks related to waveform packet generation.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle waveforms, in snapshot or continuous format.\n
7 7 *
8 8 */
9 9
10 10 #include "wf_handler.h"
11 11
12 12 //***************
13 13 // waveform rings
14 14 // F0
15 15 ring_node waveform_ring_f0[NB_RING_NODES_F0];
16 16 ring_node *current_ring_node_f0;
17 17 ring_node *ring_node_to_send_swf_f0;
18 18 // F1
19 19 ring_node waveform_ring_f1[NB_RING_NODES_F1];
20 20 ring_node *current_ring_node_f1;
21 21 ring_node *ring_node_to_send_swf_f1;
22 22 ring_node *ring_node_to_send_cwf_f1;
23 23 // F2
24 24 ring_node waveform_ring_f2[NB_RING_NODES_F2];
25 25 ring_node *current_ring_node_f2;
26 26 ring_node *ring_node_to_send_swf_f2;
27 27 ring_node *ring_node_to_send_cwf_f2;
28 28 // F3
29 29 ring_node waveform_ring_f3[NB_RING_NODES_F3];
30 30 ring_node *current_ring_node_f3;
31 31 ring_node *ring_node_to_send_cwf_f3;
32 32 char wf_cont_f3_light[ (NB_SAMPLES_PER_SNAPSHOT) * NB_BYTES_CWF3_LIGHT_BLK ];
33 33
34 34 bool extractSWF = false;
35 35 bool swf_f0_ready = false;
36 36 bool swf_f1_ready = false;
37 37 bool swf_f2_ready = false;
38 38
39 39 int wf_snap_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ];
40 40 ring_node ring_node_wf_snap_extracted;
41 41
42 42 //*********************
43 43 // Interrupt SubRoutine
44 44
45 45 ring_node * getRingNodeToSendCWF( unsigned char frequencyChannel)
46 46 {
47 47 ring_node *node;
48 48
49 49 node = NULL;
50 50 switch ( frequencyChannel ) {
51 51 case 1:
52 52 node = ring_node_to_send_cwf_f1;
53 53 break;
54 54 case 2:
55 55 node = ring_node_to_send_cwf_f2;
56 56 break;
57 57 case 3:
58 58 node = ring_node_to_send_cwf_f3;
59 59 break;
60 60 default:
61 61 break;
62 62 }
63 63
64 64 return node;
65 65 }
66 66
67 67 ring_node * getRingNodeToSendSWF( unsigned char frequencyChannel)
68 68 {
69 69 ring_node *node;
70 70
71 71 node = NULL;
72 72 switch ( frequencyChannel ) {
73 73 case 0:
74 74 node = ring_node_to_send_swf_f0;
75 75 break;
76 76 case 1:
77 77 node = ring_node_to_send_swf_f1;
78 78 break;
79 79 case 2:
80 80 node = ring_node_to_send_swf_f2;
81 81 break;
82 82 default:
83 83 break;
84 84 }
85 85
86 86 return node;
87 87 }
88 88
89 89 void reset_extractSWF( void )
90 90 {
91 91 extractSWF = false;
92 92 swf_f0_ready = false;
93 93 swf_f1_ready = false;
94 94 swf_f2_ready = false;
95 95 }
96 96
97 97 inline void waveforms_isr_f3( void )
98 98 {
99 99 rtems_status_code spare_status;
100 100
101 101 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_BURST) // in BURST the data are used to place v, e1 and e2 in the HK packet
102 102 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
103 103 { // in modes other than STANDBY and BURST, send the CWF_F3 data
104 104 //***
105 105 // F3
106 106 if ( (waveform_picker_regs->status & 0xc0) != 0x00 ) { // [1100 0000] check the f3 full bits
107 107 ring_node_to_send_cwf_f3 = current_ring_node_f3->previous;
108 108 current_ring_node_f3 = current_ring_node_f3->next;
109 109 if ((waveform_picker_regs->status & 0x40) == 0x40){ // [0100 0000] f3 buffer 0 is full
110 110 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_0_coarse_time;
111 111 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_0_fine_time;
112 112 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->buffer_address;
113 113 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008840; // [1000 1000 0100 0000]
114 114 }
115 115 else if ((waveform_picker_regs->status & 0x80) == 0x80){ // [1000 0000] f3 buffer 1 is full
116 116 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_1_coarse_time;
117 117 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_1_fine_time;
118 118 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address;
119 119 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008880; // [1000 1000 1000 0000]
120 120 }
121 121 if (rtems_event_send( Task_id[TASKID_CWF3], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
122 122 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
123 123 }
124 124 }
125 125 }
126 126 }
127 127
128 128 inline void waveforms_isr_normal( void )
129 129 {
130 130 rtems_status_code status;
131 131
132 132 if ( ( (waveform_picker_regs->status & 0x30) != 0x00 ) // [0011 0000] check the f2 full bits
133 133 && ( (waveform_picker_regs->status & 0x0c) != 0x00 ) // [0000 1100] check the f1 full bits
134 134 && ( (waveform_picker_regs->status & 0x03) != 0x00 )) // [0000 0011] check the f0 full bits
135 135 {
136 136 //***
137 137 // F0
138 138 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
139 139 current_ring_node_f0 = current_ring_node_f0->next;
140 140 if ( (waveform_picker_regs->status & 0x01) == 0x01)
141 141 {
142 142
143 143 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
144 144 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
145 145 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
146 146 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
147 147 }
148 148 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
149 149 {
150 150 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
151 151 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
152 152 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
153 153 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
154 154 }
155 155
156 156 //***
157 157 // F1
158 158 ring_node_to_send_swf_f1 = current_ring_node_f1->previous;
159 159 current_ring_node_f1 = current_ring_node_f1->next;
160 160 if ( (waveform_picker_regs->status & 0x04) == 0x04)
161 161 {
162 162 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
163 163 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
164 164 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
165 165 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
166 166 }
167 167 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
168 168 {
169 169 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
170 170 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
171 171 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
172 172 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
173 173 }
174 174
175 175 //***
176 176 // F2
177 177 ring_node_to_send_swf_f2 = current_ring_node_f2->previous;
178 178 current_ring_node_f2 = current_ring_node_f2->next;
179 179 if ( (waveform_picker_regs->status & 0x10) == 0x10)
180 180 {
181 181 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
182 182 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
183 183 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
184 184 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
185 185 }
186 186 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
187 187 {
188 188 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
189 189 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
190 190 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
191 191 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
192 192 }
193 193 //
194 194 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_NORMAL );
195 195 if ( status != RTEMS_SUCCESSFUL)
196 196 {
197 197 status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
198 198 }
199 199 }
200 200 }
201 201
202 202 inline void waveforms_isr_burst( void )
203 203 {
204 204 unsigned char status;
205 205 rtems_status_code spare_status;
206 206
207 207 status = (waveform_picker_regs->status & 0x30) >> 4; // [0011 0000] get the status bits for f2
208 208
209 209
210 210 switch(status)
211 211 {
212 212 case 1:
213 213 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
214 214 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
215 215 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
216 216 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
217 217 current_ring_node_f2 = current_ring_node_f2->next;
218 218 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
219 219 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
220 220 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
221 221 }
222 222 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
223 223 break;
224 224 case 2:
225 225 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
226 226 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
227 227 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
228 228 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
229 229 current_ring_node_f2 = current_ring_node_f2->next;
230 230 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
231 231 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
232 232 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
233 233 }
234 234 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
235 235 break;
236 236 default:
237 237 break;
238 238 }
239 239 }
240 240
241 241 inline void waveforms_isr_sbm1( void )
242 242 {
243 243 rtems_status_code status;
244 244
245 245 //***
246 246 // F1
247 247 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bits
248 248 // (1) change the receiving buffer for the waveform picker
249 249 ring_node_to_send_cwf_f1 = current_ring_node_f1->previous;
250 250 current_ring_node_f1 = current_ring_node_f1->next;
251 251 if ( (waveform_picker_regs->status & 0x04) == 0x04)
252 252 {
253 253 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
254 254 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
255 255 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
256 256 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
257 257 }
258 258 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
259 259 {
260 260 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
261 261 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
262 262 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
263 263 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
264 264 }
265 265 // (2) send an event for the the CWF1 task for transmission (and snapshot extraction if needed)
266 266 status = rtems_event_send( Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_SBM1 );
267 267 }
268 268
269 269 //***
270 270 // F0
271 271 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) { // [0000 0011] check the f0 full bits
272 272 swf_f0_ready = true;
273 273 // change f0 buffer
274 274 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
275 275 current_ring_node_f0 = current_ring_node_f0->next;
276 276 if ( (waveform_picker_regs->status & 0x01) == 0x01)
277 277 {
278 278
279 279 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
280 280 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
281 281 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
282 282 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
283 283 }
284 284 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
285 285 {
286 286 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
287 287 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
288 288 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
289 289 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
290 290 }
291 291 }
292 292
293 293 //***
294 294 // F2
295 295 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bits
296 296 swf_f2_ready = true;
297 297 // change f2 buffer
298 298 ring_node_to_send_swf_f2 = current_ring_node_f2->previous;
299 299 current_ring_node_f2 = current_ring_node_f2->next;
300 300 if ( (waveform_picker_regs->status & 0x10) == 0x10)
301 301 {
302 302 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
303 303 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
304 304 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
305 305 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
306 306 }
307 307 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
308 308 {
309 309 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
310 310 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
311 311 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
312 312 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
313 313 }
314 314 }
315 315 }
316 316
317 317 inline void waveforms_isr_sbm2( void )
318 318 {
319 319 rtems_status_code status;
320 320
321 321 //***
322 322 // F2
323 323 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bit
324 324 // (1) change the receiving buffer for the waveform picker
325 325 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
326 326 ring_node_to_send_cwf_f2->sid = SID_SBM2_CWF_F2;
327 327 current_ring_node_f2 = current_ring_node_f2->next;
328 328 if ( (waveform_picker_regs->status & 0x10) == 0x10)
329 329 {
330 330 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
331 331 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
332 332 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
333 333 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
334 334 }
335 335 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
336 336 {
337 337 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
338 338 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
339 339 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
340 340 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
341 341 }
342 342 // (2) send an event for the waveforms transmission
343 343 status = rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_SBM2 );
344 344 }
345 345
346 346 //***
347 347 // F0
348 348 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) { // [0000 0011] check the f0 full bit
349 349 swf_f0_ready = true;
350 350 // change f0 buffer
351 351 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
352 352 current_ring_node_f0 = current_ring_node_f0->next;
353 353 if ( (waveform_picker_regs->status & 0x01) == 0x01)
354 354 {
355 355
356 356 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
357 357 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
358 358 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
359 359 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
360 360 }
361 361 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
362 362 {
363 363 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
364 364 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
365 365 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
366 366 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
367 367 }
368 368 }
369 369
370 370 //***
371 371 // F1
372 372 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bit
373 373 swf_f1_ready = true;
374 374 ring_node_to_send_swf_f1 = current_ring_node_f1->previous;
375 375 current_ring_node_f1 = current_ring_node_f1->next;
376 376 if ( (waveform_picker_regs->status & 0x04) == 0x04)
377 377 {
378 378 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
379 379 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
380 380 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
381 381 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
382 382 }
383 383 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
384 384 {
385 385 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
386 386 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
387 387 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
388 388 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
389 389 }
390 390 }
391 391 }
392 392
393 393 rtems_isr waveforms_isr( rtems_vector_number vector )
394 394 {
395 395 /** This is the interrupt sub routine called by the waveform picker core.
396 396 *
397 397 * This ISR launch different actions depending mainly on two pieces of information:
398 398 * 1. the values read in the registers of the waveform picker.
399 399 * 2. the current LFR mode.
400 400 *
401 401 */
402 402
403 403 // STATUS
404 404 // new error error buffer full
405 405 // 15 14 13 12 11 10 9 8
406 406 // f3 f2 f1 f0 f3 f2 f1 f0
407 407 //
408 408 // ready buffer
409 409 // 7 6 5 4 3 2 1 0
410 410 // f3_1 f3_0 f2_1 f2_0 f1_1 f1_0 f0_1 f0_0
411 411
412 412 rtems_status_code spare_status;
413 413
414 414 waveforms_isr_f3();
415 415
416 416 if ( (waveform_picker_regs->status & 0xff00) != 0x00) // [1111 1111 0000 0000] check the error bits
417 417 {
418 418 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_10 );
419 419 }
420 420
421 421 switch(lfrCurrentMode)
422 422 {
423 423 //********
424 424 // STANDBY
425 425 case(LFR_MODE_STANDBY):
426 426 break;
427 427
428 428 //******
429 429 // NORMAL
430 430 case(LFR_MODE_NORMAL):
431 431 waveforms_isr_normal();
432 432 break;
433 433
434 434 //******
435 435 // BURST
436 436 case(LFR_MODE_BURST):
437 437 waveforms_isr_burst();
438 438 break;
439 439
440 440 //*****
441 441 // SBM1
442 442 case(LFR_MODE_SBM1):
443 443 waveforms_isr_sbm1();
444 444 break;
445 445
446 446 //*****
447 447 // SBM2
448 448 case(LFR_MODE_SBM2):
449 449 waveforms_isr_sbm2();
450 450 break;
451 451
452 452 //********
453 453 // DEFAULT
454 454 default:
455 455 break;
456 456 }
457 457 }
458 458
459 459 //************
460 460 // RTEMS TASKS
461 461
462 462 rtems_task wfrm_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
463 463 {
464 464 /** This RTEMS task is dedicated to the transmission of snapshots of the NORMAL mode.
465 465 *
466 466 * @param unused is the starting argument of the RTEMS task
467 467 *
468 468 * The following data packets are sent by this task:
469 469 * - TM_LFR_SCIENCE_NORMAL_SWF_F0
470 470 * - TM_LFR_SCIENCE_NORMAL_SWF_F1
471 471 * - TM_LFR_SCIENCE_NORMAL_SWF_F2
472 472 *
473 473 */
474 474
475 475 rtems_event_set event_out;
476 476 rtems_id queue_id;
477 477 rtems_status_code status;
478 478 bool resynchronisationEngaged;
479 479 ring_node *ring_node_wf_snap_extracted_ptr;
480 480
481 481 ring_node_wf_snap_extracted_ptr = (ring_node *) &ring_node_wf_snap_extracted;
482 482
483 483 resynchronisationEngaged = false;
484 484
485 485 status = get_message_queue_id_send( &queue_id );
486 486 if (status != RTEMS_SUCCESSFUL)
487 487 {
488 488 PRINTF1("in WFRM *** ERR get_message_queue_id_send %d\n", status)
489 489 }
490 490
491 491 BOOT_PRINTF("in WFRM ***\n")
492 492
493 493 while(1){
494 494 // wait for an RTEMS_EVENT
495 495 rtems_event_receive(RTEMS_EVENT_MODE_NORMAL | RTEMS_EVENT_MODE_SBM1
496 496 | RTEMS_EVENT_MODE_SBM2 | RTEMS_EVENT_MODE_SBM2_WFRM,
497 497 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
498 498 if(resynchronisationEngaged == false)
499 499 { // engage resynchronisation
500 500 snapshot_resynchronization( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
501 501 resynchronisationEngaged = true;
502 502 }
503 503 else
504 504 { // reset delta_snapshot to the nominal value
505 505 PRINTF("no resynchronisation, reset delta_snapshot to the nominal value\n")
506 506 set_wfp_delta_snapshot();
507 507 resynchronisationEngaged = false;
508 508 }
509 509 //
510 510
511 511 if (event_out == RTEMS_EVENT_MODE_NORMAL)
512 512 {
513 513 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_NORMAL\n")
514 514 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
515 515 ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1;
516 516 ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2;
517 517 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
518 518 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) );
519 519 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) );
520 520 }
521 521 if (event_out == RTEMS_EVENT_MODE_SBM1)
522 522 {
523 523 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM1\n")
524 524 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
525 525 ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F1;
526 526 ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2;
527 527 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
528 528 status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) );
529 529 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) );
530 530 }
531 531 if (event_out == RTEMS_EVENT_MODE_SBM2)
532 532 {
533 533 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM2\n")
534 534 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
535 535 ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1;
536 536 ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F2;
537 537 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
538 538 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) );
539 539 status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) );
540 540 }
541 541 }
542 542 }
543 543
544 544 rtems_task cwf3_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
545 545 {
546 546 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f3.
547 547 *
548 548 * @param unused is the starting argument of the RTEMS task
549 549 *
550 550 * The following data packet is sent by this task:
551 551 * - TM_LFR_SCIENCE_NORMAL_CWF_F3
552 552 *
553 553 */
554 554
555 555 rtems_event_set event_out;
556 556 rtems_id queue_id;
557 557 rtems_status_code status;
558 558 ring_node ring_node_cwf3_light;
559 559
560 560 status = get_message_queue_id_send( &queue_id );
561 561 if (status != RTEMS_SUCCESSFUL)
562 562 {
563 563 PRINTF1("in CWF3 *** ERR get_message_queue_id_send %d\n", status)
564 564 }
565 565
566 566 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
567 567
568 568 // init the ring_node_cwf3_light structure
569 569 ring_node_cwf3_light.buffer_address = (int) wf_cont_f3_light;
570 570 ring_node_cwf3_light.coarseTime = 0x00;
571 571 ring_node_cwf3_light.fineTime = 0x00;
572 572 ring_node_cwf3_light.next = NULL;
573 573 ring_node_cwf3_light.previous = NULL;
574 574 ring_node_cwf3_light.sid = SID_NORM_CWF_F3;
575 575 ring_node_cwf3_light.status = 0x00;
576 576
577 577 BOOT_PRINTF("in CWF3 ***\n")
578 578
579 579 while(1){
580 580 // wait for an RTEMS_EVENT
581 581 rtems_event_receive( RTEMS_EVENT_0,
582 582 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
583 583 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
584 584 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode==LFR_MODE_SBM2) )
585 585 {
586 586 if ( (parameter_dump_packet.sy_lfr_n_cwf_long_f3 & 0x01) == 0x01)
587 587 {
588 588 PRINTF("send CWF_LONG_F3\n")
589 589 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
590 590 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf_f3, sizeof( ring_node* ) );
591 591 }
592 592 else
593 593 {
594 594 PRINTF("send CWF_F3 (light)\n")
595 595 send_waveform_CWF3_light( ring_node_to_send_cwf_f3, &ring_node_cwf3_light, queue_id );
596 596 }
597 597
598 598 }
599 599 else
600 600 {
601 601 PRINTF1("in CWF3 *** lfrCurrentMode is %d, no data will be sent\n", lfrCurrentMode)
602 602 }
603 603 }
604 604 }
605 605
606 606 rtems_task cwf2_task(rtems_task_argument argument) // ONLY USED IN BURST AND SBM2
607 607 {
608 608 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f2.
609 609 *
610 610 * @param unused is the starting argument of the RTEMS task
611 611 *
612 612 * The following data packet is sent by this function:
613 613 * - TM_LFR_SCIENCE_BURST_CWF_F2
614 614 * - TM_LFR_SCIENCE_SBM2_CWF_F2
615 615 *
616 616 */
617 617
618 618 rtems_event_set event_out;
619 619 rtems_id queue_id;
620 620 rtems_status_code status;
621 621 ring_node *ring_node_to_send;
622 622 unsigned long long int acquisitionTimeF0_asLong;
623 623
624 624 acquisitionTimeF0_asLong = 0x00;
625 625
626 626 status = get_message_queue_id_send( &queue_id );
627 627 if (status != RTEMS_SUCCESSFUL)
628 628 {
629 629 PRINTF1("in CWF2 *** ERR get_message_queue_id_send %d\n", status)
630 630 }
631 631
632 632 BOOT_PRINTF("in CWF2 ***\n")
633 633
634 634 while(1){
635 635 // wait for an RTEMS_EVENT
636 636 rtems_event_receive( RTEMS_EVENT_MODE_BURST | RTEMS_EVENT_MODE_SBM2,
637 637 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
638 638 ring_node_to_send = getRingNodeToSendCWF( 2 );
639 639 if (event_out == RTEMS_EVENT_MODE_BURST)
640 640 {
641 641 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
642 642 }
643 643 if (event_out == RTEMS_EVENT_MODE_SBM2)
644 644 {
645 645 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
646 646 // launch snapshot extraction if needed
647 647 if (extractSWF == true)
648 648 {
649 649 ring_node_to_send_swf_f2 = ring_node_to_send_cwf_f2;
650 650 // extract the snapshot
651 651 build_snapshot_from_ring( ring_node_to_send_swf_f2, 2, acquisitionTimeF0_asLong );
652 652 // send the snapshot when built
653 653 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM2 );
654 654 extractSWF = false;
655 655 }
656 656 if (swf_f0_ready && swf_f1_ready)
657 657 {
658 658 extractSWF = true;
659 659 // record the acquition time of the fà snapshot to use to build the snapshot at f2
660 660 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
661 661 swf_f0_ready = false;
662 662 swf_f1_ready = false;
663 663 }
664 664 }
665 665 }
666 666 }
667 667
668 668 rtems_task cwf1_task(rtems_task_argument argument) // ONLY USED IN SBM1
669 669 {
670 670 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f1.
671 671 *
672 672 * @param unused is the starting argument of the RTEMS task
673 673 *
674 674 * The following data packet is sent by this function:
675 675 * - TM_LFR_SCIENCE_SBM1_CWF_F1
676 676 *
677 677 */
678 678
679 679 rtems_event_set event_out;
680 680 rtems_id queue_id;
681 681 rtems_status_code status;
682 682
683 683 ring_node *ring_node_to_send_cwf;
684 684
685 685 status = get_message_queue_id_send( &queue_id );
686 686 if (status != RTEMS_SUCCESSFUL)
687 687 {
688 688 PRINTF1("in CWF1 *** ERR get_message_queue_id_send %d\n", status)
689 689 }
690 690
691 691 BOOT_PRINTF("in CWF1 ***\n")
692 692
693 693 while(1){
694 694 // wait for an RTEMS_EVENT
695 695 rtems_event_receive( RTEMS_EVENT_MODE_SBM1,
696 696 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
697 697 ring_node_to_send_cwf = getRingNodeToSendCWF( 1 );
698 698 ring_node_to_send_cwf_f1->sid = SID_SBM1_CWF_F1;
699 699 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
700 if (status != 0)
701 printf("cwf sending failed\n");
700 702 // launch snapshot extraction if needed
701 703 if (extractSWF == true)
702 704 {
703 705 ring_node_to_send_swf_f1 = ring_node_to_send_cwf;
704 706 // launch the snapshot extraction
705 707 status = rtems_event_send( Task_id[TASKID_SWBD], RTEMS_EVENT_MODE_SBM1 );
706 708 extractSWF = false;
707 709 }
708 710 if (swf_f0_ready == true)
709 711 {
710 712 extractSWF = true;
711 713 swf_f0_ready = false; // this step shall be executed only one time
712 714 }
713 715 if ((swf_f1_ready == true) && (swf_f2_ready == true)) // swf_f1 is ready after the extraction
714 716 {
715 717 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM1 );
716 718 swf_f1_ready = false;
717 719 swf_f2_ready = false;
718 720 }
719 721 }
720 722 }
721 723
722 724 rtems_task swbd_task(rtems_task_argument argument)
723 725 {
724 726 /** This RTEMS task is dedicated to the building of snapshots from different continuous waveforms buffers.
725 727 *
726 728 * @param unused is the starting argument of the RTEMS task
727 729 *
728 730 */
729 731
730 732 rtems_event_set event_out;
731 733 unsigned long long int acquisitionTimeF0_asLong;
732 734
733 735 acquisitionTimeF0_asLong = 0x00;
734 736
735 737 BOOT_PRINTF("in SWBD ***\n")
736 738
737 739 while(1){
738 740 // wait for an RTEMS_EVENT
739 741 rtems_event_receive( RTEMS_EVENT_MODE_SBM1 | RTEMS_EVENT_MODE_SBM2,
740 742 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
741 743 if (event_out == RTEMS_EVENT_MODE_SBM1)
742 744 {
743 745 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
744 746 build_snapshot_from_ring( ring_node_to_send_swf_f1, 1, acquisitionTimeF0_asLong );
745 747 swf_f1_ready = true; // the snapshot has been extracted and is ready to be sent
746 748 }
747 749 else
748 750 {
749 751 PRINTF1("in SWBD *** unexpected rtems event received %x\n", (int) event_out)
750 752 }
751 753 }
752 754 }
753 755
754 756 //******************
755 757 // general functions
756 758
757 759 void WFP_init_rings( void )
758 760 {
759 761 // F0 RING
760 762 init_ring( waveform_ring_f0, NB_RING_NODES_F0, wf_buffer_f0, WFRM_BUFFER );
761 763 // F1 RING
762 764 init_ring( waveform_ring_f1, NB_RING_NODES_F1, wf_buffer_f1, WFRM_BUFFER );
763 765 // F2 RING
764 766 init_ring( waveform_ring_f2, NB_RING_NODES_F2, wf_buffer_f2, WFRM_BUFFER );
765 767 // F3 RING
766 768 init_ring( waveform_ring_f3, NB_RING_NODES_F3, wf_buffer_f3, WFRM_BUFFER );
767 769
768 770 ring_node_wf_snap_extracted.buffer_address = (int) wf_snap_extracted;
769 771
770 772 DEBUG_PRINTF1("waveform_ring_f0 @%x\n", (unsigned int) waveform_ring_f0)
771 773 DEBUG_PRINTF1("waveform_ring_f1 @%x\n", (unsigned int) waveform_ring_f1)
772 774 DEBUG_PRINTF1("waveform_ring_f2 @%x\n", (unsigned int) waveform_ring_f2)
773 775 DEBUG_PRINTF1("waveform_ring_f3 @%x\n", (unsigned int) waveform_ring_f3)
774 776 DEBUG_PRINTF1("wf_buffer_f0 @%x\n", (unsigned int) wf_buffer_f0)
775 777 DEBUG_PRINTF1("wf_buffer_f1 @%x\n", (unsigned int) wf_buffer_f1)
776 778 DEBUG_PRINTF1("wf_buffer_f2 @%x\n", (unsigned int) wf_buffer_f2)
777 779 DEBUG_PRINTF1("wf_buffer_f3 @%x\n", (unsigned int) wf_buffer_f3)
778 780
779 781 }
780 782
781 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
782 {
783 unsigned char i;
784
785 //***************
786 // BUFFER ADDRESS
787 for(i=0; i<nbNodes; i++)
788 {
789 ring[i].coarseTime = 0x00;
790 ring[i].fineTime = 0x00;
791 ring[i].sid = 0x00;
792 ring[i].status = 0x00;
793 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
794 }
795
796 //*****
797 // NEXT
798 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
799 for(i=0; i<nbNodes-1; i++)
800 {
801 ring[i].next = (ring_node*) &ring[ i + 1 ];
802 }
803
804 //*********
805 // PREVIOUS
806 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
807 for(i=1; i<nbNodes; i++)
808 {
809 ring[i].previous = (ring_node*) &ring[ i - 1 ];
810 }
811 }
812
813 783 void WFP_reset_current_ring_nodes( void )
814 784 {
815 785 current_ring_node_f0 = waveform_ring_f0[0].next;
816 786 current_ring_node_f1 = waveform_ring_f1[0].next;
817 787 current_ring_node_f2 = waveform_ring_f2[0].next;
818 788 current_ring_node_f3 = waveform_ring_f3[0].next;
819 789
820 790 ring_node_to_send_swf_f0 = waveform_ring_f0;
821 791 ring_node_to_send_swf_f1 = waveform_ring_f1;
822 792 ring_node_to_send_swf_f2 = waveform_ring_f2;
823 793
824 794 ring_node_to_send_cwf_f1 = waveform_ring_f1;
825 795 ring_node_to_send_cwf_f2 = waveform_ring_f2;
826 796 ring_node_to_send_cwf_f3 = waveform_ring_f3;
827 797 }
828 798
829 799 int send_waveform_CWF3_light( ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id )
830 800 {
831 801 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
832 802 *
833 803 * @param waveform points to the buffer containing the data that will be send.
834 804 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
835 805 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
836 806 * contain information to setup the transmission of the data packets.
837 807 *
838 808 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
839 809 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
840 810 *
841 811 */
842 812
843 813 unsigned int i;
844 814 int ret;
845 815 rtems_status_code status;
846 816
847 817 char *sample;
848 818 int *dataPtr;
849 819
850 820 ret = LFR_DEFAULT;
851 821
852 822 dataPtr = (int*) ring_node_to_send->buffer_address;
853 823
854 824 ring_node_cwf3_light->coarseTime = ring_node_to_send->coarseTime;
855 825 ring_node_cwf3_light->fineTime = ring_node_to_send->fineTime;
856 826
857 827 //**********************
858 828 // BUILD CWF3_light DATA
859 829 for ( i=0; i< NB_SAMPLES_PER_SNAPSHOT; i++)
860 830 {
861 831 sample = (char*) &dataPtr[ (i * NB_WORDS_SWF_BLK) ];
862 832 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) ] = sample[ 0 ];
863 833 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 1 ] = sample[ 1 ];
864 834 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 2 ] = sample[ 2 ];
865 835 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 3 ] = sample[ 3 ];
866 836 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 4 ] = sample[ 4 ];
867 837 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 5 ] = sample[ 5 ];
868 838 }
869 839
870 840 // SEND PACKET
871 841 status = rtems_message_queue_send( queue_id, &ring_node_cwf3_light, sizeof( ring_node* ) );
872 842 if (status != RTEMS_SUCCESSFUL) {
873 843 printf("%d-%d, ERR %d\n", SID_NORM_CWF_F3, i, (int) status);
874 844 ret = LFR_DEFAULT;
875 845 }
876 846
877 847 return ret;
878 848 }
879 849
880 850 void compute_acquisition_time( unsigned int coarseTime, unsigned int fineTime,
881 851 unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char * acquisitionTime )
882 852 {
883 853 unsigned long long int acquisitionTimeAsLong;
884 854 unsigned char localAcquisitionTime[6];
885 855 double deltaT;
886 856
887 857 deltaT = 0.;
888 858
889 859 localAcquisitionTime[0] = (unsigned char) ( coarseTime >> 24 );
890 860 localAcquisitionTime[1] = (unsigned char) ( coarseTime >> 16 );
891 861 localAcquisitionTime[2] = (unsigned char) ( coarseTime >> 8 );
892 862 localAcquisitionTime[3] = (unsigned char) ( coarseTime );
893 863 localAcquisitionTime[4] = (unsigned char) ( fineTime >> 8 );
894 864 localAcquisitionTime[5] = (unsigned char) ( fineTime );
895 865
896 866 acquisitionTimeAsLong = ( (unsigned long long int) localAcquisitionTime[0] << 40 )
897 867 + ( (unsigned long long int) localAcquisitionTime[1] << 32 )
898 868 + ( (unsigned long long int) localAcquisitionTime[2] << 24 )
899 869 + ( (unsigned long long int) localAcquisitionTime[3] << 16 )
900 870 + ( (unsigned long long int) localAcquisitionTime[4] << 8 )
901 871 + ( (unsigned long long int) localAcquisitionTime[5] );
902 872
903 873 switch( sid )
904 874 {
905 875 case SID_NORM_SWF_F0:
906 876 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 24576. ;
907 877 break;
908 878
909 879 case SID_NORM_SWF_F1:
910 880 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 4096. ;
911 881 break;
912 882
913 883 case SID_NORM_SWF_F2:
914 884 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 256. ;
915 885 break;
916 886
917 887 case SID_SBM1_CWF_F1:
918 888 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 4096. ;
919 889 break;
920 890
921 891 case SID_SBM2_CWF_F2:
922 892 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
923 893 break;
924 894
925 895 case SID_BURST_CWF_F2:
926 896 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
927 897 break;
928 898
929 899 case SID_NORM_CWF_F3:
930 900 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF_SHORT_F3 * 65536. / 16. ;
931 901 break;
932 902
933 903 case SID_NORM_CWF_LONG_F3:
934 904 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 16. ;
935 905 break;
936 906
937 907 default:
938 908 PRINTF1("in compute_acquisition_time *** ERR unexpected sid %d\n", sid)
939 909 deltaT = 0.;
940 910 break;
941 911 }
942 912
943 913 acquisitionTimeAsLong = acquisitionTimeAsLong + (unsigned long long int) deltaT;
944 914 //
945 915 acquisitionTime[0] = (unsigned char) (acquisitionTimeAsLong >> 40);
946 916 acquisitionTime[1] = (unsigned char) (acquisitionTimeAsLong >> 32);
947 917 acquisitionTime[2] = (unsigned char) (acquisitionTimeAsLong >> 24);
948 918 acquisitionTime[3] = (unsigned char) (acquisitionTimeAsLong >> 16);
949 919 acquisitionTime[4] = (unsigned char) (acquisitionTimeAsLong >> 8 );
950 920 acquisitionTime[5] = (unsigned char) (acquisitionTimeAsLong );
951 921
952 922 }
953 923
954 924 void build_snapshot_from_ring( ring_node *ring_node_to_send, unsigned char frequencyChannel, unsigned long long int acquisitionTimeF0_asLong )
955 925 {
956 926 unsigned int i;
957 927 unsigned long long int centerTime_asLong;
958 928 unsigned long long int acquisitionTime_asLong;
959 929 unsigned long long int bufferAcquisitionTime_asLong;
960 930 unsigned char *ptr1;
961 931 unsigned char *ptr2;
962 932 unsigned char *timeCharPtr;
963 933 unsigned char nb_ring_nodes;
964 934 unsigned long long int frequency_asLong;
965 935 unsigned long long int nbTicksPerSample_asLong;
966 936 unsigned long long int nbSamplesPart1_asLong;
967 937 unsigned long long int sampleOffset_asLong;
968 938
969 939 unsigned int deltaT_F0;
970 940 unsigned int deltaT_F1;
971 941 unsigned long long int deltaT_F2;
972 942
973 943 deltaT_F0 = 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
974 944 deltaT_F1 = 16384; // (2048. / 4096. / 2.) * 65536. = 16384;
975 945 deltaT_F2 = 262144; // (2048. / 256. / 2.) * 65536. = 262144;
976 946 sampleOffset_asLong = 0x00;
977 947
978 948 // (1) get the f0 acquisition time => the value is passed in argument
979 949
980 950 // (2) compute the central reference time
981 951 centerTime_asLong = acquisitionTimeF0_asLong + deltaT_F0;
982 952
983 953 // (3) compute the acquisition time of the current snapshot
984 954 switch(frequencyChannel)
985 955 {
986 956 case 1: // 1 is for F1 = 4096 Hz
987 957 acquisitionTime_asLong = centerTime_asLong - deltaT_F1;
988 958 nb_ring_nodes = NB_RING_NODES_F1;
989 959 frequency_asLong = 4096;
990 960 nbTicksPerSample_asLong = 16; // 65536 / 4096;
991 961 break;
992 962 case 2: // 2 is for F2 = 256 Hz
993 963 acquisitionTime_asLong = centerTime_asLong - deltaT_F2;
994 964 nb_ring_nodes = NB_RING_NODES_F2;
995 965 frequency_asLong = 256;
996 966 nbTicksPerSample_asLong = 256; // 65536 / 256;
997 967 break;
998 968 default:
999 969 acquisitionTime_asLong = centerTime_asLong;
1000 970 frequency_asLong = 256;
1001 971 nbTicksPerSample_asLong = 256;
1002 972 break;
1003 973 }
1004 974
1005 975 //****************************************************************************
1006 976 // (4) search the ring_node with the acquisition time <= acquisitionTime_asLong
1007 977 for (i=0; i<nb_ring_nodes; i++)
1008 978 {
1009 979 PRINTF1("%d ... ", i)
1010 980 bufferAcquisitionTime_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send->coarseTime );
1011 981 if (bufferAcquisitionTime_asLong <= acquisitionTime_asLong)
1012 982 {
1013 983 PRINTF1("buffer found with acquisition time = %llx\n", bufferAcquisitionTime_asLong)
1014 984 break;
1015 985 }
1016 986 ring_node_to_send = ring_node_to_send->previous;
1017 987 }
1018 988
1019 989 // (5) compute the number of samples to take in the current buffer
1020 990 sampleOffset_asLong = ((acquisitionTime_asLong - bufferAcquisitionTime_asLong) * frequency_asLong ) >> 16;
1021 991 nbSamplesPart1_asLong = NB_SAMPLES_PER_SNAPSHOT - sampleOffset_asLong;
1022 992 PRINTF2("sampleOffset_asLong = %lld, nbSamplesPart1_asLong = %lld\n", sampleOffset_asLong, nbSamplesPart1_asLong)
1023 993
1024 994 // (6) compute the final acquisition time
1025 995 acquisitionTime_asLong = bufferAcquisitionTime_asLong +
1026 996 sampleOffset_asLong * nbTicksPerSample_asLong;
1027 997
1028 998 // (7) copy the acquisition time at the beginning of the extrated snapshot
1029 999 ptr1 = (unsigned char*) &acquisitionTime_asLong;
1030 1000 // fine time
1031 1001 ptr2 = (unsigned char*) &ring_node_wf_snap_extracted.fineTime;
1032 1002 ptr2[2] = ptr1[ 4 + 2 ];
1033 1003 ptr2[3] = ptr1[ 5 + 2 ];
1034 1004 // coarse time
1035 1005 ptr2 = (unsigned char*) &ring_node_wf_snap_extracted.coarseTime;
1036 1006 ptr2[0] = ptr1[ 0 + 2 ];
1037 1007 ptr2[1] = ptr1[ 1 + 2 ];
1038 1008 ptr2[2] = ptr1[ 2 + 2 ];
1039 1009 ptr2[3] = ptr1[ 3 + 2 ];
1040 1010
1041 1011 // re set the synchronization bit
1042 1012 timeCharPtr = (unsigned char*) &ring_node_to_send->coarseTime;
1043 1013 ptr2[0] = ptr2[0] | (timeCharPtr[0] & 0x80); // [1000 0000]
1044 1014
1045 1015 if ( (nbSamplesPart1_asLong >= NB_SAMPLES_PER_SNAPSHOT) | (nbSamplesPart1_asLong < 0) )
1046 1016 {
1047 1017 nbSamplesPart1_asLong = 0;
1048 1018 }
1049 1019 // copy the part 1 of the snapshot in the extracted buffer
1050 1020 for ( i = 0; i < (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i++ )
1051 1021 {
1052 1022 wf_snap_extracted[i] =
1053 1023 ((int*) ring_node_to_send->buffer_address)[ i + (sampleOffset_asLong * NB_WORDS_SWF_BLK) ];
1054 1024 }
1055 1025 // copy the part 2 of the snapshot in the extracted buffer
1056 1026 ring_node_to_send = ring_node_to_send->next;
1057 1027 for ( i = (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i < (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK); i++ )
1058 1028 {
1059 1029 wf_snap_extracted[i] =
1060 1030 ((int*) ring_node_to_send->buffer_address)[ (i-(nbSamplesPart1_asLong * NB_WORDS_SWF_BLK)) ];
1061 1031 }
1062 1032 }
1063 1033
1064 1034 void snapshot_resynchronization( unsigned char *timePtr )
1065 1035 {
1066 1036 unsigned long long int acquisitionTime;
1067 1037 unsigned long long int centerTime;
1068 1038 unsigned long long int previousTick;
1069 1039 unsigned long long int nextTick;
1070 1040 unsigned long long int deltaPreviousTick;
1071 1041 unsigned long long int deltaNextTick;
1072 1042 unsigned int deltaTickInF2;
1073 1043 double deltaPrevious;
1074 1044 double deltaNext;
1075 1045
1076 1046 acquisitionTime = get_acquisition_time( timePtr );
1077 1047
1078 1048 // compute center time
1079 1049 centerTime = acquisitionTime + 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
1080 1050 previousTick = centerTime - (centerTime & 0xffff);
1081 1051 nextTick = previousTick + 65536;
1082 1052
1083 1053 deltaPreviousTick = centerTime - previousTick;
1084 1054 deltaNextTick = nextTick - centerTime;
1085 1055
1086 1056 deltaPrevious = ((double) deltaPreviousTick) / 65536. * 1000.;
1087 1057 deltaNext = ((double) deltaNextTick) / 65536. * 1000.;
1088 1058
1089 1059 PRINTF2("delta previous = %f ms, delta next = %f ms\n", deltaPrevious, deltaNext)
1090 1060 PRINTF2("delta previous = %llu, delta next = %llu\n", deltaPreviousTick, deltaNextTick)
1091 1061
1092 1062 // which tick is the closest
1093 1063 if (deltaPreviousTick > deltaNextTick)
1094 1064 {
1095 1065 deltaTickInF2 = floor( (deltaNext * 256. / 1000.) ); // the division by 2 is important here
1096 1066 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot + deltaTickInF2;
1097 1067 printf("correction of = + %u\n", deltaTickInF2);
1098 1068 }
1099 1069 else
1100 1070 {
1101 1071 deltaTickInF2 = floor( (deltaPrevious * 256. / 1000.) ); // the division by 2 is important here
1102 1072 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot - deltaTickInF2;
1103 1073 printf("correction of = - %u\n", deltaTickInF2);
1104 1074 }
1105 1075 }
1106 1076
1107 1077 //**************
1108 1078 // wfp registers
1109 1079 void reset_wfp_burst_enable( void )
1110 1080 {
1111 1081 /** This function resets the waveform picker burst_enable register.
1112 1082 *
1113 1083 * The burst bits [f2 f1 f0] and the enable bits [f3 f2 f1 f0] are set to 0.
1114 1084 *
1115 1085 */
1116 1086
1117 1087 // [1000 000] burst f2, f1, f0 enable f3, f2, f1, f0
1118 1088 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable & 0x80;
1119 1089 }
1120 1090
1121 1091 void reset_wfp_status( void )
1122 1092 {
1123 1093 /** This function resets the waveform picker status register.
1124 1094 *
1125 1095 * All status bits are set to 0 [new_err full_err full].
1126 1096 *
1127 1097 */
1128 1098
1129 1099 waveform_picker_regs->status = 0xffff;
1130 1100 }
1131 1101
1132 1102 void reset_wfp_buffer_addresses( void )
1133 1103 {
1134 1104 // F0
1135 1105 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->previous->buffer_address; // 0x08
1136 1106 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address; // 0x0c
1137 1107 // F1
1138 1108 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->previous->buffer_address; // 0x10
1139 1109 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address; // 0x14
1140 1110 // F2
1141 1111 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->previous->buffer_address; // 0x18
1142 1112 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; // 0x1c
1143 1113 // F3
1144 1114 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->previous->buffer_address; // 0x20
1145 1115 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address; // 0x24
1146 1116 }
1147 1117
1148 1118 void reset_waveform_picker_regs( void )
1149 1119 {
1150 1120 /** This function resets the waveform picker module registers.
1151 1121 *
1152 1122 * The registers affected by this function are located at the following offset addresses:
1153 1123 * - 0x00 data_shaping
1154 1124 * - 0x04 run_burst_enable
1155 1125 * - 0x08 addr_data_f0
1156 1126 * - 0x0C addr_data_f1
1157 1127 * - 0x10 addr_data_f2
1158 1128 * - 0x14 addr_data_f3
1159 1129 * - 0x18 status
1160 1130 * - 0x1C delta_snapshot
1161 1131 * - 0x20 delta_f0
1162 1132 * - 0x24 delta_f0_2
1163 1133 * - 0x28 delta_f1
1164 1134 * - 0x2c delta_f2
1165 1135 * - 0x30 nb_data_by_buffer
1166 1136 * - 0x34 nb_snapshot_param
1167 1137 * - 0x38 start_date
1168 1138 * - 0x3c nb_word_in_buffer
1169 1139 *
1170 1140 */
1171 1141
1172 1142 set_wfp_data_shaping(); // 0x00 *** R1 R0 SP1 SP0 BW
1173 1143
1174 1144 reset_wfp_burst_enable(); // 0x04 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
1175 1145
1176 1146 reset_wfp_buffer_addresses();
1177 1147
1178 1148 reset_wfp_status(); // 0x18
1179 1149
1180 1150 set_wfp_delta_snapshot(); // 0x1c *** 300 s => 0x12bff
1181 1151
1182 1152 set_wfp_delta_f0_f0_2(); // 0x20, 0x24
1183 1153
1184 1154 set_wfp_delta_f1(); // 0x28
1185 1155
1186 1156 set_wfp_delta_f2(); // 0x2c
1187 1157
1188 1158 DEBUG_PRINTF1("delta_snapshot %x\n", waveform_picker_regs->delta_snapshot)
1189 1159 DEBUG_PRINTF1("delta_f0 %x\n", waveform_picker_regs->delta_f0)
1190 1160 DEBUG_PRINTF1("delta_f0_2 %x\n", waveform_picker_regs->delta_f0_2)
1191 1161 DEBUG_PRINTF1("delta_f1 %x\n", waveform_picker_regs->delta_f1)
1192 1162 DEBUG_PRINTF1("delta_f2 %x\n", waveform_picker_regs->delta_f2)
1193 1163 // 2688 = 8 * 336
1194 1164 waveform_picker_regs->nb_data_by_buffer = 0xa7f; // 0x30 *** 2688 - 1 => nb samples -1
1195 1165 waveform_picker_regs->snapshot_param = 0xa80; // 0x34 *** 2688 => nb samples
1196 1166 waveform_picker_regs->start_date = 0x7fffffff; // 0x38
1197 1167 //
1198 1168 // coarse time and fine time registers are not initialized, they are volatile
1199 1169 //
1200 1170 waveform_picker_regs->buffer_length = 0x1f8;// buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
1201 1171 }
1202 1172
1203 1173 void set_wfp_data_shaping( void )
1204 1174 {
1205 1175 /** This function sets the data_shaping register of the waveform picker module.
1206 1176 *
1207 1177 * The value is read from one field of the parameter_dump_packet structure:\n
1208 1178 * bw_sp0_sp1_r0_r1
1209 1179 *
1210 1180 */
1211 1181
1212 1182 unsigned char data_shaping;
1213 1183
1214 1184 // get the parameters for the data shaping [BW SP0 SP1 R0 R1] in sy_lfr_common1 and configure the register
1215 1185 // waveform picker : [R1 R0 SP1 SP0 BW]
1216 1186
1217 1187 data_shaping = parameter_dump_packet.sy_lfr_common_parameters;
1218 1188
1219 1189 waveform_picker_regs->data_shaping =
1220 1190 ( (data_shaping & 0x10) >> 4 ) // BW
1221 1191 + ( (data_shaping & 0x08) >> 2 ) // SP0
1222 1192 + ( (data_shaping & 0x04) ) // SP1
1223 1193 + ( (data_shaping & 0x02) << 2 ) // R0
1224 1194 + ( (data_shaping & 0x01) << 4 ) // R1
1225 1195 + ( (data_shaping & 0x01) << 5 ); // R2
1226 1196 }
1227 1197
1228 1198 void set_wfp_burst_enable_register( unsigned char mode )
1229 1199 {
1230 1200 /** This function sets the waveform picker burst_enable register depending on the mode.
1231 1201 *
1232 1202 * @param mode is the LFR mode to launch.
1233 1203 *
1234 1204 * The burst bits shall be before the enable bits.
1235 1205 *
1236 1206 */
1237 1207
1238 1208 // [0000 0000] burst f2, f1, f0 enable f3 f2 f1 f0
1239 1209 // the burst bits shall be set first, before the enable bits
1240 1210 switch(mode) {
1241 1211 case(LFR_MODE_NORMAL):
1242 1212 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enable
1243 1213 waveform_picker_regs->run_burst_enable = 0x0f; // [0000 1111] enable f3 f2 f1 f0
1244 1214 break;
1245 1215 case(LFR_MODE_BURST):
1246 1216 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1247 1217 // waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x04; // [0100] enable f2
1248 1218 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0c; // [1100] enable f3 AND f2
1249 1219 break;
1250 1220 case(LFR_MODE_SBM1):
1251 1221 waveform_picker_regs->run_burst_enable = 0x20; // [0010 0000] f1 burst enabled
1252 1222 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1253 1223 break;
1254 1224 case(LFR_MODE_SBM2):
1255 1225 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1256 1226 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1257 1227 break;
1258 1228 default:
1259 1229 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enabled, no waveform enabled
1260 1230 break;
1261 1231 }
1262 1232 }
1263 1233
1264 1234 void set_wfp_delta_snapshot( void )
1265 1235 {
1266 1236 /** This function sets the delta_snapshot register of the waveform picker module.
1267 1237 *
1268 1238 * The value is read from two (unsigned char) of the parameter_dump_packet structure:
1269 1239 * - sy_lfr_n_swf_p[0]
1270 1240 * - sy_lfr_n_swf_p[1]
1271 1241 *
1272 1242 */
1273 1243
1274 1244 unsigned int delta_snapshot;
1275 1245 unsigned int delta_snapshot_in_T2;
1276 1246
1277 1247 delta_snapshot = parameter_dump_packet.sy_lfr_n_swf_p[0]*256
1278 1248 + parameter_dump_packet.sy_lfr_n_swf_p[1];
1279 1249
1280 1250 delta_snapshot_in_T2 = delta_snapshot * 256;
1281 1251 waveform_picker_regs->delta_snapshot = delta_snapshot_in_T2 - 1; // max 4 bytes
1282 1252 }
1283 1253
1284 1254 void set_wfp_delta_f0_f0_2( void )
1285 1255 {
1286 1256 unsigned int delta_snapshot;
1287 1257 unsigned int nb_samples_per_snapshot;
1288 1258 float delta_f0_in_float;
1289 1259
1290 1260 delta_snapshot = waveform_picker_regs->delta_snapshot;
1291 1261 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1292 1262 delta_f0_in_float =nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 24576.) * 256.;
1293 1263
1294 1264 waveform_picker_regs->delta_f0 = delta_snapshot - floor( delta_f0_in_float );
1295 1265 waveform_picker_regs->delta_f0_2 = 0x30; // 48 = 11 0000, max 7 bits
1296 1266 }
1297 1267
1298 1268 void set_wfp_delta_f1( void )
1299 1269 {
1300 1270 unsigned int delta_snapshot;
1301 1271 unsigned int nb_samples_per_snapshot;
1302 1272 float delta_f1_in_float;
1303 1273
1304 1274 delta_snapshot = waveform_picker_regs->delta_snapshot;
1305 1275 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1306 1276 delta_f1_in_float = nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 4096.) * 256.;
1307 1277
1308 1278 waveform_picker_regs->delta_f1 = delta_snapshot - floor( delta_f1_in_float );
1309 1279 }
1310 1280
1311 1281 void set_wfp_delta_f2()
1312 1282 {
1313 1283 unsigned int delta_snapshot;
1314 1284 unsigned int nb_samples_per_snapshot;
1315 1285
1316 1286 delta_snapshot = waveform_picker_regs->delta_snapshot;
1317 1287 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1318 1288
1319 1289 waveform_picker_regs->delta_f2 = delta_snapshot - nb_samples_per_snapshot / 2;
1320 1290 }
1321 1291
1322 1292 //*****************
1323 1293 // local parameters
1324 1294
1325 1295 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid )
1326 1296 {
1327 1297 /** This function increments the parameter "sequence_cnt" depending on the sid passed in argument.
1328 1298 *
1329 1299 * @param packet_sequence_control is a pointer toward the parameter sequence_cnt to update.
1330 1300 * @param sid is the source identifier of the packet being updated.
1331 1301 *
1332 1302 * REQ-LFR-SRS-5240 / SSS-CP-FS-590
1333 1303 * The sequence counters shall wrap around from 2^14 to zero.
1334 1304 * The sequence counter shall start at zero at startup.
1335 1305 *
1336 1306 * REQ-LFR-SRS-5239 / SSS-CP-FS-580
1337 1307 * All TM_LFR_SCIENCE_ packets are sent to ground, i.e. destination id = 0
1338 1308 *
1339 1309 */
1340 1310
1341 1311 unsigned short *sequence_cnt;
1342 1312 unsigned short segmentation_grouping_flag;
1343 1313 unsigned short new_packet_sequence_control;
1344 1314 rtems_mode initial_mode_set;
1345 1315 rtems_mode current_mode_set;
1346 1316 rtems_status_code status;
1347 1317
1348 1318 //******************************************
1349 1319 // CHANGE THE MODE OF THE CALLING RTEMS TASK
1350 1320 status = rtems_task_mode( RTEMS_NO_PREEMPT, RTEMS_PREEMPT_MASK, &initial_mode_set );
1351 1321
1352 1322 if ( (sid == SID_NORM_SWF_F0) || (sid == SID_NORM_SWF_F1) || (sid == SID_NORM_SWF_F2)
1353 1323 || (sid == SID_NORM_CWF_F3) || (sid == SID_NORM_CWF_LONG_F3)
1354 1324 || (sid == SID_BURST_CWF_F2)
1355 1325 || (sid == SID_NORM_ASM_F0) || (sid == SID_NORM_ASM_F1) || (sid == SID_NORM_ASM_F2)
1356 1326 || (sid == SID_NORM_BP1_F0) || (sid == SID_NORM_BP1_F1) || (sid == SID_NORM_BP1_F2)
1357 1327 || (sid == SID_NORM_BP2_F0) || (sid == SID_NORM_BP2_F1) || (sid == SID_NORM_BP2_F2)
1358 1328 || (sid == SID_BURST_BP1_F0) || (sid == SID_BURST_BP2_F0)
1359 1329 || (sid == SID_BURST_BP1_F1) || (sid == SID_BURST_BP2_F1) )
1360 1330 {
1361 1331 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_NORMAL_BURST;
1362 1332 }
1363 1333 else if ( (sid ==SID_SBM1_CWF_F1) || (sid ==SID_SBM2_CWF_F2)
1364 1334 || (sid == SID_SBM1_BP1_F0) || (sid == SID_SBM1_BP2_F0)
1365 1335 || (sid == SID_SBM2_BP1_F0) || (sid == SID_SBM2_BP2_F0)
1366 1336 || (sid == SID_SBM2_BP1_F1) || (sid == SID_SBM2_BP2_F1) )
1367 1337 {
1368 1338 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_SBM1_SBM2;
1369 1339 }
1370 1340 else
1371 1341 {
1372 1342 sequence_cnt = (unsigned short *) NULL;
1373 1343 PRINTF1("in increment_seq_counter_source_id *** ERR apid_destid %d not known\n", sid)
1374 1344 }
1375 1345
1376 1346 if (sequence_cnt != NULL)
1377 1347 {
1378 1348 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
1379 1349 *sequence_cnt = (*sequence_cnt) & 0x3fff;
1380 1350
1381 1351 new_packet_sequence_control = segmentation_grouping_flag | (*sequence_cnt) ;
1382 1352
1383 1353 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
1384 1354 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1385 1355
1386 1356 // increment the sequence counter
1387 1357 if ( *sequence_cnt < SEQ_CNT_MAX)
1388 1358 {
1389 1359 *sequence_cnt = *sequence_cnt + 1;
1390 1360 }
1391 1361 else
1392 1362 {
1393 1363 *sequence_cnt = 0;
1394 1364 }
1395 1365 }
1396 1366
1397 1367 //***********************************
1398 1368 // RESET THE MODE OF THE CALLING TASK
1399 1369 status = rtems_task_mode( initial_mode_set, RTEMS_PREEMPT_MASK, &current_mode_set );
1400 1370 }
General Comments 0
You need to be logged in to leave comments. Login now