##// 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

Many corrections, mainly related to 807 Don_Enumeration
paul -
r318:d3701d39af11 R3_plus draft
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,2 +1,2
1 1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
2 3e4216a0e6981bead8bcb201012ebadb53f60dff header/lfr_common_headers
2 6bab694410c69700e3455ffba21ce58dbb4da870 header/lfr_common_headers
Show file before | Show file after | Hide comments
@@ -1,109 +1,131
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 #define LFR_RESET_CAUSE_UNKNOWN_CAUSE 0
14 #define WATCHDOG_LOOP_PRINTF 10
15 #define WATCHDOG_LOOP_DEBUG 3
16
17 #define DUMB_MESSAGE_NB 15
18 #define NB_RTEMS_EVENTS 32
19 #define EVENT_12 12
20 #define EVENT_13 13
21 #define EVENT_14 14
22 #define DUMB_MESSAGE_0 "in DUMB *** default"
23 #define DUMB_MESSAGE_1 "in DUMB *** timecode_irq_handler"
24 #define DUMB_MESSAGE_2 "in DUMB *** f3 buffer changed"
25 #define DUMB_MESSAGE_3 "in DUMB *** in SMIQ *** Error sending event to AVF0"
26 #define DUMB_MESSAGE_4 "in DUMB *** spectral_matrices_isr *** Error sending event to SMIQ"
27 #define DUMB_MESSAGE_5 "in DUMB *** waveforms_simulator_isr"
28 #define DUMB_MESSAGE_6 "VHDL SM *** two buffers f0 ready"
29 #define DUMB_MESSAGE_7 "ready for dump"
30 #define DUMB_MESSAGE_8 "VHDL ERR *** spectral matrix"
31 #define DUMB_MESSAGE_9 "tick"
32 #define DUMB_MESSAGE_10 "VHDL ERR *** waveform picker"
33 #define DUMB_MESSAGE_11 "VHDL ERR *** unexpected ready matrix values"
34 #define DUMB_MESSAGE_12 "WATCHDOG timer"
35 #define DUMB_MESSAGE_13 "TIMECODE timer"
36 #define DUMB_MESSAGE_14 "TIMECODE ISR"
13 37
14 38 enum lfr_reset_cause_t{
15 39 UNKNOWN_CAUSE,
16 40 POWER_ON,
17 41 TC_RESET,
18 42 WATCHDOG,
19 43 ERROR_RESET,
20 44 UNEXP_RESET
21 45 };
22 46
23 47 typedef struct{
24 48 unsigned char dpu_spw_parity;
25 49 unsigned char dpu_spw_disconnect;
26 50 unsigned char dpu_spw_escape;
27 51 unsigned char dpu_spw_credit;
28 52 unsigned char dpu_spw_write_sync;
29 53 unsigned char timecode_erroneous;
30 54 unsigned char timecode_missing;
31 55 unsigned char timecode_invalid;
32 56 unsigned char time_timecode_it;
33 57 unsigned char time_not_synchro;
34 58 unsigned char time_timecode_ctr;
35 59 unsigned char ahb_correctable;
36 60 } hk_lfr_le_t;
37 61
38 62 typedef struct{
39 63 unsigned char dpu_spw_early_eop;
40 64 unsigned char dpu_spw_invalid_addr;
41 65 unsigned char dpu_spw_eep;
42 66 unsigned char dpu_spw_rx_too_big;
43 67 } hk_lfr_me_t;
44 68
45 69 extern gptimer_regs_t *gptimer_regs;
46 70 extern void ASR16_get_FPRF_IURF_ErrorCounters( unsigned int*, unsigned int* );
47 71 extern void CCR_getInstructionAndDataErrorCounters( unsigned int*, unsigned int* );
48 72
49 #define LFR_RESET_CAUSE_UNKNOWN_CAUSE 0
50
51 73 rtems_name name_hk_rate_monotonic; // name of the HK rate monotonic
52 74 rtems_id HK_id; // id of the HK rate monotonic period
53 75 rtems_name name_avgv_rate_monotonic; // name of the AVGV rate monotonic
54 76 rtems_id AVGV_id; // id of the AVGV rate monotonic period
55 77
56 78 void timer_configure( unsigned char timer, unsigned int clock_divider,
57 79 unsigned char interrupt_level, rtems_isr (*timer_isr)() );
58 80 void timer_start( unsigned char timer );
59 81 void timer_stop( unsigned char timer );
60 82 void timer_set_clock_divider(unsigned char timer, unsigned int clock_divider);
61 83
62 84 // WATCHDOG
63 85 rtems_isr watchdog_isr( rtems_vector_number vector );
64 86 void watchdog_configure(void);
65 87 void watchdog_stop(void);
66 88 void watchdog_reload(void);
67 89 void watchdog_start(void);
68 90
69 91 // SERIAL LINK
70 92 int send_console_outputs_on_apbuart_port( void );
71 93 int enable_apbuart_transmitter( void );
72 94 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value);
73 95
74 96 // RTEMS TASKS
75 97 rtems_task load_task( rtems_task_argument argument );
76 98 rtems_task hous_task( rtems_task_argument argument );
77 99 rtems_task avgv_task( rtems_task_argument argument );
78 100 rtems_task dumb_task( rtems_task_argument unused );
79 101
80 102 void init_housekeeping_parameters( void );
81 103 void increment_seq_counter(unsigned short *packetSequenceControl);
82 104 void getTime( unsigned char *time);
83 105 unsigned long long int getTimeAsUnsignedLongLongInt( );
84 106 void send_dumb_hk( void );
85 107 void get_temperatures( unsigned char *temperatures );
86 108 void get_v_e1_e2_f3( unsigned char *spacecraft_potential );
87 109 void get_cpu_load( unsigned char *resource_statistics );
88 110 void set_hk_lfr_sc_potential_flag( bool state );
89 111 void set_sy_lfr_pas_filter_enabled( bool state );
90 112 void set_sy_lfr_watchdog_enabled( bool state );
91 113 void set_hk_lfr_calib_enable( bool state );
92 114 void set_hk_lfr_reset_cause( enum lfr_reset_cause_t lfr_reset_cause );
93 115 void hk_lfr_le_me_he_update();
94 116 void set_hk_lfr_time_not_synchro();
95 117
96 118 extern int sched_yield( void );
97 119 extern void rtems_cpu_usage_reset();
98 120 extern ring_node *current_ring_node_f3;
99 121 extern ring_node *ring_node_to_send_cwf_f3;
100 122 extern ring_node waveform_ring_f3[];
101 123 extern unsigned short sequenceCounterHK;
102 124
103 125 extern unsigned char hk_lfr_q_sd_fifo_size_max;
104 126 extern unsigned char hk_lfr_q_rv_fifo_size_max;
105 127 extern unsigned char hk_lfr_q_p0_fifo_size_max;
106 128 extern unsigned char hk_lfr_q_p1_fifo_size_max;
107 129 extern unsigned char hk_lfr_q_p2_fifo_size_max;
108 130
109 131 #endif // FSW_MISC_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,59 +1,68
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 14 #include "fsw_init.h"
15 15
16 #define SPW_LINK_OK 5
17 #define CONF_TCODE_CTRL 0x0909 // [Time Rx : Time Tx : Link error : Tick-out IRQ]
18 #define SPW_BIT_NP 0x00100000 // [NP] set the No port force bit
19 #define SPW_BIT_NP_MASK 0xffdfffff
20 #define SPW_BIT_RE 0x00010000 // [RE] set the RMAP Enable bit
21 #define SPW_BIT_RE_MASK 0xfffdffff
22 #define SPW_LINK_STAT_POS 21
23 #define SPW_TIMECODE_MAX 63
24
16 25 extern spw_stats grspw_stats;
17 26 extern rtems_name timecode_timer_name;
18 27 extern rtems_id timecode_timer_id;
19 28 extern unsigned char oneTcLfrUpdateTimeReceived;
20 29
21 30 // RTEMS TASK
22 31 rtems_task spiq_task( rtems_task_argument argument );
23 32 rtems_task recv_task( rtems_task_argument unused );
24 33 rtems_task send_task( rtems_task_argument argument );
25 34 rtems_task link_task( rtems_task_argument argument );
26 35
27 36 int spacewire_open_link( void );
28 37 int spacewire_start_link( int fd );
29 38 int spacewire_stop_and_start_link( int fd );
30 39 int spacewire_configure_link(int fd );
31 40 int spacewire_several_connect_attemps( void );
32 41 void spacewire_set_NP( unsigned char val, unsigned int regAddr ); // No Port force
33 42 void spacewire_set_RE( unsigned char val, unsigned int regAddr ); // RMAP Enable
34 43 void spacewire_read_statistics( void );
35 44 void spacewire_get_last_error( void );
36 45 void update_hk_lfr_last_er_fields(unsigned int rid, unsigned char code);
37 46 void update_hk_with_grspw_stats(void );
38 47 void spacewire_update_hk_lfr_link_state( unsigned char *hk_lfr_status_word_0 );
39 48 void increase_unsigned_char_counter( unsigned char *counter );
40 49
41 50 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header );
42 51 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header );
43 52 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header );
44 53 int spw_send_waveform_CWF( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_CWF_t *header );
45 54 int spw_send_waveform_SWF( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_SWF_t *header );
46 55 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_CWF_t *header );
47 56 void spw_send_asm_f0( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_ASM_t *header );
48 57 void spw_send_asm_f1( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_ASM_t *header );
49 58 void spw_send_asm_f2( ring_node *ring_node_to_send, Header_TM_LFR_SCIENCE_ASM_t *header );
50 59 void spw_send_k_dump( ring_node *ring_node_to_send );
51 60
52 61 unsigned int check_timecode_and_previous_timecode_coherency(unsigned char currentTimecodeCtr);
53 62 unsigned int check_timecode_and_internal_time_coherency(unsigned char timecode, unsigned char internalTime);
54 63 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc );
55 64 rtems_timer_service_routine timecode_timer_routine( rtems_id timer_id, void *user_data );
56 65
57 66 void (*grspw_timecode_callback) ( void *pDev, void *regs, int minor, unsigned int tc );
58 67
59 68 #endif // FSW_SPACEWIRE_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,138 +1,231
1 1 #ifndef GRLIB_REGS_H_INCLUDED
2 2 #define GRLIB_REGS_H_INCLUDED
3 3
4 4 #define NB_GPTIMER 3
5 5
6 6 struct apbuart_regs_str{
7 7 volatile unsigned int data;
8 8 volatile unsigned int status;
9 9 volatile unsigned int ctrl;
10 10 volatile unsigned int scaler;
11 11 volatile unsigned int fifoDebug;
12 12 };
13 13
14 14 struct grgpio_regs_str{
15 15 volatile int io_port_data_register;
16 16 int io_port_output_register;
17 17 int io_port_direction_register;
18 18 int interrupt_mak_register;
19 19 int interrupt_polarity_register;
20 20 int interrupt_edge_register;
21 21 int bypass_register;
22 22 int reserved;
23 23 // 0x20-0x3c interrupt map register(s)
24 24 };
25 25
26 26 typedef struct {
27 27 volatile unsigned int counter;
28 28 volatile unsigned int reload;
29 29 volatile unsigned int ctrl;
30 30 volatile unsigned int unused;
31 31 } timer_regs_t;
32 32
33 //*************
34 //*************
35 // GPTIMER_REGS
36
37 #define GPTIMER_CLEAR_IRQ 0x00000010 // clear pending IRQ if any
38 #define GPTIMER_LD 0x00000004 // LD load value from the reload register
39 #define GPTIMER_EN 0x00000001 // EN enable the timer
40 #define GPTIMER_EN_MASK 0xfffffffe // EN enable the timer
41 #define GPTIMER_RS 0x00000002 // RS restart
42 #define GPTIMER_IE 0x00000008 // IE interrupt enable
43 #define GPTIMER_IE_MASK 0xffffffef // IE interrupt enable
44
33 45 typedef struct {
34 46 volatile unsigned int scaler_value;
35 47 volatile unsigned int scaler_reload;
36 48 volatile unsigned int conf;
37 49 volatile unsigned int unused0;
38 50 timer_regs_t timer[NB_GPTIMER];
39 51 } gptimer_regs_t;
40 52
53 //*********************
54 //*********************
55 // TIME_MANAGEMENT_REGS
56
57 #define VAL_SOFTWARE_RESET 0x02 // [0010] software reset
58 #define VAL_LFR_SYNCHRONIZED 0x80000000
59 #define BIT_SYNCHRONIZATION 31
60 #define COARSE_TIME_MASK 0x7fffffff
61 #define SYNC_BIT_MASK 0x7f
62 #define SYNC_BIT 0x80
63 #define BIT_CAL_RELOAD 0x00000010
64 #define MASK_CAL_RELOAD 0xffffffef // [1110 1111]
65 #define BIT_CAL_ENABLE 0x00000040
66 #define MASK_CAL_ENABLE 0xffffffbf // [1011 1111]
67 #define BIT_SET_INTERLEAVED 0x00000020 // [0010 0000]
68 #define MASK_SET_INTERLEAVED 0xffffffdf // [1101 1111]
69 #define BIT_SOFT_RESET 0x00000004 // [0100]
70 #define MASK_SOFT_RESET 0xfffffffb // [1011]
71
41 72 typedef struct {
42 73 volatile int ctrl; // bit 0 forces the load of the coarse_time_load value and resets the fine_time
43 74 // bit 1 is the soft reset for the time management module
44 75 // bit 2 is the soft reset for the waveform picker and the spectral matrix modules, set to 1 after HW reset
45 76 volatile int coarse_time_load;
46 77 volatile int coarse_time;
47 78 volatile int fine_time;
48 79 // TEMPERATURES
49 80 volatile int temp_pcb; // SEL1 = 0 SEL0 = 0
50 81 volatile int temp_fpga; // SEL1 = 0 SEL0 = 1
51 82 volatile int temp_scm; // SEL1 = 1 SEL0 = 0
52 83 // CALIBRATION
53 84 volatile unsigned int calDACCtrl;
54 85 volatile unsigned int calPrescaler;
55 86 volatile unsigned int calDivisor;
56 87 volatile unsigned int calDataPtr;
57 88 volatile unsigned int calData;
58 89 } time_management_regs_t;
59 90
91 //*********************
92 //*********************
93 // WAVEFORM_PICKER_REGS
94
95 #define BITS_WFP_STATUS_F3 0xc0 // [1100 0000] check the f3 full bits
96 #define BIT_WFP_BUF_F3_0 0x40 // [0100 0000] f3 buffer 0 is full
97 #define BIT_WFP_BUF_F3_1 0x80 // [1000 0000] f3 buffer 1 is full
98 #define RST_WFP_F3_0 0x00008840 // [1000 1000 0100 0000]
99 #define RST_WFP_F3_1 0x00008880 // [1000 1000 1000 0000]
100
101 #define BITS_WFP_STATUS_F2 0x30 // [0011 0000] get the status bits for f2
102 #define SHIFT_WFP_STATUS_F2 4
103 #define BIT_WFP_BUF_F2_0 0x10 // [0001 0000] f2 buffer 0 is full
104 #define BIT_WFP_BUF_F2_1 0x20 // [0010 0000] f2 buffer 1 is full
105 #define RST_WFP_F2_0 0x00004410 // [0100 0100 0001 0000]
106 #define RST_WFP_F2_1 0x00004420 // [0100 0100 0010 0000]
107
108 #define BITS_WFP_STATUS_F1 0x0c // [0000 1100] check the f1 full bits
109 #define BIT_WFP_BUF_F1_0 0x04 // [0000 0100] f1 buffer 0 is full
110 #define BIT_WFP_BUF_F1_1 0x08 // [0000 1000] f1 buffer 1 is full
111 #define RST_WFP_F1_0 0x00002204 // [0010 0010 0000 0100] f1 bits = 0
112 #define RST_WFP_F1_1 0x00002208 // [0010 0010 0000 1000] f1 bits = 0
113
114 #define BITS_WFP_STATUS_F0 0x03 // [0000 0011] check the f0 full bits
115 #define RST_WFP_F0_0 0x00001101 // [0001 0001 0000 0001]
116 #define RST_WFP_F0_1 0x00001102 // [0001 0001 0000 0010]
117
118 #define BIT_WFP_BUFFER_0 0x01
119 #define BIT_WFP_BUFFER_1 0x02
120
121 #define RST_BITS_RUN_BURST_EN 0x80 // [1000 0000] burst f2, f1, f0 enable f3, f2, f1, f0
122 #define RUN_BURST_ENABLE_SBM2 0x60 // [0110 0000] enable f2 and f1 burst
123 #define RUN_BURST_ENABLE_BURST 0x40 // [0100 0000] f2 burst enabled
124
125 #define DFLT_WFP_NB_DATA_BY_BUFFER 0xa7f // 0x30 *** 2688 - 1 => nb samples -1
126 #define DFLT_WFP_SNAPSHOT_PARAM 0xa80 // 0x34 *** 2688 => nb samples
127 #define DFLT_WFP_BUFFER_LENGTH 0x1f8 // buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
128 #define DFLT_WFP_DELTA_F0_2 0x30 // 48 = 11 0000, max 7 bits
129
60 130 // PDB >= 0.1.28, 0x80000f54
61 131 typedef struct{
62 132 int data_shaping; // 0x00 00 *** R2 R1 R0 SP1 SP0 BW
63 133 int run_burst_enable; // 0x04 01 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
64 134 int addr_data_f0_0; // 0x08
65 135 int addr_data_f0_1; // 0x0c
66 136 int addr_data_f1_0; // 0x10
67 137 int addr_data_f1_1; // 0x14
68 138 int addr_data_f2_0; // 0x18
69 139 int addr_data_f2_1; // 0x1c
70 140 int addr_data_f3_0; // 0x20
71 141 int addr_data_f3_1; // 0x24
72 142 volatile int status; // 0x28
73 143 volatile int delta_snapshot; // 0x2c
74 144 int delta_f0; // 0x30
75 145 int delta_f0_2; // 0x34
76 146 int delta_f1; // 0x38
77 147 int delta_f2; // 0x3c
78 148 int nb_data_by_buffer; // 0x40 number of samples in a buffer = 2688
79 149 int snapshot_param; // 0x44
80 150 int start_date; // 0x48
81 151 //
82 152 volatile unsigned int f0_0_coarse_time; // 0x4c
83 153 volatile unsigned int f0_0_fine_time; // 0x50
84 154 volatile unsigned int f0_1_coarse_time; // 0x54
85 155 volatile unsigned int f0_1_fine_time; // 0x58
86 156 //
87 157 volatile unsigned int f1_0_coarse_time; // 0x5c
88 158 volatile unsigned int f1_0_fine_time; // 0x60
89 159 volatile unsigned int f1_1_coarse_time; // 0x64
90 160 volatile unsigned int f1_1_fine_time; // 0x68
91 161 //
92 162 volatile unsigned int f2_0_coarse_time; // 0x6c
93 163 volatile unsigned int f2_0_fine_time; // 0x70
94 164 volatile unsigned int f2_1_coarse_time; // 0x74
95 165 volatile unsigned int f2_1_fine_time; // 0x78
96 166 //
97 167 volatile unsigned int f3_0_coarse_time; // 0x7c => 0x7c + 0xf54 = 0xd0
98 168 volatile unsigned int f3_0_fine_time; // 0x80
99 169 volatile unsigned int f3_1_coarse_time; // 0x84
100 170 volatile unsigned int f3_1_fine_time; // 0x88
101 171 //
102 172 unsigned int buffer_length; // 0x8c = buffer length in burst 2688 / 16 = 168
103 173 //
104 174 volatile unsigned int v; // 0x90
105 175 volatile unsigned int e1; // 0x94
106 176 volatile unsigned int e2; // 0x98
107 177 } waveform_picker_regs_0_1_18_t;
108 178
179 //*********************
180 //*********************
181 // SPECTRAL_MATRIX_REGS
182
183 #define BITS_STATUS_F0 0x03 // [0011]
184 #define BITS_STATUS_F1 0x0c // [1100]
185 #define BITS_STATUS_F2 0x30 // [0011 0000]
186 #define BITS_HK_AA_SM 0x780 // [0111 1000 0000]
187 #define BITS_SM_ERR 0x7c0 // [0111 1100 0000]
188 #define BITS_STATUS_REG 0x7ff // [0111 1111 1111]
189 #define BIT_READY_0 0x1 // [01]
190 #define BIT_READY_1 0x2 // [10]
191 #define BIT_READY_0_1 0x3 // [11]
192 #define BIT_STATUS_F1_0 0x04 // [0100]
193 #define BIT_STATUS_F1_1 0x08 // [1000]
194 #define BIT_STATUS_F2_0 0x10 // [0001 0000]
195 #define BIT_STATUS_F2_1 0x20 // [0010 0000]
196 #define DEFAULT_MATRIX_LENGTH 0xc8 // 25 * 128 / 16 = 200 = 0xc8
197 #define BIT_IRQ_ON_NEW_MATRIX 0x01
198 #define MASK_IRQ_ON_NEW_MATRIX 0xfffffffe
199 #define BIT_IRQ_ON_ERROR 0x02
200 #define MASK_IRQ_ON_ERROR 0xfffffffd
201
109 202 typedef struct {
110 203 volatile int config; // 0x00
111 204 volatile int status; // 0x04
112 205 volatile int f0_0_address; // 0x08
113 206 volatile int f0_1_address; // 0x0C
114 207 //
115 208 volatile int f1_0_address; // 0x10
116 209 volatile int f1_1_address; // 0x14
117 210 volatile int f2_0_address; // 0x18
118 211 volatile int f2_1_address; // 0x1C
119 212 //
120 213 volatile unsigned int f0_0_coarse_time; // 0x20
121 214 volatile unsigned int f0_0_fine_time; // 0x24
122 215 volatile unsigned int f0_1_coarse_time; // 0x28
123 216 volatile unsigned int f0_1_fine_time; // 0x2C
124 217 //
125 218 volatile unsigned int f1_0_coarse_time; // 0x30
126 219 volatile unsigned int f1_0_fine_time; // 0x34
127 220 volatile unsigned int f1_1_coarse_time; // 0x38
128 221 volatile unsigned int f1_1_fine_time; // 0x3C
129 222 //
130 223 volatile unsigned int f2_0_coarse_time; // 0x40
131 224 volatile unsigned int f2_0_fine_time; // 0x44
132 225 volatile unsigned int f2_1_coarse_time; // 0x48
133 226 volatile unsigned int f2_1_fine_time; // 0x4C
134 227 //
135 228 unsigned int matrix_length; // 0x50, length of a spectral matrix in burst 3200 / 16 = 200 = 0xc8
136 229 } spectral_matrix_regs_t;
137 230
138 231 #endif // GRLIB_REGS_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,32 +1,36
1 1 #ifndef LFR_CPU_USAGE_REPORT_H
2 2 #define LFR_CPU_USAGE_REPORT_H
3 3
4 4 #ifdef HAVE_CONFIG_H
5 5 #include "config.h"
6 6 #endif
7 7
8 8 #include <rtems.h>
9 9
10 10 #include <assert.h>
11 11 #include <string.h>
12 12 #include <stdlib.h>
13 13 #include <stdio.h>
14 14 #include <ctype.h>
15 15 #include <inttypes.h>
16 16
17 17 #include <rtems/cpuuse.h>
18 18 #include <rtems/bspIo.h>
19 19
20 20 #ifndef __RTEMS_USE_TICKS_FOR_STATISTICS__
21 21 #include <rtems/score/timestamp.h>
22 22 #endif
23 23
24 24 #ifndef __RTEMS_USE_TICKS_FOR_STATISTICS__
25 25 extern Timestamp_Control CPU_usage_Uptime_at_last_reset;
26 26 #else
27 27 extern uint32_t CPU_usage_Ticks_at_last_reset;
28 28 #endif
29 29
30 30 unsigned char lfr_rtems_cpu_usage_report( void );
31 31
32 #define CONST_100 100
33 #define CONST_1000 1000
34 #define CONST_100000 100000
35
32 36 #endif // LFR_CPU_USAGE_REPORT_H
Show file before | Show file after | Hide comments
@@ -1,361 +1,371
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>
9 9 #include <math.h>
10 10 #include <grlib_regs.h>
11 11
12 12 #include "fsw_params.h"
13 13
14 #define SBM_COEFF_PER_NORM_COEFF 2
15 #define MAX_SRC_DATA 780 // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1]
16 #define MAX_SRC_DATA_WITH_SPARE 143 // 13 bins * 11 Bytes
17
14 18 typedef struct ring_node_asm
15 19 {
16 20 struct ring_node_asm *next;
17 21 float matrix[ TOTAL_SIZE_SM ];
18 22 unsigned int status;
19 23 } ring_node_asm;
20 24
21 25 typedef struct
22 26 {
23 27 unsigned char targetLogicalAddress;
24 28 unsigned char protocolIdentifier;
25 29 unsigned char reserved;
26 30 unsigned char userApplication;
27 unsigned char packetID[2];
28 unsigned char packetSequenceControl[2];
29 unsigned char packetLength[2];
31 unsigned char packetID[BYTES_PER_PACKETID];
32 unsigned char packetSequenceControl[BYTES_PER_SEQ_CTRL];
33 unsigned char packetLength[BYTES_PER_PKT_LEN];
30 34 // DATA FIELD HEADER
31 35 unsigned char spare1_pusVersion_spare2;
32 36 unsigned char serviceType;
33 37 unsigned char serviceSubType;
34 38 unsigned char destinationID;
35 unsigned char time[6];
39 unsigned char time[BYTES_PER_TIME];
36 40 // AUXILIARY HEADER
37 41 unsigned char sid;
38 42 unsigned char pa_bia_status_info;
39 43 unsigned char sy_lfr_common_parameters_spare;
40 44 unsigned char sy_lfr_common_parameters;
41 unsigned char acquisitionTime[6];
42 unsigned char pa_lfr_bp_blk_nr[2];
45 unsigned char acquisitionTime[BYTES_PER_TIME];
46 unsigned char pa_lfr_bp_blk_nr[BYTES_PER_BLKNR];
43 47 // SOURCE DATA
44 unsigned char data[ 780 ]; // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1]
48 unsigned char data[ MAX_SRC_DATA ]; // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1]
45 49 } bp_packet;
46 50
47 51 typedef struct
48 52 {
49 53 unsigned char targetLogicalAddress;
50 54 unsigned char protocolIdentifier;
51 55 unsigned char reserved;
52 56 unsigned char userApplication;
53 unsigned char packetID[2];
54 unsigned char packetSequenceControl[2];
55 unsigned char packetLength[2];
57 unsigned char packetID[BYTES_PER_PACKETID];
58 unsigned char packetSequenceControl[BYTES_PER_SEQ_CTRL];
59 unsigned char packetLength[BYTES_PER_PKT_LEN];
56 60 // DATA FIELD HEADER
57 61 unsigned char spare1_pusVersion_spare2;
58 62 unsigned char serviceType;
59 63 unsigned char serviceSubType;
60 64 unsigned char destinationID;
61 unsigned char time[6];
65 unsigned char time[BYTES_PER_TIME];
62 66 // AUXILIARY HEADER
63 67 unsigned char sid;
64 68 unsigned char pa_bia_status_info;
65 69 unsigned char sy_lfr_common_parameters_spare;
66 70 unsigned char sy_lfr_common_parameters;
67 unsigned char acquisitionTime[6];
71 unsigned char acquisitionTime[BYTES_PER_TIME];
68 72 unsigned char source_data_spare;
69 unsigned char pa_lfr_bp_blk_nr[2];
73 unsigned char pa_lfr_bp_blk_nr[BYTES_PER_BLKNR];
70 74 // SOURCE DATA
71 unsigned char data[ 143 ]; // 13 bins * 11 Bytes
75 unsigned char data[ MAX_SRC_DATA_WITH_SPARE ]; // 13 bins * 11 Bytes
72 76 } bp_packet_with_spare; // only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1
73 77
74 78 typedef struct asm_msg
75 79 {
76 80 ring_node_asm *norm;
77 81 ring_node_asm *burst_sbm;
78 82 rtems_event_set event;
79 83 unsigned int coarseTimeNORM;
80 84 unsigned int fineTimeNORM;
81 85 unsigned int coarseTimeSBM;
82 86 unsigned int fineTimeSBM;
83 87 unsigned int numberOfSMInASMNORM;
84 88 unsigned int numberOfSMInASMSBM;
85 89 } asm_msg;
86 90
87 91 extern unsigned char thisIsAnASMRestart;
88 92
89 93 extern volatile int sm_f0[ ];
90 94 extern volatile int sm_f1[ ];
91 95 extern volatile int sm_f2[ ];
92 96 extern unsigned int acquisitionDurations[];
93 97
94 98 // parameters
95 99 extern struct param_local_str param_local;
96 100 extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
97 101
98 102 // registers
99 103 extern time_management_regs_t *time_management_regs;
100 104 extern volatile spectral_matrix_regs_t *spectral_matrix_regs;
101 105
102 extern rtems_name misc_name[5];
103 extern rtems_id Task_id[20]; /* array of task ids */
106 extern rtems_name misc_name[];
107 extern rtems_id Task_id[]; /* array of task ids */
104 108
105 109 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel);
106 110 // ISR
107 111 rtems_isr spectral_matrices_isr( rtems_vector_number vector );
108 112
109 113 //******************
110 114 // Spectral Matrices
111 115 void reset_nb_sm( void );
112 116 // SM
113 117 void SM_init_rings( void );
114 118 void SM_reset_current_ring_nodes( void );
115 119 // ASM
116 120 void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes );
117 121
118 122 //*****************
119 123 // Basic Parameters
120 124
121 125 void BP_reset_current_ring_nodes( void );
122 126 void BP_init_header(bp_packet *packet,
123 127 unsigned int apid, unsigned char sid,
124 128 unsigned int packetLength , unsigned char blkNr);
125 129 void BP_init_header_with_spare(bp_packet_with_spare *packet,
126 130 unsigned int apid, unsigned char sid,
127 131 unsigned int packetLength, unsigned char blkNr );
128 132 void BP_send( char *data,
129 133 rtems_id queue_id,
130 134 unsigned int nbBytesToSend , unsigned int sid );
131 135 void BP_send_s1_s2(char *data,
132 136 rtems_id queue_id,
133 137 unsigned int nbBytesToSend, unsigned int sid );
134 138
135 139 //******************
136 140 // general functions
137 141 void reset_sm_status( void );
138 142 void reset_spectral_matrix_regs( void );
139 143 void set_time(unsigned char *time, unsigned char *timeInBuffer );
140 144 unsigned long long int get_acquisition_time( unsigned char *timePtr );
141 145 unsigned char getSID( rtems_event_set event );
142 146
143 147 extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
144 148 extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
145 149
146 150 //***************************************
147 151 // DEFINITIONS OF STATIC INLINE FUNCTIONS
148 152 static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
149 153 ring_node *ring_node_tab[],
150 154 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
151 155 asm_msg *msgForMATR , unsigned char channel);
152 156
153 157 void ASM_patch( float *inputASM, float *outputASM );
154 158
155 159 void extractReImVectors(float *inputASM, float *outputASM, unsigned int asmComponent );
156 160
157 161 static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized,
158 162 float divider );
159 163
160 164 static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat,
161 165 float divider,
162 166 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
163 167
164 168 static inline void ASM_convert(volatile float *input_matrix, char *output_matrix);
165 169
166 170 unsigned char acquisitionTimeIsValid(unsigned int coarseTime, unsigned int fineTime, unsigned char channel);
167 171
168 172 void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
169 173 ring_node *ring_node_tab[],
170 174 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
171 175 asm_msg *msgForMATR, unsigned char channel )
172 176 {
173 177 float sum;
174 178 unsigned int i;
175 179 unsigned int k;
176 unsigned char incomingSMIsValid[8];
180 unsigned char incomingSMIsValid[NB_SM_BEFORE_AVF0_F1];
177 181 unsigned int numberOfValidSM;
178 182 unsigned char isValid;
179 183
180 184 //**************
181 185 // PAS FILTERING
182 186 // check acquisitionTime of the incoming data
183 187 numberOfValidSM = 0;
184 for (k=0; k<8; k++)
188 for (k=0; k<NB_SM_BEFORE_AVF0_F1; k++)
185 189 {
186 190 isValid = acquisitionTimeIsValid( ring_node_tab[k]->coarseTime, ring_node_tab[k]->fineTime, channel );
187 191 incomingSMIsValid[k] = isValid;
188 192 numberOfValidSM = numberOfValidSM + isValid;
189 193 }
190 194
191 195 //************************
192 196 // AVERAGE SPECTRAL MATRIX
193 197 for(i=0; i<TOTAL_SIZE_SM; i++)
194 198 {
195 199 // sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ]
196 200 // + ( (int *) (ring_node_tab[1]->buffer_address) ) [ i ]
197 201 // + ( (int *) (ring_node_tab[2]->buffer_address) ) [ i ]
198 202 // + ( (int *) (ring_node_tab[3]->buffer_address) ) [ i ]
199 203 // + ( (int *) (ring_node_tab[4]->buffer_address) ) [ i ]
200 204 // + ( (int *) (ring_node_tab[5]->buffer_address) ) [ i ]
201 205 // + ( (int *) (ring_node_tab[6]->buffer_address) ) [ i ]
202 206 // + ( (int *) (ring_node_tab[7]->buffer_address) ) [ i ];
203 207
204 sum = ( (incomingSMIsValid[0] == 1) ? ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ] : 0.0 )
205 + ( (incomingSMIsValid[1] == 1) ? ( (int *) (ring_node_tab[1]->buffer_address) ) [ i ] : 0.0 )
206 + ( (incomingSMIsValid[2] == 1) ? ( (int *) (ring_node_tab[2]->buffer_address) ) [ i ] : 0.0 )
207 + ( (incomingSMIsValid[3] == 1) ? ( (int *) (ring_node_tab[3]->buffer_address) ) [ i ] : 0.0 )
208 + ( (incomingSMIsValid[4] == 1) ? ( (int *) (ring_node_tab[4]->buffer_address) ) [ i ] : 0.0 )
209 + ( (incomingSMIsValid[5] == 1) ? ( (int *) (ring_node_tab[5]->buffer_address) ) [ i ] : 0.0 )
210 + ( (incomingSMIsValid[6] == 1) ? ( (int *) (ring_node_tab[6]->buffer_address) ) [ i ] : 0.0 )
211 + ( (incomingSMIsValid[7] == 1) ? ( (int *) (ring_node_tab[7]->buffer_address) ) [ i ] : 0.0 );
208 sum = ( incomingSMIsValid[0] * ((int *)(ring_node_tab[0]->buffer_address) )[ i ] )
209 + ( incomingSMIsValid[1] * ((int *)(ring_node_tab[1]->buffer_address) )[ i ] )
210 + ( incomingSMIsValid[2] * ((int *)(ring_node_tab[2]->buffer_address) )[ i ] )
211 + ( incomingSMIsValid[3] * ((int *)(ring_node_tab[3]->buffer_address) )[ i ] )
212 + ( incomingSMIsValid[4] * ((int *)(ring_node_tab[4]->buffer_address) )[ i ] )
213 + ( incomingSMIsValid[5] * ((int *)(ring_node_tab[5]->buffer_address) )[ i ] )
214 + ( incomingSMIsValid[6] * ((int *)(ring_node_tab[6]->buffer_address) )[ i ] )
215 + ( incomingSMIsValid[7] * ((int *)(ring_node_tab[7]->buffer_address) )[ i ] );
212 216
213 217 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
214 218 {
215 219 averaged_spec_mat_NORM[ i ] = sum;
216 220 averaged_spec_mat_SBM[ i ] = sum;
217 221 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
218 222 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
219 223 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
220 224 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
221 225 }
222 226 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
223 227 {
224 228 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
225 229 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
226 230 }
227 231 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
228 232 {
229 233 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
230 234 averaged_spec_mat_SBM[ i ] = sum;
231 235 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
232 236 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
233 237 }
234 238 else
235 239 {
236 240 averaged_spec_mat_NORM[ i ] = sum;
237 241 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
238 242 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
239 243 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
240 244 // PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
241 245 }
242 246 }
243 247
244 248 //*******************
245 249 // UPDATE SM COUNTERS
246 250 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
247 251 {
248 252 msgForMATR->numberOfSMInASMNORM = numberOfValidSM;
249 253 msgForMATR->numberOfSMInASMSBM = numberOfValidSM;
250 254 }
251 255 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
252 256 {
253 257 msgForMATR->numberOfSMInASMNORM = msgForMATR->numberOfSMInASMNORM + numberOfValidSM;
254 258 msgForMATR->numberOfSMInASMSBM = msgForMATR->numberOfSMInASMSBM + numberOfValidSM;
255 259 }
256 260 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
257 261 {
258 262 msgForMATR->numberOfSMInASMNORM = msgForMATR->numberOfSMInASMNORM + numberOfValidSM;
259 263 msgForMATR->numberOfSMInASMSBM = numberOfValidSM;
260 264 }
261 265 else
262 266 {
263 267 msgForMATR->numberOfSMInASMNORM = numberOfValidSM;
264 268 msgForMATR->numberOfSMInASMSBM = msgForMATR->numberOfSMInASMSBM + numberOfValidSM;
265 269 }
266 270 }
267 271
268 272 void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
269 273 {
270 274 int frequencyBin;
271 275 int asmComponent;
272 276 unsigned int offsetASM;
273 277 unsigned int offsetASMReorganized;
274 278
275 279 // BUILD DATA
276 280 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
277 281 {
278 282 for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
279 283 {
280 284 offsetASMReorganized =
281 frequencyBin * NB_VALUES_PER_SM
285 (frequencyBin * NB_VALUES_PER_SM)
282 286 + asmComponent;
283 287 offsetASM =
284 asmComponent * NB_BINS_PER_SM
288 (asmComponent * NB_BINS_PER_SM)
285 289 + frequencyBin;
286 averaged_spec_mat_reorganized[offsetASMReorganized ] =
287 (divider != 0.0) ? averaged_spec_mat[ offsetASM ] / divider : 0.0;
290 if ( divider != INIT_FLOAT )
291 {
292 averaged_spec_mat_reorganized[offsetASMReorganized ] = averaged_spec_mat[ offsetASM ] / divider;
293 }
294 else
295 {
296 averaged_spec_mat_reorganized[offsetASMReorganized ] = INIT_FLOAT;
297 }
288 298 }
289 299 }
290 300 }
291 301
292 302 void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
293 303 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
294 304 {
295 305 int frequencyBin;
296 306 int asmComponent;
297 307 int offsetASM;
298 308 int offsetCompressed;
299 309 int k;
300 310
301 311 // BUILD DATA
302 312 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
303 313 {
304 314 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
305 315 {
306 316 offsetCompressed = // NO TIME OFFSET
307 frequencyBin * NB_VALUES_PER_SM
317 (frequencyBin * NB_VALUES_PER_SM)
308 318 + asmComponent;
309 319 offsetASM = // NO TIME OFFSET
310 asmComponent * NB_BINS_PER_SM
320 (asmComponent * NB_BINS_PER_SM)
311 321 + ASMIndexStart
312 + frequencyBin * nbBinsToAverage;
322 + (frequencyBin * nbBinsToAverage);
313 323 compressed_spec_mat[ offsetCompressed ] = 0;
314 324 for ( k = 0; k < nbBinsToAverage; k++ )
315 325 {
316 326 compressed_spec_mat[offsetCompressed ] =
317 327 ( compressed_spec_mat[ offsetCompressed ]
318 328 + averaged_spec_mat[ offsetASM + k ] );
319 329 }
320 330 compressed_spec_mat[ offsetCompressed ] =
321 331 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
322 332 }
323 333 }
324 334 }
325 335
326 336 void ASM_convert( volatile float *input_matrix, char *output_matrix)
327 337 {
328 338 unsigned int frequencyBin;
329 339 unsigned int asmComponent;
330 340 char * pt_char_input;
331 341 char * pt_char_output;
332 342 unsigned int offsetInput;
333 343 unsigned int offsetOutput;
334 344
335 345 pt_char_input = (char*) &input_matrix;
336 346 pt_char_output = (char*) &output_matrix;
337 347
338 348 // convert all other data
339 349 for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++)
340 350 {
341 351 for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++)
342 352 {
343 offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ;
344 offsetOutput = 2 * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ;
353 offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ;
354 offsetOutput = SM_BYTES_PER_VAL * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ;
345 355 pt_char_input = (char*) &input_matrix [ offsetInput ];
346 356 pt_char_output = (char*) &output_matrix[ offsetOutput ];
347 357 pt_char_output[0] = pt_char_input[0]; // bits 31 downto 24 of the float
348 358 pt_char_output[1] = pt_char_input[1]; // bits 23 downto 16 of the float
349 359 }
350 360 }
351 361 }
352 362
353 363 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat,
354 364 float divider,
355 365 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart, unsigned char channel);
356 366
357 367 int getFBinMask(int k, unsigned char channel);
358 368
359 369 void init_kcoeff_sbm_from_kcoeff_norm( float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm);
360 370
361 371 #endif // FSW_PROCESSING_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,25 +1,44
1 1 #ifndef TC_ACCEPTANCE_H_INCLUDED
2 2 #define TC_ACCEPTANCE_H_INCLUDED
3 3
4 //#include "tm_lfr_tc_exe.h"
5 4 #include "fsw_params.h"
6 5
6 #define BIT_0 0x01
7 #define BIT_1 0x02
8 #define BIT_2 0x04
9 #define BIT_3 0x08
10 #define BIT_4 0x10
11 #define BIT_5 0x20
12 #define BIT_6 0x40
13 #define BIT_7 0x80
14
15 #define CONST_CRC_0 0x1021
16 #define CONST_CRC_1 0x2042
17 #define CONST_CRC_2 0x4084
18 #define CONST_CRC_3 0x8108
19 #define CONST_CRC_4 0x1231
20 #define CONST_CRC_5 0x2462
21 #define CONST_CRC_6 0x48c4
22 #define CONST_CRC_7 0x9188
23
24 #define CRC_RESET 0xffff
25
7 26 //**********************
8 27 // GENERAL USE FUNCTIONS
9 28 unsigned int Crc_opt( unsigned char D, unsigned int Chk);
10 29 void initLookUpTableForCRC( void );
11 30 void GetCRCAsTwoBytes(unsigned char* data, unsigned char* crcAsTwoBytes, unsigned int sizeOfData);
12 31
13 32 //*********************
14 33 // ACCEPTANCE FUNCTIONS
15 34 int tc_parser( ccsdsTelecommandPacket_t * TCPacket, unsigned int estimatedPacketLength, unsigned char *computed_CRC );
16 35 int tc_check_type( unsigned char packetType );
17 36 int tc_check_type_subtype( unsigned char packetType, unsigned char packetSubType );
18 37 int tc_check_sid( unsigned char sid );
19 38 int tc_check_length( unsigned char packetType, unsigned int length );
20 39 int tc_check_crc(ccsdsTelecommandPacket_t * TCPacket, unsigned int length , unsigned char *computed_CRC);
21 40
22 41 #endif // TC_ACCEPTANCE_H_INCLUDED
23 42
24 43
25 44
Show file before | Show file after | Hide comments
@@ -1,81 +1,111
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 #define MAX_DELTA_COARSE_TIME 3
16 #define NB_SCIENCE_TASKS 10
17 #define NB_ASM_TASKS 6
18 #define STATUS_0 0
19 #define STATUS_1 1
20 #define STATUS_2 2
21 #define STATUS_3 3
22 #define STATUS_4 4
23 #define STATUS_5 5
24 #define STATUS_6 6
25 #define STATUS_7 7
26 #define STATUS_8 8
27 #define STATUS_9 9
28
29 #define CAL_F0 625
30 #define CAL_F1 10000
31 #define CAL_FS 160256.410
32 #define CAL_SCALE_FACTOR (0.250 / 0.000654) // 191, 500 mVpp, 2 sinus waves => 500 mVpp each, amplitude = 250 mV
33 #define CAL_NB_PTS 256
34 #define CAL_DATA_MASK 0xfff
35 #define CAL_F_DIVISOR 38 // 25 MHz => 160 256 (39 - 1)
36 // INTERLEAVED MODE
37 #define CAL_FS_INTER 240384.615
38 #define CAL_NB_PTS_INTER 384
39 #define CAL_DATA_MASK_INTER 0x3f
40 #define CAL_DATA_SHIFT_INTER 12
41 #define BYTES_FOR_2_SAMPLES 3 // one need 3 bytes = 24 bits to store 3 samples of 12 bits in interleaved mode
42 #define STEPS_FOR_STORAGE_INTER 128
43 #define CAL_F_DIVISOR_INTER 26 // 25 MHz => 240 384
44
15 45 extern unsigned int lastValidEnterModeTime;
16 46 extern unsigned char oneTcLfrUpdateTimeReceived;
17 47
18 48 //****
19 49 // ISR
20 50 rtems_isr commutation_isr1( rtems_vector_number vector );
21 51 rtems_isr commutation_isr2( rtems_vector_number vector );
22 52
23 53 //***********
24 54 // RTEMS TASK
25 55 rtems_task actn_task( rtems_task_argument unused );
26 56
27 57 //***********
28 58 // TC ACTIONS
29 59 int action_reset( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time );
30 60 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id);
31 61 int action_update_info( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
32 62 int action_enable_calibration( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time );
33 63 int action_disable_calibration( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time );
34 64 int action_update_time( ccsdsTelecommandPacket_t *TC);
35 65
36 66 // mode transition
37 67 int check_mode_value( unsigned char requestedMode );
38 68 int check_mode_transition( unsigned char requestedMode );
39 69 void update_last_valid_transition_date( unsigned int transitionCoarseTime );
40 70 int check_transition_date( unsigned int transitionCoarseTime );
41 71 int stop_spectral_matrices( void );
42 72 int stop_current_mode( void );
43 73 int enter_mode_standby(void );
44 74 int enter_mode_normal( unsigned int transitionCoarseTime );
45 75 int enter_mode_burst( unsigned int transitionCoarseTime );
46 76 int enter_mode_sbm1( unsigned int transitionCoarseTime );
47 77 int enter_mode_sbm2( unsigned int transitionCoarseTime );
48 78 int restart_science_tasks( unsigned char lfrRequestedMode );
49 79 int restart_asm_tasks(unsigned char lfrRequestedMode );
50 80 int suspend_science_tasks(void);
51 81 int suspend_asm_tasks( void );
52 82 void launch_waveform_picker( unsigned char mode , unsigned int transitionCoarseTime );
53 83 void launch_spectral_matrix( void );
54 84 void set_sm_irq_onNewMatrix( unsigned char value );
55 85 void set_sm_irq_onError( unsigned char value );
56 86
57 87 // other functions
58 88 void updateLFRCurrentMode(unsigned char requestedMode);
59 89 void set_lfr_soft_reset( unsigned char value );
60 90 void reset_lfr( void );
61 91 // CALIBRATION
62 92 void setCalibrationPrescaler( unsigned int prescaler );
63 93 void setCalibrationDivisor( unsigned int divisionFactor );
64 94 void setCalibrationData( void );
65 95 void setCalibrationReload( bool state);
66 96 void setCalibrationEnable( bool state );
67 97 void setCalibrationInterleaved( bool state );
68 98 void setCalibration( bool state );
69 99 void configureCalibration( bool interleaved );
70 100 //
71 101 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC , unsigned char *time );
72 102 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC , unsigned char *time );
73 103 void close_action( ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id );
74 104
75 105 extern rtems_status_code get_message_queue_id_send( rtems_id *queue_id );
76 106 extern rtems_status_code get_message_queue_id_recv( rtems_id *queue_id );
77 107
78 108 #endif // TC_HANDLER_H_INCLUDED
79 109
80 110
81 111
Show file before | Show file after | Hide comments
@@ -1,83 +1,101
1 1 #ifndef TC_LOAD_DUMP_PARAMETERS_H
2 2 #define TC_LOAD_DUMP_PARAMETERS_H
3 3
4 4 #include <rtems.h>
5 5 #include <stdio.h>
6 6
7 7 #include "fsw_params.h"
8 8 #include "wf_handler.h"
9 9 #include "tm_lfr_tc_exe.h"
10 10 #include "fsw_misc.h"
11 11 #include "basic_parameters_params.h"
12 12 #include "avf0_prc0.h"
13 13
14 #define FLOAT_EQUAL_ZERO 0.001
14 #define FLOAT_EQUAL_ZERO 0.001
15 #define NB_BINS_TO_REMOVE 3
16 #define FI_INTERVAL_COEFF 0.285
17 #define BIN_MIN 0
18 #define BIN_MAX 127
19 #define DELTAF_F0 96.
20 #define DELTAF_F1 16.
21 #define DELTAF_F2 1.
22
23 #define BIT_RW1_F1 0x80
24 #define BIT_RW1_F2 0x40
25 #define BIT_RW2_F1 0x20
26 #define BIT_RW2_F2 0x10
27 #define BIT_RW3_F1 0x08
28 #define BIT_RW3_F2 0x04
29 #define BIT_RW4_F1 0x02
30 #define BIT_RW4_F2 0x01
31
32 #define SBM_KCOEFF_PER_NORM_KCOEFF 2
15 33
16 34 extern unsigned short sequenceCounterParameterDump;
17 35 extern unsigned short sequenceCounters_TM_DUMP[];
18 36 extern float k_coeff_intercalib_f0_norm[ ];
19 37 extern float k_coeff_intercalib_f0_sbm[ ];
20 38 extern float k_coeff_intercalib_f1_norm[ ];
21 39 extern float k_coeff_intercalib_f1_sbm[ ];
22 40 extern float k_coeff_intercalib_f2[ ];
23 41 extern fbins_masks_t fbins_masks;
24 42
25 43 int action_load_common_par( ccsdsTelecommandPacket_t *TC );
26 44 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
27 45 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
28 46 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
29 47 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
30 48 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
31 49 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
32 50 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
33 51 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
34 52 int action_dump_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
35 53
36 54 // NORMAL
37 55 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
38 56 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC );
39 57 int set_sy_lfr_n_swf_p( ccsdsTelecommandPacket_t *TC );
40 58 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC );
41 59 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC );
42 60 int set_sy_lfr_n_bp_p1( ccsdsTelecommandPacket_t *TC );
43 61 int set_sy_lfr_n_cwf_long_f3( ccsdsTelecommandPacket_t *TC );
44 62
45 63 // BURST
46 64 int set_sy_lfr_b_bp_p0( ccsdsTelecommandPacket_t *TC );
47 65 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC );
48 66
49 67 // SBM1
50 68 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC );
51 69 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC );
52 70
53 71 // SBM2
54 72 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC );
55 73 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC );
56 74
57 75 // TC_LFR_UPDATE_INFO
58 76 unsigned int check_update_info_hk_lfr_mode( unsigned char mode );
59 77 unsigned int check_update_info_hk_tds_mode( unsigned char mode );
60 78 unsigned int check_update_info_hk_thr_mode( unsigned char mode );
61 79 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC );
62 80 void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag );
63 81 void build_sy_lfr_rw_mask( unsigned int channel );
64 82 void build_sy_lfr_rw_masks();
65 83 void merge_fbins_masks( void );
66 84
67 85 // FBINS_MASK
68 86 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC );
69 87
70 88 // TC_LFR_LOAD_PARS_FILTER_PAR
71 89 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
72 90
73 91 // KCOEFFICIENTS
74 92 int set_sy_lfr_kcoeff(ccsdsTelecommandPacket_t *TC , rtems_id queue_id);
75 93 void copyFloatByChar( unsigned char *destination, unsigned char *source );
76 94 void floatToChar( float value, unsigned char* ptr);
77 95
78 96 void init_parameter_dump( void );
79 97 void init_kcoefficients_dump( void );
80 98 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr );
81 99 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id );
82 100
83 101 #endif // TC_LOAD_DUMP_PARAMETERS_H
Show file before | Show file after | Hide comments
@@ -1,89 +1,116
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 10 #include "fsw_init.h"
11 11 #include "fsw_params_wf_handler.h"
12 12
13 13 #define pi 3.14159265359
14 #define T0_IN_FINETIME ( 65536. / 24576. )
15 #define T1_IN_FINETIME ( 65536. / 4096. )
16 #define T2_IN_FINETIME ( 65536. / 256. )
17 #define T3_IN_FINETIME ( 65536. / 16. )
18
19 #define TICKS_PER_T1 16
20 #define TICKS_PER_T2 256
21 #define TICKS_PER_S 65536.
22 #define MS_PER_S 1000.
23
24 #define FREQ_F0 24576.
25 #define FREQ_F1 4096.
26 #define FREQ_F2 256.
27 #define FREQ_F3 16.
28
29 #define DELTAT_F0 2731 // (2048. / 24576. / 2.) * 65536. = 2730.667;
30 #define DELTAT_F1 16384 // (2048. / 4096. / 2.) * 65536. = 16384;
31 #define DELTAT_F2 262144 // (2048. / 256. / 2.) * 65536. = 262144;
32
33 #define OFFSET_2_BYTES 2
34
35 #define ONE_TICK_CORR_INTERVAL_0_MIN 0.5
36 #define ONE_TICK_CORR_INTERVAL_0_MAX 1.0
37 #define ONE_TICK_CORR_INTERVAL_1_MIN -1.0
38 #define ONE_TICK_CORR_INTERVAL_1_MAX -0.5
39 #define ONE_TICK_CORR 1
40 #define CORR_MULT 2
14 41
15 42 extern int fdSPW;
16 43
17 44 //*****************
18 45 // waveform buffers
19 46 extern volatile int wf_buffer_f0[ ];
20 47 extern volatile int wf_buffer_f1[ ];
21 48 extern volatile int wf_buffer_f2[ ];
22 49 extern volatile int wf_buffer_f3[ ];
23 50
24 51 extern waveform_picker_regs_0_1_18_t *waveform_picker_regs;
25 52 extern time_management_regs_t *time_management_regs;
26 53 extern Packet_TM_LFR_HK_t housekeeping_packet;
27 54 extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
28 55 extern struct param_local_str param_local;
29 56
30 57 extern unsigned short sequenceCounters_SCIENCE_NORMAL_BURST;
31 58 extern unsigned short sequenceCounters_SCIENCE_SBM1_SBM2;
32 59
33 extern rtems_id Task_id[20]; /* array of task ids */
60 extern rtems_id Task_id[]; /* array of task ids */
34 61
35 62 extern unsigned char lfrCurrentMode;
36 63
37 64 //**********
38 65 // RTEMS_ISR
39 66 void reset_extractSWF( void );
40 67 rtems_isr waveforms_isr( rtems_vector_number vector );
41 68
42 69 //***********
43 70 // RTEMS_TASK
44 71 rtems_task wfrm_task( rtems_task_argument argument );
45 72 rtems_task cwf3_task( rtems_task_argument argument );
46 73 rtems_task cwf2_task( rtems_task_argument argument );
47 74 rtems_task cwf1_task( rtems_task_argument argument );
48 75 rtems_task swbd_task( rtems_task_argument argument );
49 76
50 77 //******************
51 78 // general functions
52 79 void WFP_init_rings( void );
53 80 void init_ring( ring_node ring[], unsigned char nbNodes, volatile int buffer[] , unsigned int bufferSize );
54 81 void WFP_reset_current_ring_nodes( void );
55 82 //
56 83 int init_header_continuous_cwf3_light_table( Header_TM_LFR_SCIENCE_CWF_t *headerCWF );
57 84 //
58 85 int send_waveform_CWF3_light(ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id );
59 86 //
60 87 void compute_acquisition_time(unsigned int coarseTime, unsigned int fineTime,
61 88 unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char *acquisitionTime );
62 89 void build_snapshot_from_ring(ring_node *ring_node_to_send, unsigned char frequencyChannel ,
63 90 unsigned long long acquisitionTimeF0_asLong, ring_node *ring_node_swf_extracted, int *swf_extracted);
64 91 double computeCorrection( unsigned char *timePtr );
65 92 void applyCorrection( double correction );
66 93 void snapshot_resynchronization( unsigned char *timePtr );
67 94 //
68 95 rtems_id get_pkts_queue_id( void );
69 96
70 97 //**************
71 98 // wfp registers
72 99 // RESET
73 100 void reset_wfp_burst_enable( void );
74 101 void reset_wfp_status( void );
75 102 void reset_wfp_buffer_addresses( void );
76 103 void reset_waveform_picker_regs( void );
77 104 // SET
78 105 void set_wfp_data_shaping(void);
79 106 void set_wfp_burst_enable_register( unsigned char mode );
80 107 void set_wfp_delta_snapshot( void );
81 108 void set_wfp_delta_f0_f0_2( void );
82 109 void set_wfp_delta_f1( void );
83 110 void set_wfp_delta_f2( void );
84 111
85 112 //*****************
86 113 // local parameters
87 114 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid );
88 115
89 116 #endif // WF_HANDLER_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,35 +1,36
1 1 #include <drvmgr/ambapp_bus.h>
2 2 #include <drvmgr/drvmgr.h>
3 #include <ccsds_types.h>
3 4
4 5 // GRSPW0 resources
5 6 struct drvmgr_key grlib_grspw_0n1_res[] =
6 7 {
7 {"txBdCnt", KEY_TYPE_INT, {(unsigned int)50}}, // 7 SWF_F0, 7 SWF_F1, 7 SWF_F2, 7 CWF_F3, 7 CWF_F1 ou 7 CWF_F2
8 {"rxBdCnt", KEY_TYPE_INT, {(unsigned int)10}},
9 {"txDataSize", KEY_TYPE_INT, {(unsigned int)4096}},
10 {"txHdrSize", KEY_TYPE_INT, {(unsigned int)34}},
11 {"rxPktSize", KEY_TYPE_INT, {(unsigned int)200}},
12 KEY_EMPTY
8 {"txBdCnt", KEY_TYPE_INT, {(unsigned int)TXBDCNT}}, // 7 SWF_F0, 7 SWF_F1, 7 SWF_F2, 7 CWF_F3, 7 CWF_F1 ou 7 CWF_F2
9 {"rxBdCnt", KEY_TYPE_INT, {(unsigned int)RXBDCNT}},
10 {"txDataSize", KEY_TYPE_INT, {(unsigned int)TXDATASIZE}},
11 {"txHdrSize", KEY_TYPE_INT, {(unsigned int)TXHDRSIZE}},
12 {"rxPktSize", KEY_TYPE_INT, {(unsigned int)RXPKTSIZE}},
13 KEY_EMPTY
13 14 };
14 15
15 16 // If RTEMS_DRVMGR_STARTUP is defined we override the "weak defaults" that is defined by the LEON3 BSP.
16 17
17 18 //struct drvmgr_bus_res grlib_drv_resources =
18 19 //{
19 20 // .next = NULL,
20 21 // .resource = {
21 22 // {DRIVER_AMBAPP_GAISLER_GRSPW_ID, 0, &grlib_grspw_0n1_res[0]},
22 23 // {DRIVER_AMBAPP_GAISLER_GRSPW_ID, 1, &grlib_grspw_0n1_res[0]},
23 24 // RES_EMPTY /* Mark end of resource array */
24 25 // }
25 26 //};
26 27
27 28 struct drvmgr_bus_res grlib_drv_resources =
28 29 {
29 30 NULL,
30 31 {
31 32 {DRIVER_AMBAPP_GAISLER_GRSPW_ID, 0, &grlib_grspw_0n1_res[0]},
32 33 {DRIVER_AMBAPP_GAISLER_GRSPW_ID, 1, &grlib_grspw_0n1_res[0]},
33 34 RES_EMPTY /* Mark end of resource array */
34 35 }
35 36 };
Show file before | Show file after | Hide comments
@@ -1,98 +1,102
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 #define NB_OF_TASKS 20
26 #define NB_OF_MISC_NAMES 5
27
25 28 // RTEMS GLOBAL VARIABLES
26 rtems_name misc_name[5];
27 rtems_name Task_name[20]; /* array of task names */
28 rtems_id Task_id[20]; /* array of task ids */
29 rtems_name misc_name[NB_OF_MISC_NAMES];
30 rtems_name Task_name[NB_OF_TASKS]; /* array of task names */
31 rtems_id Task_id[NB_OF_TASKS]; /* array of task ids */
29 32 rtems_name timecode_timer_name;
30 33 rtems_id timecode_timer_id;
31 34 int fdSPW = 0;
32 35 int fdUART = 0;
33 36 unsigned char lfrCurrentMode;
34 37 unsigned char pa_bia_status_info;
35 38 unsigned char thisIsAnASMRestart = 0;
36 39 unsigned char oneTcLfrUpdateTimeReceived = 0;
37 40
38 41 // WAVEFORMS GLOBAL VARIABLES // 2048 * 3 * 4 + 2 * 4 = 24576 + 8 bytes = 24584
39 42 // 97 * 256 = 24832 => delta = 248 bytes = 62 words
40 43 // WAVEFORMS GLOBAL VARIABLES // 2688 * 3 * 4 + 2 * 4 = 32256 + 8 bytes = 32264
41 44 // 127 * 256 = 32512 => delta = 248 bytes = 62 words
42 45 // F0 F1 F2 F3
43 46 volatile int wf_buffer_f0[ NB_RING_NODES_F0 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
44 47 volatile int wf_buffer_f1[ NB_RING_NODES_F1 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
45 48 volatile int wf_buffer_f2[ NB_RING_NODES_F2 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
46 49 volatile int wf_buffer_f3[ NB_RING_NODES_F3 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
47 50
48 51 //***********************************
49 52 // SPECTRAL MATRICES GLOBAL VARIABLES
50 53
51 54 // alignment constraints for the spectral matrices buffers => the first data after the time (8 bytes) shall be aligned on 0x00
52 55 volatile int sm_f0[ NB_RING_NODES_SM_F0 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
53 56 volatile int sm_f1[ NB_RING_NODES_SM_F1 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
54 57 volatile int sm_f2[ NB_RING_NODES_SM_F2 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
55 58
56 59 // APB CONFIGURATION REGISTERS
57 60 time_management_regs_t *time_management_regs = (time_management_regs_t*) REGS_ADDR_TIME_MANAGEMENT;
58 61 gptimer_regs_t *gptimer_regs = (gptimer_regs_t *) REGS_ADDR_GPTIMER;
59 62 waveform_picker_regs_0_1_18_t *waveform_picker_regs = (waveform_picker_regs_0_1_18_t*) REGS_ADDR_WAVEFORM_PICKER;
60 63 spectral_matrix_regs_t *spectral_matrix_regs = (spectral_matrix_regs_t*) REGS_ADDR_SPECTRAL_MATRIX;
61 64
62 65 // MODE PARAMETERS
63 66 Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
64 67 struct param_local_str param_local;
65 68 unsigned int lastValidEnterModeTime;
66 69
67 70 // HK PACKETS
68 71 Packet_TM_LFR_HK_t housekeeping_packet;
69 72 unsigned char cp_rpw_sc_rw_f_flags;
70 73 // message queues occupancy
71 74 unsigned char hk_lfr_q_sd_fifo_size_max;
72 75 unsigned char hk_lfr_q_rv_fifo_size_max;
73 76 unsigned char hk_lfr_q_p0_fifo_size_max;
74 77 unsigned char hk_lfr_q_p1_fifo_size_max;
75 78 unsigned char hk_lfr_q_p2_fifo_size_max;
76 79 // sequence counters are incremented by APID (PID + CAT) and destination ID
77 80 unsigned short sequenceCounters_SCIENCE_NORMAL_BURST;
78 81 unsigned short sequenceCounters_SCIENCE_SBM1_SBM2;
79 82 unsigned short sequenceCounters_TC_EXE[SEQ_CNT_NB_DEST_ID];
80 83 unsigned short sequenceCounters_TM_DUMP[SEQ_CNT_NB_DEST_ID];
81 84 unsigned short sequenceCounterHK;
82 85 spw_stats grspw_stats;
83 86
84 87 // TC_LFR_UPDATE_INFO
85 88 float cp_rpw_sc_rw1_f1;
86 89 float cp_rpw_sc_rw1_f2;
87 90 float cp_rpw_sc_rw2_f1;
88 91 float cp_rpw_sc_rw2_f2;
89 92 float cp_rpw_sc_rw3_f1;
90 93 float cp_rpw_sc_rw3_f2;
91 94 float cp_rpw_sc_rw4_f1;
92 95 float cp_rpw_sc_rw4_f2;
93 96
94 97 // TC_LFR_LOAD_FILTER_PAR
95 98 filterPar_t filterPar;
96 99
97 100 fbins_masks_t fbins_masks;
98 unsigned int acquisitionDurations[3] = {ACQUISITION_DURATION_F0, ACQUISITION_DURATION_F1, ACQUISITION_DURATION_F2};
101 unsigned int acquisitionDurations[NB_ACQUISITION_DURATION]
102 = {ACQUISITION_DURATION_F0, ACQUISITION_DURATION_F1, ACQUISITION_DURATION_F2};
Show file before | Show file after | Hide comments
@@ -1,938 +1,938
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 // [spiq] [link] [spacewire_reset_link]
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
51 51 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
52 52 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
53 53 #endif
54 54
55 55 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
56 56 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
57 57 #endif
58 58
59 59 #endif
60 60 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
61 61
62 62 #include <drvmgr/drvmgr_confdefs.h>
63 63 #endif
64 64
65 65 #include "fsw_init.h"
66 66 #include "fsw_config.c"
67 67 #include "GscMemoryLPP.hpp"
68 68
69 69 void initCache()
70 70 {
71 71 // ASI 2 contains a few control registers that have not been assigned as ancillary state registers.
72 72 // These should only be read and written using 32-bit LDA/STA instructions.
73 73 // All cache registers are accessed through load/store operations to the alternate address space (LDA/STA), using ASI = 2.
74 74 // The table below shows the register addresses:
75 75 // 0x00 Cache control register
76 76 // 0x04 Reserved
77 77 // 0x08 Instruction cache configuration register
78 78 // 0x0C Data cache configuration register
79 79
80 80 // Cache Control Register Leon3 / Leon3FT
81 81 // 31..30 29 28 27..24 23 22 21 20..19 18 17 16
82 82 // RFT PS TB DS FD FI FT ST IB
83 83 // 15 14 13..12 11..10 9..8 7..6 5 4 3..2 1..0
84 84 // IP DP ITE IDE DTE DDE DF IF DCS ICS
85 85
86 86 unsigned int cacheControlRegister;
87 87
88 88 CCR_resetCacheControlRegister();
89 89 ASR16_resetRegisterProtectionControlRegister();
90 90
91 91 cacheControlRegister = CCR_getValue();
92 92 PRINTF1("(0) CCR - Cache Control Register = %x\n", cacheControlRegister);
93 93 PRINTF1("(0) ASR16 = %x\n", *asr16Ptr);
94 94
95 95 CCR_enableInstructionCache(); // ICS bits
96 96 CCR_enableDataCache(); // DCS bits
97 97 CCR_enableInstructionBurstFetch(); // IB bit
98 98
99 99 faultTolerantScheme();
100 100
101 101 cacheControlRegister = CCR_getValue();
102 102 PRINTF1("(1) CCR - Cache Control Register = %x\n", cacheControlRegister);
103 103 PRINTF1("(1) ASR16 Register protection control register = %x\n", *asr16Ptr);
104 104
105 105 PRINTF("\n");
106 106 }
107 107
108 108 rtems_task Init( rtems_task_argument ignored )
109 109 {
110 110 /** This is the RTEMS INIT taks, it is the first task launched by the system.
111 111 *
112 112 * @param unused is the starting argument of the RTEMS task
113 113 *
114 114 * The INIT task create and run all other RTEMS tasks.
115 115 *
116 116 */
117 117
118 118 //***********
119 119 // INIT CACHE
120 120
121 121 unsigned char *vhdlVersion;
122 122
123 123 reset_lfr();
124 124
125 125 reset_local_time();
126 126
127 127 rtems_cpu_usage_reset();
128 128
129 129 rtems_status_code status;
130 130 rtems_status_code status_spw;
131 131 rtems_isr_entry old_isr_handler;
132 132
133 133 // UART settings
134 134 enable_apbuart_transmitter();
135 135 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
136 136
137 137 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
138 138
139 139
140 140 PRINTF("\n\n\n\n\n")
141 141
142 142 initCache();
143 143
144 144 PRINTF("*************************\n")
145 145 PRINTF("** LFR Flight Software **\n")
146 146
147 147 PRINTF1("** %d-", SW_VERSION_N1)
148 148 PRINTF1("%d-" , SW_VERSION_N2)
149 149 PRINTF1("%d-" , SW_VERSION_N3)
150 150 PRINTF1("%d **\n", SW_VERSION_N4)
151 151
152 152 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
153 153 PRINTF("** VHDL **\n")
154 154 PRINTF1("** %d.", vhdlVersion[1])
155 155 PRINTF1("%d." , vhdlVersion[2])
156 156 PRINTF1("%d **\n", vhdlVersion[3])
157 157 PRINTF("*************************\n")
158 158 PRINTF("\n\n")
159 159
160 160 init_parameter_dump();
161 161 init_kcoefficients_dump();
162 162 init_local_mode_parameters();
163 163 init_housekeeping_parameters();
164 164 init_k_coefficients_prc0();
165 165 init_k_coefficients_prc1();
166 166 init_k_coefficients_prc2();
167 pa_bia_status_info = 0x00;
168 cp_rpw_sc_rw_f_flags = 0x00;
169 cp_rpw_sc_rw1_f1 = 0.0;
170 cp_rpw_sc_rw1_f2 = 0.0;
171 cp_rpw_sc_rw2_f1 = 0.0;
172 cp_rpw_sc_rw2_f2 = 0.0;
173 cp_rpw_sc_rw3_f1 = 0.0;
174 cp_rpw_sc_rw3_f2 = 0.0;
175 cp_rpw_sc_rw4_f1 = 0.0;
176 cp_rpw_sc_rw4_f2 = 0.0;
167 pa_bia_status_info = INIT_CHAR;
168 cp_rpw_sc_rw_f_flags = INIT_CHAR;
169 cp_rpw_sc_rw1_f1 = INIT_FLOAT;
170 cp_rpw_sc_rw1_f2 = INIT_FLOAT;
171 cp_rpw_sc_rw2_f1 = INIT_FLOAT;
172 cp_rpw_sc_rw2_f2 = INIT_FLOAT;
173 cp_rpw_sc_rw3_f1 = INIT_FLOAT;
174 cp_rpw_sc_rw3_f2 = INIT_FLOAT;
175 cp_rpw_sc_rw4_f1 = INIT_FLOAT;
176 cp_rpw_sc_rw4_f2 = INIT_FLOAT;
177 177 // initialize filtering parameters
178 178 filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED;
179 179 filterPar.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
180 180 filterPar.sy_lfr_pas_filter_tbad = DEFAULT_SY_LFR_PAS_FILTER_TBAD;
181 181 filterPar.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
182 182 filterPar.sy_lfr_pas_filter_shift = DEFAULT_SY_LFR_PAS_FILTER_SHIFT;
183 183 filterPar.sy_lfr_sc_rw_delta_f = DEFAULT_SY_LFR_SC_RW_DELTA_F;
184 184 update_last_valid_transition_date( DEFAULT_LAST_VALID_TRANSITION_DATE );
185 185
186 186 // waveform picker initialization
187 187 WFP_init_rings();
188 188 LEON_Clear_interrupt( IRQ_SPARC_GPTIMER_WATCHDOG ); // initialize the waveform rings
189 189 WFP_reset_current_ring_nodes();
190 190 reset_waveform_picker_regs();
191 191
192 192 // spectral matrices initialization
193 193 SM_init_rings(); // initialize spectral matrices rings
194 194 SM_reset_current_ring_nodes();
195 195 reset_spectral_matrix_regs();
196 196
197 197 // configure calibration
198 198 configureCalibration( false ); // true means interleaved mode, false is for normal mode
199 199
200 200 updateLFRCurrentMode( LFR_MODE_STANDBY );
201 201
202 202 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
203 203
204 204 create_names(); // create all names
205 205
206 206 status = create_timecode_timer(); // create the timer used by timecode_irq_handler
207 207 if (status != RTEMS_SUCCESSFUL)
208 208 {
209 209 PRINTF1("in INIT *** ERR in create_timer_timecode, code %d", status)
210 210 }
211 211
212 212 status = create_message_queues(); // create message queues
213 213 if (status != RTEMS_SUCCESSFUL)
214 214 {
215 215 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
216 216 }
217 217
218 218 status = create_all_tasks(); // create all tasks
219 219 if (status != RTEMS_SUCCESSFUL)
220 220 {
221 221 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
222 222 }
223 223
224 224 // **************************
225 225 // <SPACEWIRE INITIALIZATION>
226 226 status_spw = spacewire_open_link(); // (1) open the link
227 227 if ( status_spw != RTEMS_SUCCESSFUL )
228 228 {
229 229 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
230 230 }
231 231
232 232 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
233 233 {
234 234 status_spw = spacewire_configure_link( fdSPW );
235 235 if ( status_spw != RTEMS_SUCCESSFUL )
236 236 {
237 237 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
238 238 }
239 239 }
240 240
241 241 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
242 242 {
243 243 status_spw = spacewire_start_link( fdSPW );
244 244 if ( status_spw != RTEMS_SUCCESSFUL )
245 245 {
246 246 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
247 247 }
248 248 }
249 249 // </SPACEWIRE INITIALIZATION>
250 250 // ***************************
251 251
252 252 status = start_all_tasks(); // start all tasks
253 253 if (status != RTEMS_SUCCESSFUL)
254 254 {
255 255 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
256 256 }
257 257
258 258 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
259 259 status = start_recv_send_tasks();
260 260 if ( status != RTEMS_SUCCESSFUL )
261 261 {
262 262 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
263 263 }
264 264
265 265 // suspend science tasks, they will be restarted later depending on the mode
266 266 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
267 267 if (status != RTEMS_SUCCESSFUL)
268 268 {
269 269 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
270 270 }
271 271
272 272 // configure IRQ handling for the waveform picker unit
273 273 status = rtems_interrupt_catch( waveforms_isr,
274 274 IRQ_SPARC_WAVEFORM_PICKER,
275 275 &old_isr_handler) ;
276 276 // configure IRQ handling for the spectral matrices unit
277 277 status = rtems_interrupt_catch( spectral_matrices_isr,
278 278 IRQ_SPARC_SPECTRAL_MATRIX,
279 279 &old_isr_handler) ;
280 280
281 281 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
282 282 if ( status_spw != RTEMS_SUCCESSFUL )
283 283 {
284 284 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
285 285 if ( status != RTEMS_SUCCESSFUL ) {
286 286 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
287 287 }
288 288 }
289 289
290 290 BOOT_PRINTF("delete INIT\n")
291 291
292 292 set_hk_lfr_sc_potential_flag( true );
293 293
294 294 // start the timer to detect a missing spacewire timecode
295 295 // the timeout is larger because the spw IP needs to receive several valid timecodes before generating a tickout
296 296 // if a tickout is generated, the timer is restarted
297 297 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT_INIT, timecode_timer_routine, NULL );
298 298
299 299 grspw_timecode_callback = &timecode_irq_handler;
300 300
301 301 status = rtems_task_delete(RTEMS_SELF);
302 302
303 303 }
304 304
305 305 void init_local_mode_parameters( void )
306 306 {
307 307 /** This function initialize the param_local global variable with default values.
308 308 *
309 309 */
310 310
311 311 unsigned int i;
312 312
313 313 // LOCAL PARAMETERS
314 314
315 315 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
316 316 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
317 317
318 318 // init sequence counters
319 319
320 320 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
321 321 {
322 sequenceCounters_TC_EXE[i] = 0x00;
323 sequenceCounters_TM_DUMP[i] = 0x00;
322 sequenceCounters_TC_EXE[i] = INIT_CHAR;
323 sequenceCounters_TM_DUMP[i] = INIT_CHAR;
324 324 }
325 sequenceCounters_SCIENCE_NORMAL_BURST = 0x00;
326 sequenceCounters_SCIENCE_SBM1_SBM2 = 0x00;
327 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
325 sequenceCounters_SCIENCE_NORMAL_BURST = INIT_CHAR;
326 sequenceCounters_SCIENCE_SBM1_SBM2 = INIT_CHAR;
327 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << TM_PACKET_SEQ_SHIFT;
328 328 }
329 329
330 330 void reset_local_time( void )
331 331 {
332 time_management_regs->ctrl = time_management_regs->ctrl | 0x02; // [0010] software reset, coarse time = 0x80000000
332 time_management_regs->ctrl = time_management_regs->ctrl | VAL_SOFTWARE_RESET; // [0010] software reset, coarse time = 0x80000000
333 333 }
334 334
335 335 void create_names( void ) // create all names for tasks and queues
336 336 {
337 337 /** This function creates all RTEMS names used in the software for tasks and queues.
338 338 *
339 339 * @return RTEMS directive status codes:
340 340 * - RTEMS_SUCCESSFUL - successful completion
341 341 *
342 342 */
343 343
344 344 // task names
345 345 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
346 346 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
347 347 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
348 348 Task_name[TASKID_LOAD] = rtems_build_name( 'L', 'O', 'A', 'D' );
349 349 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
350 350 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
351 351 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
352 352 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
353 353 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
354 354 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
355 355 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
356 356 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
357 357 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
358 358 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
359 359 Task_name[TASKID_LINK] = rtems_build_name( 'L', 'I', 'N', 'K' );
360 360 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
361 361 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
362 362 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
363 363 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
364 364
365 365 // rate monotonic period names
366 366 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
367 367
368 368 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
369 369 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
370 370 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
371 371 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
372 372 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
373 373
374 374 timecode_timer_name = rtems_build_name( 'S', 'P', 'T', 'C' );
375 375 }
376 376
377 377 int create_all_tasks( void ) // create all tasks which run in the software
378 378 {
379 379 /** This function creates all RTEMS tasks used in the software.
380 380 *
381 381 * @return RTEMS directive status codes:
382 382 * - RTEMS_SUCCESSFUL - task created successfully
383 383 * - RTEMS_INVALID_ADDRESS - id is NULL
384 384 * - RTEMS_INVALID_NAME - invalid task name
385 385 * - RTEMS_INVALID_PRIORITY - invalid task priority
386 386 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
387 387 * - RTEMS_TOO_MANY - too many tasks created
388 388 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
389 389 * - RTEMS_TOO_MANY - too many global objects
390 390 *
391 391 */
392 392
393 393 rtems_status_code status;
394 394
395 395 //**********
396 396 // SPACEWIRE
397 397 // RECV
398 398 status = rtems_task_create(
399 399 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
400 400 RTEMS_DEFAULT_MODES,
401 401 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
402 402 );
403 403 if (status == RTEMS_SUCCESSFUL) // SEND
404 404 {
405 405 status = rtems_task_create(
406 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * 2,
406 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
407 407 RTEMS_DEFAULT_MODES,
408 408 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
409 409 );
410 410 }
411 411 if (status == RTEMS_SUCCESSFUL) // LINK
412 412 {
413 413 status = rtems_task_create(
414 414 Task_name[TASKID_LINK], TASK_PRIORITY_LINK, RTEMS_MINIMUM_STACK_SIZE,
415 415 RTEMS_DEFAULT_MODES,
416 416 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LINK]
417 417 );
418 418 }
419 419 if (status == RTEMS_SUCCESSFUL) // ACTN
420 420 {
421 421 status = rtems_task_create(
422 422 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
423 423 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
424 424 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
425 425 );
426 426 }
427 427 if (status == RTEMS_SUCCESSFUL) // SPIQ
428 428 {
429 429 status = rtems_task_create(
430 430 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
431 431 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
432 432 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
433 433 );
434 434 }
435 435
436 436 //******************
437 437 // SPECTRAL MATRICES
438 438 if (status == RTEMS_SUCCESSFUL) // AVF0
439 439 {
440 440 status = rtems_task_create(
441 441 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
442 442 RTEMS_DEFAULT_MODES,
443 443 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
444 444 );
445 445 }
446 446 if (status == RTEMS_SUCCESSFUL) // PRC0
447 447 {
448 448 status = rtems_task_create(
449 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * 2,
449 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
450 450 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
451 451 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
452 452 );
453 453 }
454 454 if (status == RTEMS_SUCCESSFUL) // AVF1
455 455 {
456 456 status = rtems_task_create(
457 457 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
458 458 RTEMS_DEFAULT_MODES,
459 459 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
460 460 );
461 461 }
462 462 if (status == RTEMS_SUCCESSFUL) // PRC1
463 463 {
464 464 status = rtems_task_create(
465 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * 2,
465 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
466 466 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
467 467 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
468 468 );
469 469 }
470 470 if (status == RTEMS_SUCCESSFUL) // AVF2
471 471 {
472 472 status = rtems_task_create(
473 473 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
474 474 RTEMS_DEFAULT_MODES,
475 475 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
476 476 );
477 477 }
478 478 if (status == RTEMS_SUCCESSFUL) // PRC2
479 479 {
480 480 status = rtems_task_create(
481 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * 2,
481 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
482 482 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
483 483 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
484 484 );
485 485 }
486 486
487 487 //****************
488 488 // WAVEFORM PICKER
489 489 if (status == RTEMS_SUCCESSFUL) // WFRM
490 490 {
491 491 status = rtems_task_create(
492 492 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
493 493 RTEMS_DEFAULT_MODES,
494 494 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
495 495 );
496 496 }
497 497 if (status == RTEMS_SUCCESSFUL) // CWF3
498 498 {
499 499 status = rtems_task_create(
500 500 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
501 501 RTEMS_DEFAULT_MODES,
502 502 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
503 503 );
504 504 }
505 505 if (status == RTEMS_SUCCESSFUL) // CWF2
506 506 {
507 507 status = rtems_task_create(
508 508 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
509 509 RTEMS_DEFAULT_MODES,
510 510 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
511 511 );
512 512 }
513 513 if (status == RTEMS_SUCCESSFUL) // CWF1
514 514 {
515 515 status = rtems_task_create(
516 516 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
517 517 RTEMS_DEFAULT_MODES,
518 518 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
519 519 );
520 520 }
521 521 if (status == RTEMS_SUCCESSFUL) // SWBD
522 522 {
523 523 status = rtems_task_create(
524 524 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
525 525 RTEMS_DEFAULT_MODES,
526 526 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
527 527 );
528 528 }
529 529
530 530 //*****
531 531 // MISC
532 532 if (status == RTEMS_SUCCESSFUL) // LOAD
533 533 {
534 534 status = rtems_task_create(
535 535 Task_name[TASKID_LOAD], TASK_PRIORITY_LOAD, RTEMS_MINIMUM_STACK_SIZE,
536 536 RTEMS_DEFAULT_MODES,
537 537 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LOAD]
538 538 );
539 539 }
540 540 if (status == RTEMS_SUCCESSFUL) // DUMB
541 541 {
542 542 status = rtems_task_create(
543 543 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
544 544 RTEMS_DEFAULT_MODES,
545 545 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
546 546 );
547 547 }
548 548 if (status == RTEMS_SUCCESSFUL) // HOUS
549 549 {
550 550 status = rtems_task_create(
551 551 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
552 552 RTEMS_DEFAULT_MODES,
553 553 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
554 554 );
555 555 }
556 556
557 557 return status;
558 558 }
559 559
560 560 int start_recv_send_tasks( void )
561 561 {
562 562 rtems_status_code status;
563 563
564 564 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
565 565 if (status!=RTEMS_SUCCESSFUL) {
566 566 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
567 567 }
568 568
569 569 if (status == RTEMS_SUCCESSFUL) // SEND
570 570 {
571 571 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
572 572 if (status!=RTEMS_SUCCESSFUL) {
573 573 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
574 574 }
575 575 }
576 576
577 577 return status;
578 578 }
579 579
580 580 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
581 581 {
582 582 /** This function starts all RTEMS tasks used in the software.
583 583 *
584 584 * @return RTEMS directive status codes:
585 585 * - RTEMS_SUCCESSFUL - ask started successfully
586 586 * - RTEMS_INVALID_ADDRESS - invalid task entry point
587 587 * - RTEMS_INVALID_ID - invalid task id
588 588 * - RTEMS_INCORRECT_STATE - task not in the dormant state
589 589 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
590 590 *
591 591 */
592 592 // starts all the tasks fot eh flight software
593 593
594 594 rtems_status_code status;
595 595
596 596 //**********
597 597 // SPACEWIRE
598 598 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
599 599 if (status!=RTEMS_SUCCESSFUL) {
600 600 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
601 601 }
602 602
603 603 if (status == RTEMS_SUCCESSFUL) // LINK
604 604 {
605 605 status = rtems_task_start( Task_id[TASKID_LINK], link_task, 1 );
606 606 if (status!=RTEMS_SUCCESSFUL) {
607 607 BOOT_PRINTF("in INIT *** Error starting TASK_LINK\n")
608 608 }
609 609 }
610 610
611 611 if (status == RTEMS_SUCCESSFUL) // ACTN
612 612 {
613 613 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
614 614 if (status!=RTEMS_SUCCESSFUL) {
615 615 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
616 616 }
617 617 }
618 618
619 619 //******************
620 620 // SPECTRAL MATRICES
621 621 if (status == RTEMS_SUCCESSFUL) // AVF0
622 622 {
623 623 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
624 624 if (status!=RTEMS_SUCCESSFUL) {
625 625 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
626 626 }
627 627 }
628 628 if (status == RTEMS_SUCCESSFUL) // PRC0
629 629 {
630 630 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
631 631 if (status!=RTEMS_SUCCESSFUL) {
632 632 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
633 633 }
634 634 }
635 635 if (status == RTEMS_SUCCESSFUL) // AVF1
636 636 {
637 637 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
638 638 if (status!=RTEMS_SUCCESSFUL) {
639 639 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
640 640 }
641 641 }
642 642 if (status == RTEMS_SUCCESSFUL) // PRC1
643 643 {
644 644 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
645 645 if (status!=RTEMS_SUCCESSFUL) {
646 646 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
647 647 }
648 648 }
649 649 if (status == RTEMS_SUCCESSFUL) // AVF2
650 650 {
651 651 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
652 652 if (status!=RTEMS_SUCCESSFUL) {
653 653 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
654 654 }
655 655 }
656 656 if (status == RTEMS_SUCCESSFUL) // PRC2
657 657 {
658 658 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
659 659 if (status!=RTEMS_SUCCESSFUL) {
660 660 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
661 661 }
662 662 }
663 663
664 664 //****************
665 665 // WAVEFORM PICKER
666 666 if (status == RTEMS_SUCCESSFUL) // WFRM
667 667 {
668 668 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
669 669 if (status!=RTEMS_SUCCESSFUL) {
670 670 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
671 671 }
672 672 }
673 673 if (status == RTEMS_SUCCESSFUL) // CWF3
674 674 {
675 675 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
676 676 if (status!=RTEMS_SUCCESSFUL) {
677 677 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
678 678 }
679 679 }
680 680 if (status == RTEMS_SUCCESSFUL) // CWF2
681 681 {
682 682 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
683 683 if (status!=RTEMS_SUCCESSFUL) {
684 684 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
685 685 }
686 686 }
687 687 if (status == RTEMS_SUCCESSFUL) // CWF1
688 688 {
689 689 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
690 690 if (status!=RTEMS_SUCCESSFUL) {
691 691 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
692 692 }
693 693 }
694 694 if (status == RTEMS_SUCCESSFUL) // SWBD
695 695 {
696 696 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
697 697 if (status!=RTEMS_SUCCESSFUL) {
698 698 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
699 699 }
700 700 }
701 701
702 702 //*****
703 703 // MISC
704 704 if (status == RTEMS_SUCCESSFUL) // HOUS
705 705 {
706 706 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
707 707 if (status!=RTEMS_SUCCESSFUL) {
708 708 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
709 709 }
710 710 }
711 711 if (status == RTEMS_SUCCESSFUL) // DUMB
712 712 {
713 713 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
714 714 if (status!=RTEMS_SUCCESSFUL) {
715 715 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
716 716 }
717 717 }
718 718 if (status == RTEMS_SUCCESSFUL) // LOAD
719 719 {
720 720 status = rtems_task_start( Task_id[TASKID_LOAD], load_task, 1 );
721 721 if (status!=RTEMS_SUCCESSFUL) {
722 722 BOOT_PRINTF("in INIT *** Error starting TASK_LOAD\n")
723 723 }
724 724 }
725 725
726 726 return status;
727 727 }
728 728
729 729 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
730 730 {
731 731 rtems_status_code status_recv;
732 732 rtems_status_code status_send;
733 733 rtems_status_code status_q_p0;
734 734 rtems_status_code status_q_p1;
735 735 rtems_status_code status_q_p2;
736 736 rtems_status_code ret;
737 737 rtems_id queue_id;
738 738
739 739 //****************************************
740 740 // create the queue for handling valid TCs
741 741 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
742 742 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
743 743 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
744 744 if ( status_recv != RTEMS_SUCCESSFUL ) {
745 745 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
746 746 }
747 747
748 748 //************************************************
749 749 // create the queue for handling TM packet sending
750 750 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
751 751 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
752 752 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
753 753 if ( status_send != RTEMS_SUCCESSFUL ) {
754 754 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
755 755 }
756 756
757 757 //*****************************************************************************
758 758 // create the queue for handling averaged spectral matrices for processing @ f0
759 759 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
760 760 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
761 761 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
762 762 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
763 763 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
764 764 }
765 765
766 766 //*****************************************************************************
767 767 // create the queue for handling averaged spectral matrices for processing @ f1
768 768 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
769 769 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
770 770 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
771 771 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
772 772 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
773 773 }
774 774
775 775 //*****************************************************************************
776 776 // create the queue for handling averaged spectral matrices for processing @ f2
777 777 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
778 778 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
779 779 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
780 780 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
781 781 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
782 782 }
783 783
784 784 if ( status_recv != RTEMS_SUCCESSFUL )
785 785 {
786 786 ret = status_recv;
787 787 }
788 788 else if( status_send != RTEMS_SUCCESSFUL )
789 789 {
790 790 ret = status_send;
791 791 }
792 792 else if( status_q_p0 != RTEMS_SUCCESSFUL )
793 793 {
794 794 ret = status_q_p0;
795 795 }
796 796 else if( status_q_p1 != RTEMS_SUCCESSFUL )
797 797 {
798 798 ret = status_q_p1;
799 799 }
800 800 else
801 801 {
802 802 ret = status_q_p2;
803 803 }
804 804
805 805 return ret;
806 806 }
807 807
808 808 rtems_status_code create_timecode_timer( void )
809 809 {
810 810 rtems_status_code status;
811 811
812 812 status = rtems_timer_create( timecode_timer_name, &timecode_timer_id );
813 813
814 814 if ( status != RTEMS_SUCCESSFUL )
815 815 {
816 816 PRINTF1("in create_timer_timecode *** ERR creating SPTC timer, %d\n", status)
817 817 }
818 818 else
819 819 {
820 820 PRINTF("in create_timer_timecode *** OK creating SPTC timer\n")
821 821 }
822 822
823 823 return status;
824 824 }
825 825
826 826 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
827 827 {
828 828 rtems_status_code status;
829 829 rtems_name queue_name;
830 830
831 831 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
832 832
833 833 status = rtems_message_queue_ident( queue_name, 0, queue_id );
834 834
835 835 return status;
836 836 }
837 837
838 838 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
839 839 {
840 840 rtems_status_code status;
841 841 rtems_name queue_name;
842 842
843 843 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
844 844
845 845 status = rtems_message_queue_ident( queue_name, 0, queue_id );
846 846
847 847 return status;
848 848 }
849 849
850 850 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
851 851 {
852 852 rtems_status_code status;
853 853 rtems_name queue_name;
854 854
855 855 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
856 856
857 857 status = rtems_message_queue_ident( queue_name, 0, queue_id );
858 858
859 859 return status;
860 860 }
861 861
862 862 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
863 863 {
864 864 rtems_status_code status;
865 865 rtems_name queue_name;
866 866
867 867 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
868 868
869 869 status = rtems_message_queue_ident( queue_name, 0, queue_id );
870 870
871 871 return status;
872 872 }
873 873
874 874 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
875 875 {
876 876 rtems_status_code status;
877 877 rtems_name queue_name;
878 878
879 879 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
880 880
881 881 status = rtems_message_queue_ident( queue_name, 0, queue_id );
882 882
883 883 return status;
884 884 }
885 885
886 886 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max )
887 887 {
888 888 u_int32_t count;
889 889 rtems_status_code status;
890 890
891 891 status = rtems_message_queue_get_number_pending( queue_id, &count );
892 892
893 893 count = count + 1;
894 894
895 895 if (status != RTEMS_SUCCESSFUL)
896 896 {
897 897 PRINTF1("in update_queue_max_count *** ERR = %d\n", status)
898 898 }
899 899 else
900 900 {
901 901 if (count > *fifo_size_max)
902 902 {
903 903 *fifo_size_max = count;
904 904 }
905 905 }
906 906 }
907 907
908 908 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
909 909 {
910 910 unsigned char i;
911 911
912 912 //***************
913 913 // BUFFER ADDRESS
914 914 for(i=0; i<nbNodes; i++)
915 915 {
916 ring[i].coarseTime = 0xffffffff;
917 ring[i].fineTime = 0xffffffff;
918 ring[i].sid = 0x00;
919 ring[i].status = 0x00;
916 ring[i].coarseTime = INT32_ALL_F;
917 ring[i].fineTime = INT32_ALL_F;
918 ring[i].sid = INIT_CHAR;
919 ring[i].status = INIT_CHAR;
920 920 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
921 921 }
922 922
923 923 //*****
924 924 // NEXT
925 925 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
926 926 for(i=0; i<nbNodes-1; i++)
927 927 {
928 928 ring[i].next = (ring_node*) &ring[ i + 1 ];
929 929 }
930 930
931 931 //*********
932 932 // PREVIOUS
933 933 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
934 934 for(i=1; i<nbNodes; i++)
935 935 {
936 936 ring[i].previous = (ring_node*) &ring[ i - 1 ];
937 937 }
938 938 }
Show file before | Show file after | Hide comments
@@ -1,983 +1,993
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 timer_configure(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 gptimer_regs->timer[timer].ctrl = 0x00; // reset the control register
28 gptimer_regs->timer[timer].ctrl = INIT_CHAR; // 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( timer, clock_divider);
37 37 }
38 38
39 39 void timer_start(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 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000010; // clear pending IRQ if any
49 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000004; // LD load value from the reload register
50 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000001; // EN enable the timer
51 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000002; // RS restart
52 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000008; // IE interrupt enable
48 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_CLEAR_IRQ;
49 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_LD;
50 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_EN;
51 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_RS;
52 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_IE;
53 53 }
54 54
55 55 void timer_stop(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 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & 0xfffffffe; // EN enable the timer
65 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & 0xffffffef; // IE interrupt enable
66 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000010; // clear pending IRQ if any
64 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & GPTIMER_EN_MASK;
65 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & GPTIMER_IE_MASK;
66 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_CLEAR_IRQ;
67 67 }
68 68
69 69 void timer_set_clock_divider(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 // WATCHDOG
83 83
84 84 rtems_isr watchdog_isr( rtems_vector_number vector )
85 85 {
86 86 rtems_status_code status_code;
87 87
88 88 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_12 );
89 89
90 90 PRINTF("watchdog_isr *** this is the end, exit(0)\n");
91 91
92 92 exit(0);
93 93 }
94 94
95 95 void watchdog_configure(void)
96 96 {
97 97 /** This function configure the watchdog.
98 98 *
99 99 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
100 100 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
101 101 *
102 102 * The watchdog is a timer provided by the GPTIMER IP core of the GRLIB.
103 103 *
104 104 */
105 105
106 106 LEON_Mask_interrupt( IRQ_GPTIMER_WATCHDOG ); // mask gptimer/watchdog interrupt during configuration
107 107
108 108 timer_configure( TIMER_WATCHDOG, CLKDIV_WATCHDOG, IRQ_SPARC_GPTIMER_WATCHDOG, watchdog_isr );
109 109
110 110 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG ); // clear gptimer/watchdog interrupt
111 111 }
112 112
113 113 void watchdog_stop(void)
114 114 {
115 115 LEON_Mask_interrupt( IRQ_GPTIMER_WATCHDOG ); // mask gptimer/watchdog interrupt line
116 116 timer_stop( TIMER_WATCHDOG );
117 117 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG ); // clear gptimer/watchdog interrupt
118 118 }
119 119
120 120 void watchdog_reload(void)
121 121 {
122 122 /** This function reloads the watchdog timer counter with the timer reload value.
123 123 *
124 124 * @param void
125 125 *
126 126 * @return void
127 127 *
128 128 */
129 129
130 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | 0x00000004; // LD load value from the reload register
130 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_LD;
131 131 }
132 132
133 133 void watchdog_start(void)
134 134 {
135 135 /** This function starts the watchdog timer.
136 136 *
137 137 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
138 138 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
139 139 *
140 140 */
141 141
142 142 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG );
143 143
144 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | 0x00000010; // clear pending IRQ if any
145 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | 0x00000004; // LD load value from the reload register
146 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | 0x00000001; // EN enable the timer
147 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | 0x00000008; // IE interrupt enable
144 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_CLEAR_IRQ;
145 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_LD;
146 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_EN;
147 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_IE;
148 148
149 149 LEON_Unmask_interrupt( IRQ_GPTIMER_WATCHDOG );
150 150
151 151 }
152 152
153 153 int enable_apbuart_transmitter( void ) // set the bit 1, TE Transmitter Enable to 1 in the APBUART control register
154 154 {
155 155 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
156 156
157 157 apbuart_regs->ctrl = APBUART_CTRL_REG_MASK_TE;
158 158
159 159 return 0;
160 160 }
161 161
162 162 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value)
163 163 {
164 164 /** This function sets the scaler reload register of the apbuart module
165 165 *
166 166 * @param regs is the address of the apbuart registers in memory
167 167 * @param value is the value that will be stored in the scaler register
168 168 *
169 169 * The value shall be set by the software to get data on the serial interface.
170 170 *
171 171 */
172 172
173 173 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) regs;
174 174
175 175 apbuart_regs->scaler = value;
176 176
177 177 BOOT_PRINTF1("OK *** apbuart port scaler reload register set to 0x%x\n", value)
178 178 }
179 179
180 180 //************
181 181 // RTEMS TASKS
182 182
183 183 rtems_task load_task(rtems_task_argument argument)
184 184 {
185 185 BOOT_PRINTF("in LOAD *** \n")
186 186
187 187 rtems_status_code status;
188 188 unsigned int i;
189 189 unsigned int j;
190 190 rtems_name name_watchdog_rate_monotonic; // name of the watchdog rate monotonic
191 191 rtems_id watchdog_period_id; // id of the watchdog rate monotonic period
192 192
193 193 name_watchdog_rate_monotonic = rtems_build_name( 'L', 'O', 'A', 'D' );
194 194
195 195 status = rtems_rate_monotonic_create( name_watchdog_rate_monotonic, &watchdog_period_id );
196 196 if( status != RTEMS_SUCCESSFUL ) {
197 197 PRINTF1( "in LOAD *** rtems_rate_monotonic_create failed with status of %d\n", status )
198 198 }
199 199
200 200 i = 0;
201 201 j = 0;
202 202
203 203 watchdog_configure();
204 204
205 205 watchdog_start();
206 206
207 207 set_sy_lfr_watchdog_enabled( true );
208 208
209 209 while(1){
210 210 status = rtems_rate_monotonic_period( watchdog_period_id, WATCHDOG_PERIOD );
211 211 watchdog_reload();
212 212 i = i + 1;
213 if ( i == 10 )
213 if ( i == WATCHDOG_LOOP_PRINTF )
214 214 {
215 215 i = 0;
216 216 j = j + 1;
217 217 PRINTF1("%d\n", j)
218 218 }
219 219 #ifdef DEBUG_WATCHDOG
220 if (j == 3 )
220 if (j == WATCHDOG_LOOP_DEBUG )
221 221 {
222 222 status = rtems_task_delete(RTEMS_SELF);
223 223 }
224 224 #endif
225 225 }
226 226 }
227 227
228 228 rtems_task hous_task(rtems_task_argument argument)
229 229 {
230 230 rtems_status_code status;
231 231 rtems_status_code spare_status;
232 232 rtems_id queue_id;
233 233 rtems_rate_monotonic_period_status period_status;
234 234
235 235 status = get_message_queue_id_send( &queue_id );
236 236 if (status != RTEMS_SUCCESSFUL)
237 237 {
238 238 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
239 239 }
240 240
241 241 BOOT_PRINTF("in HOUS ***\n");
242 242
243 243 if (rtems_rate_monotonic_ident( name_hk_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
244 244 status = rtems_rate_monotonic_create( name_hk_rate_monotonic, &HK_id );
245 245 if( status != RTEMS_SUCCESSFUL ) {
246 246 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status );
247 247 }
248 248 }
249 249
250 250 status = rtems_rate_monotonic_cancel(HK_id);
251 251 if( status != RTEMS_SUCCESSFUL ) {
252 252 PRINTF1( "ERR *** in HOUS *** rtems_rate_monotonic_cancel(HK_id) ***code: %d\n", status );
253 253 }
254 254 else {
255 255 DEBUG_PRINTF("OK *** in HOUS *** rtems_rate_monotonic_cancel(HK_id)\n");
256 256 }
257 257
258 258 // startup phase
259 259 status = rtems_rate_monotonic_period( HK_id, SY_LFR_TIME_SYN_TIMEOUT_in_ticks );
260 260 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
261 261 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
262 262 while(period_status.state != RATE_MONOTONIC_EXPIRED ) // after SY_LFR_TIME_SYN_TIMEOUT ms, starts HK anyway
263 263 {
264 if ((time_management_regs->coarse_time & 0x80000000) == 0x00000000) // check time synchronization
264 if ((time_management_regs->coarse_time & VAL_LFR_SYNCHRONIZED) == INT32_ALL_0) // check time synchronization
265 265 {
266 266 break; // break if LFR is synchronized
267 267 }
268 268 else
269 269 {
270 270 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
271 // sched_yield();
272 status = rtems_task_wake_after( 10 ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 100 ms = 10 * 10 ms
271
272 status = rtems_task_wake_after( HK_SYNC_WAIT ); // wait HK_SYNCH_WAIT 100 ms = 10 * 10 ms
273 273 }
274 274 }
275 275 status = rtems_rate_monotonic_cancel(HK_id);
276 276 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
277 277
278 278 set_hk_lfr_reset_cause( POWER_ON );
279 279
280 280 while(1){ // launch the rate monotonic task
281 281 status = rtems_rate_monotonic_period( HK_id, HK_PERIOD );
282 282 if ( status != RTEMS_SUCCESSFUL ) {
283 283 PRINTF1( "in HOUS *** ERR period: %d\n", status);
284 284 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_6 );
285 285 }
286 286 else {
287 housekeeping_packet.packetSequenceControl[0] = (unsigned char) (sequenceCounterHK >> 8);
288 housekeeping_packet.packetSequenceControl[1] = (unsigned char) (sequenceCounterHK );
287 housekeeping_packet.packetSequenceControl[BYTE_0] = (unsigned char) (sequenceCounterHK >> SHIFT_1_BYTE);
288 housekeeping_packet.packetSequenceControl[BYTE_1] = (unsigned char) (sequenceCounterHK );
289 289 increment_seq_counter( &sequenceCounterHK );
290 290
291 housekeeping_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
292 housekeeping_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
293 housekeeping_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
294 housekeeping_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
295 housekeeping_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
296 housekeeping_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
291 housekeeping_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
292 housekeeping_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
293 housekeeping_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
294 housekeeping_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
295 housekeeping_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
296 housekeeping_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
297 297
298 298 spacewire_update_hk_lfr_link_state( &housekeeping_packet.lfr_status_word[0] );
299 299
300 300 spacewire_read_statistics();
301 301
302 302 update_hk_with_grspw_stats();
303 303
304 304 set_hk_lfr_time_not_synchro();
305 305
306 306 housekeeping_packet.hk_lfr_q_sd_fifo_size_max = hk_lfr_q_sd_fifo_size_max;
307 307 housekeeping_packet.hk_lfr_q_rv_fifo_size_max = hk_lfr_q_rv_fifo_size_max;
308 308 housekeeping_packet.hk_lfr_q_p0_fifo_size_max = hk_lfr_q_p0_fifo_size_max;
309 309 housekeeping_packet.hk_lfr_q_p1_fifo_size_max = hk_lfr_q_p1_fifo_size_max;
310 310 housekeeping_packet.hk_lfr_q_p2_fifo_size_max = hk_lfr_q_p2_fifo_size_max;
311 311
312 312 housekeeping_packet.sy_lfr_common_parameters_spare = parameter_dump_packet.sy_lfr_common_parameters_spare;
313 313 housekeeping_packet.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
314 314 get_temperatures( housekeeping_packet.hk_lfr_temp_scm );
315 315 get_v_e1_e2_f3( housekeeping_packet.hk_lfr_sc_v_f3 );
316 316 get_cpu_load( (unsigned char *) &housekeeping_packet.hk_lfr_cpu_load );
317 317
318 318 hk_lfr_le_me_he_update();
319 319
320 320 housekeeping_packet.hk_lfr_sc_rw_f_flags = cp_rpw_sc_rw_f_flags;
321 321
322 322 // SEND PACKET
323 323 status = rtems_message_queue_send( queue_id, &housekeeping_packet,
324 324 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
325 325 if (status != RTEMS_SUCCESSFUL) {
326 326 PRINTF1("in HOUS *** ERR send: %d\n", status)
327 327 }
328 328 }
329 329 }
330 330
331 331 PRINTF("in HOUS *** deleting task\n")
332 332
333 333 status = rtems_task_delete( RTEMS_SELF ); // should not return
334 334
335 335 return;
336 336 }
337 337
338 338 rtems_task avgv_task(rtems_task_argument argument)
339 339 {
340 340 #define MOVING_AVERAGE 16
341 341 rtems_status_code status;
342 342 unsigned int v[MOVING_AVERAGE];
343 343 unsigned int e1[MOVING_AVERAGE];
344 344 unsigned int e2[MOVING_AVERAGE];
345 345 float average_v;
346 346 float average_e1;
347 347 float average_e2;
348 348 unsigned char k;
349 349 unsigned char indexOfOldValue;
350 350
351 351 BOOT_PRINTF("in AVGV ***\n");
352 352
353 353 if (rtems_rate_monotonic_ident( name_avgv_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
354 354 status = rtems_rate_monotonic_create( name_avgv_rate_monotonic, &AVGV_id );
355 355 if( status != RTEMS_SUCCESSFUL ) {
356 356 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status );
357 357 }
358 358 }
359 359
360 360 status = rtems_rate_monotonic_cancel(AVGV_id);
361 361 if( status != RTEMS_SUCCESSFUL ) {
362 362 PRINTF1( "ERR *** in AVGV *** rtems_rate_monotonic_cancel(AVGV_id) ***code: %d\n", status );
363 363 }
364 364 else {
365 365 DEBUG_PRINTF("OK *** in AVGV *** rtems_rate_monotonic_cancel(AVGV_id)\n");
366 366 }
367 367
368 368 // initialize values
369 369 k = 0;
370 370 indexOfOldValue = MOVING_AVERAGE - 1;
371 371 for (k = 0; k < MOVING_AVERAGE; k++)
372 372 {
373 373 v[k] = 0;
374 374 e1[k] = 0;
375 375 e2[k] = 0;
376 376 average_v = 0.;
377 377 average_e1 = 0.;
378 378 average_e2 = 0.;
379 379 }
380 380
381 381 k = 0;
382 382
383 383 while(1){ // launch the rate monotonic task
384 384 status = rtems_rate_monotonic_period( AVGV_id, AVGV_PERIOD );
385 385 if ( status != RTEMS_SUCCESSFUL ) {
386 386 PRINTF1( "in AVGV *** ERR period: %d\n", status);
387 387 }
388 388 else {
389 389 // store new value in buffer
390 390 v[k] = waveform_picker_regs->v;
391 391 e1[k] = waveform_picker_regs->e1;
392 392 e2[k] = waveform_picker_regs->e2;
393 393 if (k == (MOVING_AVERAGE - 1))
394 394 {
395 395 indexOfOldValue = 0;
396 396 }
397 397 else
398 398 {
399 399 indexOfOldValue = k + 1;
400 400 }
401 401 average_v = average_v + v[k] - v[indexOfOldValue];
402 402 average_e1 = average_e1 + e1[k] - e1[indexOfOldValue];
403 403 average_e2 = average_e2 + e2[k] - e2[indexOfOldValue];
404 404 }
405 405 if (k == (MOVING_AVERAGE-1))
406 406 {
407 407 k = 0;
408 408 printf("tick\n");
409 409 }
410 410 else
411 411 {
412 412 k++;
413 413 }
414 414 }
415 415
416 416 PRINTF("in AVGV *** deleting task\n")
417 417
418 418 status = rtems_task_delete( RTEMS_SELF ); // should not return
419 419
420 420 return;
421 421 }
422 422
423 423 rtems_task dumb_task( rtems_task_argument unused )
424 424 {
425 425 /** This RTEMS taks is used to print messages without affecting the general behaviour of the software.
426 426 *
427 427 * @param unused is the starting argument of the RTEMS task
428 428 *
429 429 * The DUMB taks waits for RTEMS events and print messages depending on the incoming events.
430 430 *
431 431 */
432 432
433 433 unsigned int i;
434 434 unsigned int intEventOut;
435 435 unsigned int coarse_time = 0;
436 436 unsigned int fine_time = 0;
437 437 rtems_event_set event_out;
438 438
439 char *DumbMessages[15] = {"in DUMB *** default", // RTEMS_EVENT_0
440 "in DUMB *** timecode_irq_handler", // RTEMS_EVENT_1
441 "in DUMB *** f3 buffer changed", // RTEMS_EVENT_2
442 "in DUMB *** in SMIQ *** Error sending event to AVF0", // RTEMS_EVENT_3
443 "in DUMB *** spectral_matrices_isr *** Error sending event to SMIQ", // RTEMS_EVENT_4
444 "in DUMB *** waveforms_simulator_isr", // RTEMS_EVENT_5
445 "VHDL SM *** two buffers f0 ready", // RTEMS_EVENT_6
446 "ready for dump", // RTEMS_EVENT_7
447 "VHDL ERR *** spectral matrix", // RTEMS_EVENT_8
448 "tick", // RTEMS_EVENT_9
449 "VHDL ERR *** waveform picker", // RTEMS_EVENT_10
450 "VHDL ERR *** unexpected ready matrix values", // RTEMS_EVENT_11
451 "WATCHDOG timer", // RTEMS_EVENT_12
452 "TIMECODE timer", // RTEMS_EVENT_13
453 "TIMECODE ISR" // RTEMS_EVENT_14
454 };
439 char *DumbMessages[DUMB_MESSAGE_NB] = {DUMB_MESSAGE_0, // RTEMS_EVENT_0
440 DUMB_MESSAGE_1, // RTEMS_EVENT_1
441 DUMB_MESSAGE_2, // RTEMS_EVENT_2
442 DUMB_MESSAGE_3, // RTEMS_EVENT_3
443 DUMB_MESSAGE_4, // RTEMS_EVENT_4
444 DUMB_MESSAGE_5, // RTEMS_EVENT_5
445 DUMB_MESSAGE_6, // RTEMS_EVENT_6
446 DUMB_MESSAGE_7, // RTEMS_EVENT_7
447 DUMB_MESSAGE_8, // RTEMS_EVENT_8
448 DUMB_MESSAGE_9, // RTEMS_EVENT_9
449 DUMB_MESSAGE_10, // RTEMS_EVENT_10
450 DUMB_MESSAGE_11, // RTEMS_EVENT_11
451 DUMB_MESSAGE_12, // RTEMS_EVENT_12
452 DUMB_MESSAGE_13, // RTEMS_EVENT_13
453 DUMB_MESSAGE_14 // RTEMS_EVENT_14
454 };
455 455
456 456 BOOT_PRINTF("in DUMB *** \n")
457 457
458 458 while(1){
459 459 rtems_event_receive(RTEMS_EVENT_0 | RTEMS_EVENT_1 | RTEMS_EVENT_2 | RTEMS_EVENT_3
460 460 | RTEMS_EVENT_4 | RTEMS_EVENT_5 | RTEMS_EVENT_6 | RTEMS_EVENT_7
461 461 | RTEMS_EVENT_8 | RTEMS_EVENT_9 | RTEMS_EVENT_12 | RTEMS_EVENT_13
462 462 | RTEMS_EVENT_14,
463 463 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT
464 464 intEventOut = (unsigned int) event_out;
465 for ( i=0; i<32; i++)
465 for ( i=0; i<NB_RTEMS_EVENTS; i++)
466 466 {
467 if ( ((intEventOut >> i) & 0x0001) != 0)
467 if ( ((intEventOut >> i) & 1) != 0)
468 468 {
469 469 coarse_time = time_management_regs->coarse_time;
470 470 fine_time = time_management_regs->fine_time;
471 if (i==12)
471 if (i==EVENT_12)
472 472 {
473 PRINTF1("%s\n", DumbMessages[12])
473 PRINTF1("%s\n", DUMB_MESSAGE_12)
474 474 }
475 if (i==13)
475 if (i==EVENT_13)
476 476 {
477 PRINTF1("%s\n", DumbMessages[13])
477 PRINTF1("%s\n", DUMB_MESSAGE_13)
478 478 }
479 if (i==14)
479 if (i==EVENT_14)
480 480 {
481 PRINTF1("%s\n", DumbMessages[1])
481 PRINTF1("%s\n", DUMB_MESSAGE_1)
482 482 }
483 483 }
484 484 }
485 485 }
486 486 }
487 487
488 488 //*****************************
489 489 // init housekeeping parameters
490 490
491 491 void init_housekeeping_parameters( void )
492 492 {
493 493 /** This function initialize the housekeeping_packet global variable with default values.
494 494 *
495 495 */
496 496
497 497 unsigned int i = 0;
498 498 unsigned char *parameters;
499 499 unsigned char sizeOfHK;
500 500
501 501 sizeOfHK = sizeof( Packet_TM_LFR_HK_t );
502 502
503 503 parameters = (unsigned char*) &housekeeping_packet;
504 504
505 505 for(i = 0; i< sizeOfHK; i++)
506 506 {
507 parameters[i] = 0x00;
507 parameters[i] = INIT_CHAR;
508 508 }
509 509
510 510 housekeeping_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
511 511 housekeeping_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
512 512 housekeeping_packet.reserved = DEFAULT_RESERVED;
513 513 housekeeping_packet.userApplication = CCSDS_USER_APP;
514 housekeeping_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
514 housekeeping_packet.packetID[0] = (unsigned char) (APID_TM_HK >> SHIFT_1_BYTE);
515 515 housekeeping_packet.packetID[1] = (unsigned char) (APID_TM_HK);
516 516 housekeeping_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
517 517 housekeeping_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
518 housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
518 housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> SHIFT_1_BYTE);
519 519 housekeeping_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
520 520 housekeeping_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
521 521 housekeeping_packet.serviceType = TM_TYPE_HK;
522 522 housekeeping_packet.serviceSubType = TM_SUBTYPE_HK;
523 523 housekeeping_packet.destinationID = TM_DESTINATION_ID_GROUND;
524 524 housekeeping_packet.sid = SID_HK;
525 525
526 526 // init status word
527 527 housekeeping_packet.lfr_status_word[0] = DEFAULT_STATUS_WORD_BYTE0;
528 528 housekeeping_packet.lfr_status_word[1] = DEFAULT_STATUS_WORD_BYTE1;
529 529 // init software version
530 530 housekeeping_packet.lfr_sw_version[0] = SW_VERSION_N1;
531 531 housekeeping_packet.lfr_sw_version[1] = SW_VERSION_N2;
532 housekeeping_packet.lfr_sw_version[2] = SW_VERSION_N3;
533 housekeeping_packet.lfr_sw_version[3] = SW_VERSION_N4;
532 housekeeping_packet.lfr_sw_version[BYTE_2] = SW_VERSION_N3;
533 housekeeping_packet.lfr_sw_version[BYTE_3] = SW_VERSION_N4;
534 534 // init fpga version
535 535 parameters = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
536 housekeeping_packet.lfr_fpga_version[0] = parameters[1]; // n1
537 housekeeping_packet.lfr_fpga_version[1] = parameters[2]; // n2
538 housekeeping_packet.lfr_fpga_version[2] = parameters[3]; // n3
536 housekeeping_packet.lfr_fpga_version[BYTE_0] = parameters[BYTE_1]; // n1
537 housekeeping_packet.lfr_fpga_version[BYTE_1] = parameters[BYTE_2]; // n2
538 housekeeping_packet.lfr_fpga_version[BYTE_2] = parameters[BYTE_3]; // n3
539 539
540 540 housekeeping_packet.hk_lfr_q_sd_fifo_size = MSG_QUEUE_COUNT_SEND;
541 541 housekeeping_packet.hk_lfr_q_rv_fifo_size = MSG_QUEUE_COUNT_RECV;
542 542 housekeeping_packet.hk_lfr_q_p0_fifo_size = MSG_QUEUE_COUNT_PRC0;
543 543 housekeeping_packet.hk_lfr_q_p1_fifo_size = MSG_QUEUE_COUNT_PRC1;
544 544 housekeeping_packet.hk_lfr_q_p2_fifo_size = MSG_QUEUE_COUNT_PRC2;
545 545 }
546 546
547 547 void increment_seq_counter( unsigned short *packetSequenceControl )
548 548 {
549 549 /** This function increment the sequence counter passes in argument.
550 550 *
551 551 * The increment does not affect the grouping flag. In case of an overflow, the counter is reset to 0.
552 552 *
553 553 */
554 554
555 555 unsigned short segmentation_grouping_flag;
556 556 unsigned short sequence_cnt;
557 557
558 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8; // keep bits 7 downto 6
559 sequence_cnt = (*packetSequenceControl) & 0x3fff; // [0011 1111 1111 1111]
558 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE; // keep bits 7 downto 6
559 sequence_cnt = (*packetSequenceControl) & SEQ_CNT_MASK; // [0011 1111 1111 1111]
560 560
561 561 if ( sequence_cnt < SEQ_CNT_MAX)
562 562 {
563 563 sequence_cnt = sequence_cnt + 1;
564 564 }
565 565 else
566 566 {
567 567 sequence_cnt = 0;
568 568 }
569 569
570 570 *packetSequenceControl = segmentation_grouping_flag | sequence_cnt ;
571 571 }
572 572
573 573 void getTime( unsigned char *time)
574 574 {
575 575 /** This function write the current local time in the time buffer passed in argument.
576 576 *
577 577 */
578 578
579 time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
580 time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
581 time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
579 time[0] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_3_BYTES);
580 time[1] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_2_BYTES);
581 time[2] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_1_BYTE);
582 582 time[3] = (unsigned char) (time_management_regs->coarse_time);
583 time[4] = (unsigned char) (time_management_regs->fine_time>>8);
583 time[4] = (unsigned char) (time_management_regs->fine_time>>SHIFT_1_BYTE);
584 584 time[5] = (unsigned char) (time_management_regs->fine_time);
585 585 }
586 586
587 587 unsigned long long int getTimeAsUnsignedLongLongInt( )
588 588 {
589 589 /** This function write the current local time in the time buffer passed in argument.
590 590 *
591 591 */
592 592 unsigned long long int time;
593 593
594 time = ( (unsigned long long int) (time_management_regs->coarse_time & 0x7fffffff) << 16 )
594 time = ( (unsigned long long int) (time_management_regs->coarse_time & COARSE_TIME_MASK) << SHIFT_2_BYTES )
595 595 + time_management_regs->fine_time;
596 596
597 597 return time;
598 598 }
599 599
600 600 void send_dumb_hk( void )
601 601 {
602 602 Packet_TM_LFR_HK_t dummy_hk_packet;
603 603 unsigned char *parameters;
604 604 unsigned int i;
605 605 rtems_id queue_id;
606 606
607 607 dummy_hk_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
608 608 dummy_hk_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
609 609 dummy_hk_packet.reserved = DEFAULT_RESERVED;
610 610 dummy_hk_packet.userApplication = CCSDS_USER_APP;
611 dummy_hk_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
611 dummy_hk_packet.packetID[0] = (unsigned char) (APID_TM_HK >> SHIFT_1_BYTE);
612 612 dummy_hk_packet.packetID[1] = (unsigned char) (APID_TM_HK);
613 613 dummy_hk_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
614 614 dummy_hk_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
615 dummy_hk_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
615 dummy_hk_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> SHIFT_1_BYTE);
616 616 dummy_hk_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
617 617 dummy_hk_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
618 618 dummy_hk_packet.serviceType = TM_TYPE_HK;
619 619 dummy_hk_packet.serviceSubType = TM_SUBTYPE_HK;
620 620 dummy_hk_packet.destinationID = TM_DESTINATION_ID_GROUND;
621 dummy_hk_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
622 dummy_hk_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
623 dummy_hk_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
624 dummy_hk_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
625 dummy_hk_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
626 dummy_hk_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
621 dummy_hk_packet.time[0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
622 dummy_hk_packet.time[1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
623 dummy_hk_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
624 dummy_hk_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
625 dummy_hk_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
626 dummy_hk_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
627 627 dummy_hk_packet.sid = SID_HK;
628 628
629 629 // init status word
630 dummy_hk_packet.lfr_status_word[0] = 0xff;
631 dummy_hk_packet.lfr_status_word[1] = 0xff;
630 dummy_hk_packet.lfr_status_word[0] = INT8_ALL_F;
631 dummy_hk_packet.lfr_status_word[1] = INT8_ALL_F;
632 632 // init software version
633 633 dummy_hk_packet.lfr_sw_version[0] = SW_VERSION_N1;
634 634 dummy_hk_packet.lfr_sw_version[1] = SW_VERSION_N2;
635 dummy_hk_packet.lfr_sw_version[2] = SW_VERSION_N3;
636 dummy_hk_packet.lfr_sw_version[3] = SW_VERSION_N4;
635 dummy_hk_packet.lfr_sw_version[BYTE_2] = SW_VERSION_N3;
636 dummy_hk_packet.lfr_sw_version[BYTE_3] = SW_VERSION_N4;
637 637 // init fpga version
638 parameters = (unsigned char *) (REGS_ADDR_WAVEFORM_PICKER + 0xb0);
639 dummy_hk_packet.lfr_fpga_version[0] = parameters[1]; // n1
640 dummy_hk_packet.lfr_fpga_version[1] = parameters[2]; // n2
641 dummy_hk_packet.lfr_fpga_version[2] = parameters[3]; // n3
638 parameters = (unsigned char *) (REGS_ADDR_WAVEFORM_PICKER + APB_OFFSET_VHDL_REV);
639 dummy_hk_packet.lfr_fpga_version[BYTE_0] = parameters[BYTE_1]; // n1
640 dummy_hk_packet.lfr_fpga_version[BYTE_1] = parameters[BYTE_2]; // n2
641 dummy_hk_packet.lfr_fpga_version[BYTE_2] = parameters[BYTE_3]; // n3
642 642
643 643 parameters = (unsigned char *) &dummy_hk_packet.hk_lfr_cpu_load;
644 644
645 for (i=0; i<100; i++)
645 for (i=0; i<(BYTE_POS_HK_REACTION_WHEELS_FREQUENCY - BYTE_POS_HK_LFR_CPU_LOAD); i++)
646 646 {
647 parameters[i] = 0xff;
647 parameters[i] = INT8_ALL_F;
648 648 }
649 649
650 650 get_message_queue_id_send( &queue_id );
651 651
652 652 rtems_message_queue_send( queue_id, &dummy_hk_packet,
653 653 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
654 654 }
655 655
656 656 void get_temperatures( unsigned char *temperatures )
657 657 {
658 658 unsigned char* temp_scm_ptr;
659 659 unsigned char* temp_pcb_ptr;
660 660 unsigned char* temp_fpga_ptr;
661 661
662 662 // SEL1 SEL0
663 663 // 0 0 => PCB
664 664 // 0 1 => FPGA
665 665 // 1 0 => SCM
666 666
667 667 temp_scm_ptr = (unsigned char *) &time_management_regs->temp_scm;
668 668 temp_pcb_ptr = (unsigned char *) &time_management_regs->temp_pcb;
669 669 temp_fpga_ptr = (unsigned char *) &time_management_regs->temp_fpga;
670 670
671 temperatures[0] = temp_scm_ptr[2];
672 temperatures[1] = temp_scm_ptr[3];
673 temperatures[2] = temp_pcb_ptr[2];
674 temperatures[3] = temp_pcb_ptr[3];
675 temperatures[4] = temp_fpga_ptr[2];
676 temperatures[5] = temp_fpga_ptr[3];
671 temperatures[ BYTE_0 ] = temp_scm_ptr[ BYTE_2 ];
672 temperatures[ BYTE_1 ] = temp_scm_ptr[ BYTE_3 ];
673 temperatures[ BYTE_2 ] = temp_pcb_ptr[ BYTE_2 ];
674 temperatures[ BYTE_3 ] = temp_pcb_ptr[ BYTE_3 ];
675 temperatures[ BYTE_4 ] = temp_fpga_ptr[ BYTE_2 ];
676 temperatures[ BYTE_5 ] = temp_fpga_ptr[ BYTE_3 ];
677 677 }
678 678
679 679 void get_v_e1_e2_f3( unsigned char *spacecraft_potential )
680 680 {
681 681 unsigned char* v_ptr;
682 682 unsigned char* e1_ptr;
683 683 unsigned char* e2_ptr;
684 684
685 685 v_ptr = (unsigned char *) &waveform_picker_regs->v;
686 686 e1_ptr = (unsigned char *) &waveform_picker_regs->e1;
687 687 e2_ptr = (unsigned char *) &waveform_picker_regs->e2;
688 688
689 spacecraft_potential[0] = v_ptr[2];
690 spacecraft_potential[1] = v_ptr[3];
691 spacecraft_potential[2] = e1_ptr[2];
692 spacecraft_potential[3] = e1_ptr[3];
693 spacecraft_potential[4] = e2_ptr[2];
694 spacecraft_potential[5] = e2_ptr[3];
689 spacecraft_potential[ BYTE_0 ] = v_ptr[ BYTE_2 ];
690 spacecraft_potential[ BYTE_1 ] = v_ptr[ BYTE_3 ];
691 spacecraft_potential[ BYTE_2 ] = e1_ptr[ BYTE_2 ];
692 spacecraft_potential[ BYTE_3 ] = e1_ptr[ BYTE_3 ];
693 spacecraft_potential[ BYTE_4 ] = e2_ptr[ BYTE_2 ];
694 spacecraft_potential[ BYTE_5 ] = e2_ptr[ BYTE_3 ];
695 695 }
696 696
697 697 void get_cpu_load( unsigned char *resource_statistics )
698 698 {
699 699 unsigned char cpu_load;
700 700
701 701 cpu_load = lfr_rtems_cpu_usage_report();
702 702
703 703 // HK_LFR_CPU_LOAD
704 704 resource_statistics[0] = cpu_load;
705 705
706 706 // HK_LFR_CPU_LOAD_MAX
707 707 if (cpu_load > resource_statistics[1])
708 708 {
709 709 resource_statistics[1] = cpu_load;
710 710 }
711 711
712 712 // CPU_LOAD_AVE
713 resource_statistics[2] = 0;
713 resource_statistics[BYTE_2] = 0;
714 714
715 715 #ifndef PRINT_TASK_STATISTICS
716 716 rtems_cpu_usage_reset();
717 717 #endif
718 718
719 719 }
720 720
721 721 void set_hk_lfr_sc_potential_flag( bool state )
722 722 {
723 723 if (state == true)
724 724 {
725 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] | 0x40; // [0100 0000]
725 housekeeping_packet.lfr_status_word[1] =
726 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_SC_POTENTIAL_FLAG_BIT; // [0100 0000]
726 727 }
727 728 else
728 729 {
729 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xbf; // [1011 1111]
730 housekeeping_packet.lfr_status_word[1] =
731 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_SC_POTENTIAL_FLAG_MASK; // [1011 1111]
730 732 }
731 733 }
732 734
733 735 void set_sy_lfr_pas_filter_enabled( bool state )
734 736 {
735 737 if (state == true)
736 738 {
737 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] | 0x20; // [0010 0000]
739 housekeeping_packet.lfr_status_word[1] =
740 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_SC_POTENTIAL_FLAG_BIT; // [0010 0000]
738 741 }
739 742 else
740 743 {
741 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xdf; // [1101 1111]
744 housekeeping_packet.lfr_status_word[1] =
745 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_SC_POTENTIAL_FLAG_MASK; // [1101 1111]
742 746 }
743 747 }
744 748
745 749 void set_sy_lfr_watchdog_enabled( bool state )
746 750 {
747 751 if (state == true)
748 752 {
749 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] | 0x10; // [0001 0000]
753 housekeeping_packet.lfr_status_word[1] =
754 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_WATCHDOG_BIT; // [0001 0000]
750 755 }
751 756 else
752 757 {
753 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xef; // [1110 1111]
758 housekeeping_packet.lfr_status_word[1] =
759 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_WATCHDOG_MASK; // [1110 1111]
754 760 }
755 761 }
756 762
757 763 void set_hk_lfr_calib_enable( bool state )
758 764 {
759 765 if (state == true)
760 766 {
761 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] | 0x08; // [0000 1000]
767 housekeeping_packet.lfr_status_word[1] =
768 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_CALIB_BIT; // [0000 1000]
762 769 }
763 770 else
764 771 {
765 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xf7; // [1111 0111]
772 housekeeping_packet.lfr_status_word[1] =
773 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_CALIB_MASK; // [1111 0111]
766 774 }
767 775 }
768 776
769 777 void set_hk_lfr_reset_cause( enum lfr_reset_cause_t lfr_reset_cause )
770 778 {
771 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xf8; // [1111 1000]
779 housekeeping_packet.lfr_status_word[1] =
780 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_RESET_CAUSE_MASK; // [1111 1000]
772 781
773 782 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1]
774 | (lfr_reset_cause & 0x07 ); // [0000 0111]
783 | (lfr_reset_cause & STATUS_WORD_RESET_CAUSE_BITS ); // [0000 0111]
775 784
776 785 }
777 786
778 787 void increment_hk_counter( unsigned char newValue, unsigned char oldValue, unsigned int *counter )
779 788 {
780 789 int delta;
781 790
782 791 delta = 0;
783 792
784 793 if (newValue >= oldValue)
785 794 {
786 795 delta = newValue - oldValue;
787 796 }
788 797 else
789 798 {
790 799 delta = 255 - oldValue + newValue;
791 800 }
792 801
793 802 *counter = *counter + delta;
794 803 }
795 804
796 805 void hk_lfr_le_update( void )
797 806 {
798 807 static hk_lfr_le_t old_hk_lfr_le = {0};
799 808 hk_lfr_le_t new_hk_lfr_le;
800 809 unsigned int counter;
801 810
802 counter = ((unsigned int) housekeeping_packet.hk_lfr_le_cnt[0]) * 256 + housekeeping_packet.hk_lfr_le_cnt[1];
811 counter = (((unsigned int) housekeeping_packet.hk_lfr_le_cnt[0]) * 256) + housekeeping_packet.hk_lfr_le_cnt[1];
803 812
804 813 // DPU
805 814 new_hk_lfr_le.dpu_spw_parity = housekeeping_packet.hk_lfr_dpu_spw_parity;
806 815 new_hk_lfr_le.dpu_spw_disconnect= housekeeping_packet.hk_lfr_dpu_spw_disconnect;
807 816 new_hk_lfr_le.dpu_spw_escape = housekeeping_packet.hk_lfr_dpu_spw_escape;
808 817 new_hk_lfr_le.dpu_spw_credit = housekeeping_packet.hk_lfr_dpu_spw_credit;
809 818 new_hk_lfr_le.dpu_spw_write_sync= housekeeping_packet.hk_lfr_dpu_spw_write_sync;
810 819 // TIMECODE
811 820 new_hk_lfr_le.timecode_erroneous= housekeeping_packet.hk_lfr_timecode_erroneous;
812 821 new_hk_lfr_le.timecode_missing = housekeeping_packet.hk_lfr_timecode_missing;
813 822 new_hk_lfr_le.timecode_invalid = housekeeping_packet.hk_lfr_timecode_invalid;
814 823 // TIME
815 824 new_hk_lfr_le.time_timecode_it = housekeeping_packet.hk_lfr_time_timecode_it;
816 825 new_hk_lfr_le.time_not_synchro = housekeeping_packet.hk_lfr_time_not_synchro;
817 826 new_hk_lfr_le.time_timecode_ctr = housekeeping_packet.hk_lfr_time_timecode_ctr;
818 827 //AHB
819 828 new_hk_lfr_le.ahb_correctable = housekeeping_packet.hk_lfr_ahb_correctable;
820 829 // housekeeping_packet.hk_lfr_dpu_spw_rx_ahb => not handled by the grspw driver
821 830 // housekeeping_packet.hk_lfr_dpu_spw_tx_ahb => not handled by the grspw driver
822 831
823 832 // update the le counter
824 833 // DPU
825 increment_hk_counter( new_hk_lfr_le.dpu_spw_parity, old_hk_lfr_le.dpu_spw_parity, counter );
826 increment_hk_counter( new_hk_lfr_le.dpu_spw_disconnect,old_hk_lfr_le.dpu_spw_disconnect, counter );
827 increment_hk_counter( new_hk_lfr_le.dpu_spw_escape, old_hk_lfr_le.dpu_spw_escape, counter );
828 increment_hk_counter( new_hk_lfr_le.dpu_spw_credit, old_hk_lfr_le.dpu_spw_credit, counter );
829 increment_hk_counter( new_hk_lfr_le.dpu_spw_write_sync,old_hk_lfr_le.dpu_spw_write_sync, counter );
834 increment_hk_counter( new_hk_lfr_le.dpu_spw_parity, old_hk_lfr_le.dpu_spw_parity, &counter );
835 increment_hk_counter( new_hk_lfr_le.dpu_spw_disconnect,old_hk_lfr_le.dpu_spw_disconnect, &counter );
836 increment_hk_counter( new_hk_lfr_le.dpu_spw_escape, old_hk_lfr_le.dpu_spw_escape, &counter );
837 increment_hk_counter( new_hk_lfr_le.dpu_spw_credit, old_hk_lfr_le.dpu_spw_credit, &counter );
838 increment_hk_counter( new_hk_lfr_le.dpu_spw_write_sync,old_hk_lfr_le.dpu_spw_write_sync, &counter );
830 839 // TIMECODE
831 increment_hk_counter( new_hk_lfr_le.timecode_erroneous,old_hk_lfr_le.timecode_erroneous, counter );
832 increment_hk_counter( new_hk_lfr_le.timecode_missing, old_hk_lfr_le.timecode_missing, counter );
833 increment_hk_counter( new_hk_lfr_le.timecode_invalid, old_hk_lfr_le.timecode_invalid, counter );
840 increment_hk_counter( new_hk_lfr_le.timecode_erroneous,old_hk_lfr_le.timecode_erroneous, &counter );
841 increment_hk_counter( new_hk_lfr_le.timecode_missing, old_hk_lfr_le.timecode_missing, &counter );
842 increment_hk_counter( new_hk_lfr_le.timecode_invalid, old_hk_lfr_le.timecode_invalid, &counter );
834 843 // TIME
835 increment_hk_counter( new_hk_lfr_le.time_timecode_it, old_hk_lfr_le.time_timecode_it, counter );
836 increment_hk_counter( new_hk_lfr_le.time_not_synchro, old_hk_lfr_le.time_not_synchro, counter );
837 increment_hk_counter( new_hk_lfr_le.time_timecode_ctr, old_hk_lfr_le.time_timecode_ctr, counter );
844 increment_hk_counter( new_hk_lfr_le.time_timecode_it, old_hk_lfr_le.time_timecode_it, &counter );
845 increment_hk_counter( new_hk_lfr_le.time_not_synchro, old_hk_lfr_le.time_not_synchro, &counter );
846 increment_hk_counter( new_hk_lfr_le.time_timecode_ctr, old_hk_lfr_le.time_timecode_ctr, &counter );
838 847 // AHB
839 increment_hk_counter( new_hk_lfr_le.ahb_correctable, old_hk_lfr_le.ahb_correctable, counter );
848 increment_hk_counter( new_hk_lfr_le.ahb_correctable, old_hk_lfr_le.ahb_correctable, &counter );
840 849
841 850 // DPU
842 851 old_hk_lfr_le.dpu_spw_parity = new_hk_lfr_le.dpu_spw_parity;
843 852 old_hk_lfr_le.dpu_spw_disconnect= new_hk_lfr_le.dpu_spw_disconnect;
844 853 old_hk_lfr_le.dpu_spw_escape = new_hk_lfr_le.dpu_spw_escape;
845 854 old_hk_lfr_le.dpu_spw_credit = new_hk_lfr_le.dpu_spw_credit;
846 855 old_hk_lfr_le.dpu_spw_write_sync= new_hk_lfr_le.dpu_spw_write_sync;
847 856 // TIMECODE
848 857 old_hk_lfr_le.timecode_erroneous= new_hk_lfr_le.timecode_erroneous;
849 858 old_hk_lfr_le.timecode_missing = new_hk_lfr_le.timecode_missing;
850 859 old_hk_lfr_le.timecode_invalid = new_hk_lfr_le.timecode_invalid;
851 860 // TIME
852 861 old_hk_lfr_le.time_timecode_it = new_hk_lfr_le.time_timecode_it;
853 862 old_hk_lfr_le.time_not_synchro = new_hk_lfr_le.time_not_synchro;
854 863 old_hk_lfr_le.time_timecode_ctr = new_hk_lfr_le.time_timecode_ctr;
855 864 //AHB
856 865 old_hk_lfr_le.ahb_correctable = new_hk_lfr_le.ahb_correctable;
857 866 // housekeeping_packet.hk_lfr_dpu_spw_rx_ahb => not handled by the grspw driver
858 867 // housekeeping_packet.hk_lfr_dpu_spw_tx_ahb => not handled by the grspw driver
859 868
860 869 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
861 870 // LE
862 housekeeping_packet.hk_lfr_le_cnt[0] = (unsigned char) ((hk_lfr_le_cnt & 0xff00) >> 8);
863 housekeeping_packet.hk_lfr_le_cnt[1] = (unsigned char) (hk_lfr_le_cnt & 0x00ff);
871 housekeeping_packet.hk_lfr_le_cnt[0] = (unsigned char) ((counter & BYTE0_MASK) >> SHIFT_1_BYTE);
872 housekeeping_packet.hk_lfr_le_cnt[1] = (unsigned char) (counter & BYTE1_MASK);
864 873 }
865 874
866 875 void hk_lfr_me_update( void )
867 876 {
868 877 static hk_lfr_me_t old_hk_lfr_me = {0};
869 878 hk_lfr_me_t new_hk_lfr_me;
870 879 unsigned int counter;
871 880
872 counter = ((unsigned int) housekeeping_packet.hk_lfr_me_cnt[0]) * 256 + housekeeping_packet.hk_lfr_me_cnt[1];
881 counter = (((unsigned int) housekeeping_packet.hk_lfr_me_cnt[0]) * 256) + housekeeping_packet.hk_lfr_me_cnt[1];
873 882
874 883 // get the current values
875 884 new_hk_lfr_me.dpu_spw_early_eop = housekeeping_packet.hk_lfr_dpu_spw_early_eop;
876 885 new_hk_lfr_me.dpu_spw_invalid_addr = housekeeping_packet.hk_lfr_dpu_spw_invalid_addr;
877 886 new_hk_lfr_me.dpu_spw_eep = housekeeping_packet.hk_lfr_dpu_spw_eep;
878 887 new_hk_lfr_me.dpu_spw_rx_too_big = housekeeping_packet.hk_lfr_dpu_spw_rx_too_big;
879 888
880 889 // update the me counter
881 increment_hk_counter( new_hk_lfr_me.dpu_spw_early_eop, old_hk_lfr_me.dpu_spw_early_eop, counter );
882 increment_hk_counter( new_hk_lfr_me.dpu_spw_invalid_addr, old_hk_lfr_me.dpu_spw_invalid_addr, counter );
883 increment_hk_counter( new_hk_lfr_me.dpu_spw_eep, old_hk_lfr_me.dpu_spw_eep, counter );
884 increment_hk_counter( new_hk_lfr_me.dpu_spw_rx_too_big, old_hk_lfr_me.dpu_spw_rx_too_big, counter );
890 increment_hk_counter( new_hk_lfr_me.dpu_spw_early_eop, old_hk_lfr_me.dpu_spw_early_eop, &counter );
891 increment_hk_counter( new_hk_lfr_me.dpu_spw_invalid_addr, old_hk_lfr_me.dpu_spw_invalid_addr, &counter );
892 increment_hk_counter( new_hk_lfr_me.dpu_spw_eep, old_hk_lfr_me.dpu_spw_eep, &counter );
893 increment_hk_counter( new_hk_lfr_me.dpu_spw_rx_too_big, old_hk_lfr_me.dpu_spw_rx_too_big, &counter );
885 894
886 895 // store the counters for the next time
887 896 old_hk_lfr_me.dpu_spw_early_eop = new_hk_lfr_me.dpu_spw_early_eop;
888 897 old_hk_lfr_me.dpu_spw_invalid_addr = new_hk_lfr_me.dpu_spw_invalid_addr;
889 898 old_hk_lfr_me.dpu_spw_eep = new_hk_lfr_me.dpu_spw_eep;
890 899 old_hk_lfr_me.dpu_spw_rx_too_big = new_hk_lfr_me.dpu_spw_rx_too_big;
891 900
892 901 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
893 902 // ME
894 housekeeping_packet.hk_lfr_me_cnt[0] = (unsigned char) ((hk_lfr_me_cnt & 0xff00) >> 8);
895 housekeeping_packet.hk_lfr_me_cnt[1] = (unsigned char) (hk_lfr_me_cnt & 0x00ff);
903 housekeeping_packet.hk_lfr_me_cnt[0] = (unsigned char) ((counter & BYTE0_MASK) >> SHIFT_1_BYTE);
904 housekeeping_packet.hk_lfr_me_cnt[1] = (unsigned char) (counter & BYTE1_MASK);
896 905 }
897 906
898 907 void hk_lfr_le_me_he_update()
899 908 {
900 909
901 910 unsigned int hk_lfr_he_cnt;
902 911
903 912 hk_lfr_he_cnt = ((unsigned int) housekeeping_packet.hk_lfr_he_cnt[0]) * 256 + housekeeping_packet.hk_lfr_he_cnt[1];
904 913
905 914 //update the low severity error counter
906 915 hk_lfr_le_update( );
907 916
908 917 //update the medium severity error counter
909 918 hk_lfr_me_update();
910 919
911 920 //update the high severity error counter
912 921 hk_lfr_he_cnt = 0;
913 922
914 923 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
915 924 // HE
916 housekeeping_packet.hk_lfr_he_cnt[0] = (unsigned char) ((hk_lfr_he_cnt & 0xff00) >> 8);
917 housekeeping_packet.hk_lfr_he_cnt[1] = (unsigned char) (hk_lfr_he_cnt & 0x00ff);
925 housekeeping_packet.hk_lfr_he_cnt[0] = (unsigned char) ((hk_lfr_he_cnt & BYTE0_MASK) >> SHIFT_1_BYTE);
926 housekeeping_packet.hk_lfr_he_cnt[1] = (unsigned char) (hk_lfr_he_cnt & BYTE1_MASK);
918 927
919 928 }
920 929
921 930 void set_hk_lfr_time_not_synchro()
922 931 {
923 932 static unsigned char synchroLost = 1;
924 933 int synchronizationBit;
925 934
926 935 // get the synchronization bit
927 synchronizationBit = (time_management_regs->coarse_time & 0x80000000) >> 31; // 1000 0000 0000 0000
936 synchronizationBit =
937 (time_management_regs->coarse_time & VAL_LFR_SYNCHRONIZED) >> BIT_SYNCHRONIZATION; // 1000 0000 0000 0000
928 938
929 939 switch (synchronizationBit)
930 940 {
931 941 case 0:
932 942 if (synchroLost == 1)
933 943 {
934 944 synchroLost = 0;
935 945 }
936 946 break;
937 947 case 1:
938 948 if (synchroLost == 0 )
939 949 {
940 950 synchroLost = 1;
941 951 increase_unsigned_char_counter(&housekeeping_packet.hk_lfr_time_not_synchro);
942 952 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_NOT_SYNCHRO );
943 953 }
944 954 break;
945 955 default:
946 956 PRINTF1("in hk_lfr_time_not_synchro *** unexpected value for synchronizationBit = %d\n", synchronizationBit);
947 957 break;
948 958 }
949 959
950 960 }
951 961
952 962 void set_hk_lfr_ahb_correctable() // CRITICITY L
953 963 {
954 964 /** This function builds the error counter hk_lfr_ahb_correctable using the statistics provided
955 965 * by the Cache Control Register (ASI 2, offset 0) and in the Register Protection Control Register (ASR16) on the
956 966 * detected errors in the cache, in the integer unit and in the floating point unit.
957 967 *
958 968 * @param void
959 969 *
960 970 * @return void
961 971 *
962 972 * All errors are summed to set the value of the hk_lfr_ahb_correctable counter.
963 973 *
964 974 */
965 975
966 976 unsigned int ahb_correctable;
967 977 unsigned int instructionErrorCounter;
968 978 unsigned int dataErrorCounter;
969 979 unsigned int fprfErrorCounter;
970 980 unsigned int iurfErrorCounter;
971 981
972 982 CCR_getInstructionAndDataErrorCounters( &instructionErrorCounter, &dataErrorCounter);
973 983 ASR16_get_FPRF_IURF_ErrorCounters( &fprfErrorCounter, &iurfErrorCounter);
974 984
975 985 ahb_correctable = instructionErrorCounter
976 986 + dataErrorCounter
977 987 + fprfErrorCounter
978 988 + iurfErrorCounter
979 989 + housekeeping_packet.hk_lfr_ahb_correctable;
980 990
981 housekeeping_packet.hk_lfr_ahb_correctable = (unsigned char) (ahb_correctable & 0xff); // [1111 1111]
991 housekeeping_packet.hk_lfr_ahb_correctable = (unsigned char) (ahb_correctable & INT8_ALL_F); // [1111 1111]
982 992
983 993 }
Show file before | Show file after | Hide comments
@@ -1,1609 +1,1610
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 unsigned char previousTimecodeCtr = 0;
26 26 unsigned int *grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER);
27 27
28 28 //***********
29 29 // RTEMS TASK
30 30 rtems_task spiq_task(rtems_task_argument unused)
31 31 {
32 32 /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver.
33 33 *
34 34 * @param unused is the starting argument of the RTEMS task
35 35 *
36 36 */
37 37
38 38 rtems_event_set event_out;
39 39 rtems_status_code status;
40 40 int linkStatus;
41 41
42 42 BOOT_PRINTF("in SPIQ *** \n")
43 43
44 44 while(true){
45 45 rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT
46 46 PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n")
47 47
48 48 // [0] SUSPEND RECV AND SEND TASKS
49 49 status = rtems_task_suspend( Task_id[ TASKID_RECV ] );
50 50 if ( status != RTEMS_SUCCESSFUL ) {
51 51 PRINTF("in SPIQ *** ERR suspending RECV Task\n")
52 52 }
53 53 status = rtems_task_suspend( Task_id[ TASKID_SEND ] );
54 54 if ( status != RTEMS_SUCCESSFUL ) {
55 55 PRINTF("in SPIQ *** ERR suspending SEND Task\n")
56 56 }
57 57
58 58 // [1] CHECK THE LINK
59 59 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1)
60 if ( linkStatus != 5) {
60 if ( linkStatus != SPW_LINK_OK) {
61 61 PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus)
62 62 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
63 63 }
64 64
65 65 // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT
66 66 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2)
67 if ( linkStatus != 5 ) // [2.a] not in run state, reset the link
67 if ( linkStatus != SPW_LINK_OK ) // [2.a] not in run state, reset the link
68 68 {
69 69 spacewire_read_statistics();
70 70 status = spacewire_several_connect_attemps( );
71 71 }
72 72 else // [2.b] in run state, start the link
73 73 {
74 74 status = spacewire_stop_and_start_link( fdSPW ); // start the link
75 75 if ( status != RTEMS_SUCCESSFUL)
76 76 {
77 77 PRINTF1("in SPIQ *** ERR spacewire_stop_and_start_link %d\n", status)
78 78 }
79 79 }
80 80
81 81 // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS
82 82 if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully
83 83 {
84 84 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
85 85 if ( status != RTEMS_SUCCESSFUL ) {
86 86 PRINTF("in SPIQ *** ERR resuming SEND Task\n")
87 87 }
88 88 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
89 89 if ( status != RTEMS_SUCCESSFUL ) {
90 90 PRINTF("in SPIQ *** ERR resuming RECV Task\n")
91 91 }
92 92 }
93 93 else // [3.b] the link is not in run state, go in STANDBY mode
94 94 {
95 95 status = enter_mode_standby();
96 96 if ( status != RTEMS_SUCCESSFUL )
97 97 {
98 98 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
99 99 }
100 100 {
101 101 updateLFRCurrentMode( LFR_MODE_STANDBY );
102 102 }
103 103 // wake the LINK task up to wait for the link recovery
104 104 status = rtems_event_send ( Task_id[TASKID_LINK], RTEMS_EVENT_0 );
105 105 status = rtems_task_suspend( RTEMS_SELF );
106 106 }
107 107 }
108 108 }
109 109
110 110 rtems_task recv_task( rtems_task_argument unused )
111 111 {
112 112 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
113 113 *
114 114 * @param unused is the starting argument of the RTEMS task
115 115 *
116 116 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
117 117 * 1. It reads the incoming data.
118 118 * 2. Launches the acceptance procedure.
119 119 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
120 120 *
121 121 */
122 122
123 123 int len;
124 124 ccsdsTelecommandPacket_t currentTC;
125 unsigned char computed_CRC[ 2 ];
126 unsigned char currentTC_LEN_RCV[ 2 ];
125 unsigned char computed_CRC[ BYTES_PER_CRC ];
126 unsigned char currentTC_LEN_RCV[ BYTES_PER_PKT_LEN ];
127 127 unsigned char destinationID;
128 128 unsigned int estimatedPacketLength;
129 129 unsigned int parserCode;
130 130 rtems_status_code status;
131 131 rtems_id queue_recv_id;
132 132 rtems_id queue_send_id;
133 133
134 134 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
135 135
136 136 status = get_message_queue_id_recv( &queue_recv_id );
137 137 if (status != RTEMS_SUCCESSFUL)
138 138 {
139 139 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
140 140 }
141 141
142 142 status = get_message_queue_id_send( &queue_send_id );
143 143 if (status != RTEMS_SUCCESSFUL)
144 144 {
145 145 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
146 146 }
147 147
148 148 BOOT_PRINTF("in RECV *** \n")
149 149
150 150 while(1)
151 151 {
152 152 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
153 153 if (len == -1){ // error during the read call
154 154 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
155 155 }
156 156 else {
157 157 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
158 158 PRINTF("in RECV *** packet lenght too short\n")
159 159 }
160 160 else {
161 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // => -3 is for Prot ID, Reserved and User App bytes
161 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - PROTID_RES_APP); // => -3 is for Prot ID, Reserved and User App bytes
162 162 //PRINTF1("incoming TC with Length (byte): %d\n", len - 3);
163 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> 8);
163 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> SHIFT_1_BYTE);
164 164 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
165 165 // CHECK THE TC
166 166 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
167 167 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
168 168 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
169 169 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
170 170 || (parserCode == WRONG_SRC_ID) )
171 171 { // send TM_LFR_TC_EXE_CORRUPTED
172 172 PRINTF1("TC corrupted received, with code: %d\n", parserCode);
173 173 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
174 174 &&
175 175 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
176 176 )
177 177 {
178 178 if ( parserCode == WRONG_SRC_ID )
179 179 {
180 180 destinationID = SID_TC_GROUND;
181 181 }
182 182 else
183 183 {
184 184 destinationID = currentTC.sourceID;
185 185 }
186 186 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
187 187 computed_CRC, currentTC_LEN_RCV,
188 188 destinationID );
189 189 }
190 190 }
191 191 else
192 192 { // send valid TC to the action launcher
193 193 status = rtems_message_queue_send( queue_recv_id, &currentTC,
194 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + 3);
194 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + PROTID_RES_APP);
195 195 }
196 196 }
197 197 }
198 198
199 199 update_queue_max_count( queue_recv_id, &hk_lfr_q_rv_fifo_size_max );
200 200
201 201 }
202 202 }
203 203
204 204 rtems_task send_task( rtems_task_argument argument)
205 205 {
206 206 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
207 207 *
208 208 * @param unused is the starting argument of the RTEMS task
209 209 *
210 210 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
211 211 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
212 212 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
213 213 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
214 214 * data it contains.
215 215 *
216 216 */
217 217
218 218 rtems_status_code status; // RTEMS status code
219 219 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
220 220 ring_node *incomingRingNodePtr;
221 221 int ring_node_address;
222 222 char *charPtr;
223 223 spw_ioctl_pkt_send *spw_ioctl_send;
224 224 size_t size; // size of the incoming TC packet
225 225 rtems_id queue_send_id;
226 226 unsigned int sid;
227 227 unsigned char sidAsUnsignedChar;
228 228 unsigned char type;
229 229
230 230 incomingRingNodePtr = NULL;
231 231 ring_node_address = 0;
232 232 charPtr = (char *) &ring_node_address;
233 233 sid = 0;
234 234 sidAsUnsignedChar = 0;
235 235
236 236 init_header_cwf( &headerCWF );
237 237 init_header_swf( &headerSWF );
238 238 init_header_asm( &headerASM );
239 239
240 240 status = get_message_queue_id_send( &queue_send_id );
241 241 if (status != RTEMS_SUCCESSFUL)
242 242 {
243 243 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
244 244 }
245 245
246 246 BOOT_PRINTF("in SEND *** \n")
247 247
248 248 while(1)
249 249 {
250 250 status = rtems_message_queue_receive( queue_send_id, incomingData, &size,
251 251 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
252 252
253 253 if (status!=RTEMS_SUCCESSFUL)
254 254 {
255 255 PRINTF1("in SEND *** (1) ERR = %d\n", status)
256 256 }
257 257 else
258 258 {
259 259 if ( size == sizeof(ring_node*) )
260 260 {
261 261 charPtr[0] = incomingData[0];
262 262 charPtr[1] = incomingData[1];
263 charPtr[2] = incomingData[2];
264 charPtr[3] = incomingData[3];
263 charPtr[BYTE_2] = incomingData[BYTE_2];
264 charPtr[BYTE_3] = incomingData[BYTE_3];
265 265 incomingRingNodePtr = (ring_node*) ring_node_address;
266 266 sid = incomingRingNodePtr->sid;
267 267 if ( (sid==SID_NORM_CWF_LONG_F3)
268 268 || (sid==SID_BURST_CWF_F2 )
269 269 || (sid==SID_SBM1_CWF_F1 )
270 270 || (sid==SID_SBM2_CWF_F2 ))
271 271 {
272 272 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
273 273 }
274 274 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
275 275 {
276 276 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
277 277 }
278 278 else if ( (sid==SID_NORM_CWF_F3) )
279 279 {
280 280 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
281 281 }
282 282 else if (sid==SID_NORM_ASM_F0)
283 283 {
284 284 spw_send_asm_f0( incomingRingNodePtr, &headerASM );
285 285 }
286 286 else if (sid==SID_NORM_ASM_F1)
287 287 {
288 288 spw_send_asm_f1( incomingRingNodePtr, &headerASM );
289 289 }
290 290 else if (sid==SID_NORM_ASM_F2)
291 291 {
292 292 spw_send_asm_f2( incomingRingNodePtr, &headerASM );
293 293 }
294 294 else if ( sid==TM_CODE_K_DUMP )
295 295 {
296 296 spw_send_k_dump( incomingRingNodePtr );
297 297 }
298 298 else
299 299 {
300 300 PRINTF1("unexpected sid = %d\n", sid);
301 301 }
302 302 }
303 303 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
304 304 {
305 305 sidAsUnsignedChar = (unsigned char) incomingData[ PACKET_POS_PA_LFR_SID_PKT ];
306 306 sid = sidAsUnsignedChar;
307 307 type = (unsigned char) incomingData[ PACKET_POS_SERVICE_TYPE ];
308 308 if (type == TM_TYPE_LFR_SCIENCE) // this is a BP packet, all other types are handled differently
309 309 // SET THE SEQUENCE_CNT PARAMETER IN CASE OF BP0 OR BP1 PACKETS
310 310 {
311 311 increment_seq_counter_source_id( (unsigned char*) &incomingData[ PACKET_POS_SEQUENCE_CNT ], sid );
312 312 }
313 313
314 314 status = write( fdSPW, incomingData, size );
315 315 if (status == -1){
316 316 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
317 317 }
318 318 }
319 319 else // the incoming message is a spw_ioctl_pkt_send structure
320 320 {
321 321 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
322 322 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
323 323 if (status == -1){
324 324 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
325 325 }
326 326 }
327 327 }
328 328
329 329 update_queue_max_count( queue_send_id, &hk_lfr_q_sd_fifo_size_max );
330 330
331 331 }
332 332 }
333 333
334 334 rtems_task link_task( rtems_task_argument argument )
335 335 {
336 336 rtems_event_set event_out;
337 337 rtems_status_code status;
338 338 int linkStatus;
339 339
340 340 BOOT_PRINTF("in LINK ***\n")
341 341
342 342 while(1)
343 343 {
344 344 // wait for an RTEMS_EVENT
345 345 rtems_event_receive( RTEMS_EVENT_0,
346 346 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
347 347 PRINTF("in LINK *** wait for the link\n")
348 348 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
349 while( linkStatus != 5) // wait for the link
349 while( linkStatus != SPW_LINK_OK) // wait for the link
350 350 {
351 status = rtems_task_wake_after( 10 ); // monitor the link each 100ms
351 status = rtems_task_wake_after( SPW_LINK_WAIT ); // monitor the link each 100ms
352 352 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
353 353 watchdog_reload();
354 354 }
355 355
356 356 spacewire_read_statistics();
357 357 status = spacewire_stop_and_start_link( fdSPW );
358 358
359 359 if (status != RTEMS_SUCCESSFUL)
360 360 {
361 361 PRINTF1("in LINK *** ERR link not started %d\n", status)
362 362 }
363 363 else
364 364 {
365 365 PRINTF("in LINK *** OK link started\n")
366 366 }
367 367
368 368 // restart the SPIQ task
369 369 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
370 370 if ( status != RTEMS_SUCCESSFUL ) {
371 371 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
372 372 }
373 373
374 374 // restart RECV and SEND
375 375 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
376 376 if ( status != RTEMS_SUCCESSFUL ) {
377 377 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
378 378 }
379 379 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
380 380 if ( status != RTEMS_SUCCESSFUL ) {
381 381 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
382 382 }
383 383 }
384 384 }
385 385
386 386 //****************
387 387 // OTHER FUNCTIONS
388 388 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
389 389 {
390 390 /** This function opens the SpaceWire link.
391 391 *
392 392 * @return a valid file descriptor in case of success, -1 in case of a failure
393 393 *
394 394 */
395 395 rtems_status_code status;
396 396
397 397 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
398 398 if ( fdSPW < 0 ) {
399 399 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
400 400 }
401 401 else
402 402 {
403 403 status = RTEMS_SUCCESSFUL;
404 404 }
405 405
406 406 return status;
407 407 }
408 408
409 409 int spacewire_start_link( int fd )
410 410 {
411 411 rtems_status_code status;
412 412
413 413 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
414 414 // -1 default hardcoded driver timeout
415 415
416 416 return status;
417 417 }
418 418
419 419 int spacewire_stop_and_start_link( int fd )
420 420 {
421 421 rtems_status_code status;
422 422
423 423 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
424 424 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
425 425 // -1 default hardcoded driver timeout
426 426
427 427 return status;
428 428 }
429 429
430 430 int spacewire_configure_link( int fd )
431 431 {
432 432 /** This function configures the SpaceWire link.
433 433 *
434 434 * @return GR-RTEMS-DRIVER directive status codes:
435 435 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
436 436 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
437 437 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
438 438 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
439 439 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
440 440 * - 5 EIO - Error when writing to grswp hardware registers.
441 441 * - 2 ENOENT - No such file or directory
442 442 */
443 443
444 444 rtems_status_code status;
445 445
446 446 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
447 447 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
448 448 spw_ioctl_packetsize packetsize;
449 449
450 packetsize.rxsize = 228;
451 packetsize.txdsize = 4096;
452 packetsize.txhsize = 34;
450 packetsize.rxsize = SPW_RXSIZE;
451 packetsize.txdsize = SPW_TXDSIZE;
452 packetsize.txhsize = SPW_TXHSIZE;
453 453
454 454 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
455 455 if (status!=RTEMS_SUCCESSFUL) {
456 456 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
457 457 }
458 458 //
459 459 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
460 460 if (status!=RTEMS_SUCCESSFUL) {
461 461 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
462 462 }
463 463 //
464 464 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
465 465 if (status!=RTEMS_SUCCESSFUL) {
466 466 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
467 467 }
468 468 //
469 469 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
470 470 if (status!=RTEMS_SUCCESSFUL) {
471 471 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
472 472 }
473 473 //
474 474 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
475 475 if (status!=RTEMS_SUCCESSFUL) {
476 476 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
477 477 }
478 478 //
479 479 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
480 480 if (status!=RTEMS_SUCCESSFUL) {
481 481 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
482 482 }
483 483 //
484 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, 0x0909); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
484 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, CONF_TCODE_CTRL); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
485 485 if (status!=RTEMS_SUCCESSFUL) {
486 486 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
487 487 }
488 488 //
489 489 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_PACKETSIZE, packetsize); // set rxsize, txdsize and txhsize
490 490 if (status!=RTEMS_SUCCESSFUL) {
491 491 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_PACKETSIZE,\n")
492 492 }
493 493
494 494 return status;
495 495 }
496 496
497 497 int spacewire_several_connect_attemps( void )
498 498 {
499 499 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
500 500 *
501 501 * @return RTEMS directive status code:
502 502 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
503 503 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
504 504 *
505 505 */
506 506
507 507 rtems_status_code status_spw;
508 508 rtems_status_code status;
509 509 int i;
510 510
511 511 for ( i=0; i<SY_LFR_DPU_CONNECT_ATTEMPT; i++ )
512 512 {
513 513 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
514 514
515 515 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
516 516
517 517 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
518 518
519 519 status_spw = spacewire_stop_and_start_link( fdSPW );
520 520
521 521 if ( status_spw != RTEMS_SUCCESSFUL )
522 522 {
523 523 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw)
524 524 }
525 525
526 526 if ( status_spw == RTEMS_SUCCESSFUL)
527 527 {
528 528 break;
529 529 }
530 530 }
531 531
532 532 return status_spw;
533 533 }
534 534
535 535 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
536 536 {
537 537 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
538 538 *
539 539 * @param val is the value, 0 or 1, used to set the value of the NP bit.
540 540 * @param regAddr is the address of the GRSPW control register.
541 541 *
542 542 * NP is the bit 20 of the GRSPW control register.
543 543 *
544 544 */
545 545
546 546 unsigned int *spwptr = (unsigned int*) regAddr;
547 547
548 548 if (val == 1) {
549 *spwptr = *spwptr | 0x00100000; // [NP] set the No port force bit
549 *spwptr = *spwptr | SPW_BIT_NP; // [NP] set the No port force bit
550 550 }
551 551 if (val== 0) {
552 *spwptr = *spwptr & 0xffdfffff;
552 *spwptr = *spwptr & SPW_BIT_NP_MASK;
553 553 }
554 554 }
555 555
556 556 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
557 557 {
558 558 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
559 559 *
560 560 * @param val is the value, 0 or 1, used to set the value of the RE bit.
561 561 * @param regAddr is the address of the GRSPW control register.
562 562 *
563 563 * RE is the bit 16 of the GRSPW control register.
564 564 *
565 565 */
566 566
567 567 unsigned int *spwptr = (unsigned int*) regAddr;
568 568
569 569 if (val == 1)
570 570 {
571 *spwptr = *spwptr | 0x00010000; // [RE] set the RMAP Enable bit
571 *spwptr = *spwptr | SPW_BIT_RE; // [RE] set the RMAP Enable bit
572 572 }
573 573 if (val== 0)
574 574 {
575 *spwptr = *spwptr & 0xfffdffff;
575 *spwptr = *spwptr & SPW_BIT_RE_MASK;
576 576 }
577 577 }
578 578
579 579 void spacewire_read_statistics( void )
580 580 {
581 581 /** This function reads the SpaceWire statistics from the grspw RTEMS driver.
582 582 *
583 583 * @param void
584 584 *
585 585 * @return void
586 586 *
587 587 * Once they are read, the counters are stored in a global variable used during the building of the
588 588 * HK packets.
589 589 *
590 590 */
591 591
592 592 rtems_status_code status;
593 593 spw_stats current;
594 594
595 595 spacewire_get_last_error();
596 596
597 597 // read the current statistics
598 598 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &current );
599 599
600 600 // clear the counters
601 601 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_CLR_STATISTICS );
602 602
603 603 // typedef struct {
604 604 // unsigned int tx_link_err; // NOT IN HK
605 605 // unsigned int rx_rmap_header_crc_err; // NOT IN HK
606 606 // unsigned int rx_rmap_data_crc_err; // NOT IN HK
607 607 // unsigned int rx_eep_err;
608 608 // unsigned int rx_truncated;
609 609 // unsigned int parity_err;
610 610 // unsigned int escape_err;
611 611 // unsigned int credit_err;
612 612 // unsigned int write_sync_err;
613 613 // unsigned int disconnect_err;
614 614 // unsigned int early_ep;
615 615 // unsigned int invalid_address;
616 616 // unsigned int packets_sent;
617 617 // unsigned int packets_received;
618 618 // } spw_stats;
619 619
620 620 // rx_eep_err
621 621 grspw_stats.rx_eep_err = grspw_stats.rx_eep_err + current.rx_eep_err;
622 622 // rx_truncated
623 623 grspw_stats.rx_truncated = grspw_stats.rx_truncated + current.rx_truncated;
624 624 // parity_err
625 625 grspw_stats.parity_err = grspw_stats.parity_err + current.parity_err;
626 626 // escape_err
627 627 grspw_stats.escape_err = grspw_stats.escape_err + current.escape_err;
628 628 // credit_err
629 629 grspw_stats.credit_err = grspw_stats.credit_err + current.credit_err;
630 630 // write_sync_err
631 631 grspw_stats.write_sync_err = grspw_stats.write_sync_err + current.write_sync_err;
632 632 // disconnect_err
633 633 grspw_stats.disconnect_err = grspw_stats.disconnect_err + current.disconnect_err;
634 634 // early_ep
635 635 grspw_stats.early_ep = grspw_stats.early_ep + current.early_ep;
636 636 // invalid_address
637 637 grspw_stats.invalid_address = grspw_stats.invalid_address + current.invalid_address;
638 638 // packets_sent
639 639 grspw_stats.packets_sent = grspw_stats.packets_sent + current.packets_sent;
640 640 // packets_received
641 641 grspw_stats.packets_received= grspw_stats.packets_received + current.packets_received;
642 642
643 643 }
644 644
645 645 void spacewire_get_last_error( void )
646 646 {
647 647 static spw_stats previous;
648 648 spw_stats current;
649 649 rtems_status_code status;
650 650
651 651 unsigned int hk_lfr_last_er_rid;
652 652 unsigned char hk_lfr_last_er_code;
653 653 int coarseTime;
654 654 int fineTime;
655 655 unsigned char update_hk_lfr_last_er;
656 656
657 657 update_hk_lfr_last_er = 0;
658 658
659 659 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &current );
660 660
661 661 // get current time
662 662 coarseTime = time_management_regs->coarse_time;
663 663 fineTime = time_management_regs->fine_time;
664 664
665 665 // typedef struct {
666 666 // unsigned int tx_link_err; // NOT IN HK
667 667 // unsigned int rx_rmap_header_crc_err; // NOT IN HK
668 668 // unsigned int rx_rmap_data_crc_err; // NOT IN HK
669 669 // unsigned int rx_eep_err;
670 670 // unsigned int rx_truncated;
671 671 // unsigned int parity_err;
672 672 // unsigned int escape_err;
673 673 // unsigned int credit_err;
674 674 // unsigned int write_sync_err;
675 675 // unsigned int disconnect_err;
676 676 // unsigned int early_ep;
677 677 // unsigned int invalid_address;
678 678 // unsigned int packets_sent;
679 679 // unsigned int packets_received;
680 680 // } spw_stats;
681 681
682 682 // tx_link_err *** no code associated to this field
683 683 // rx_rmap_header_crc_err *** LE *** in HK
684 684 if (previous.rx_rmap_header_crc_err != current.rx_rmap_header_crc_err)
685 685 {
686 686 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
687 687 hk_lfr_last_er_code = CODE_HEADER_CRC;
688 688 update_hk_lfr_last_er = 1;
689 689 }
690 690 // rx_rmap_data_crc_err *** LE *** NOT IN HK
691 691 if (previous.rx_rmap_data_crc_err != current.rx_rmap_data_crc_err)
692 692 {
693 693 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
694 694 hk_lfr_last_er_code = CODE_DATA_CRC;
695 695 update_hk_lfr_last_er = 1;
696 696 }
697 697 // rx_eep_err
698 698 if (previous.rx_eep_err != current.rx_eep_err)
699 699 {
700 700 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
701 701 hk_lfr_last_er_code = CODE_EEP;
702 702 update_hk_lfr_last_er = 1;
703 703 }
704 704 // rx_truncated
705 705 if (previous.rx_truncated != current.rx_truncated)
706 706 {
707 707 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
708 708 hk_lfr_last_er_code = CODE_RX_TOO_BIG;
709 709 update_hk_lfr_last_er = 1;
710 710 }
711 711 // parity_err
712 712 if (previous.parity_err != current.parity_err)
713 713 {
714 714 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
715 715 hk_lfr_last_er_code = CODE_PARITY;
716 716 update_hk_lfr_last_er = 1;
717 717 }
718 718 // escape_err
719 719 if (previous.parity_err != current.parity_err)
720 720 {
721 721 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
722 722 hk_lfr_last_er_code = CODE_ESCAPE;
723 723 update_hk_lfr_last_er = 1;
724 724 }
725 725 // credit_err
726 726 if (previous.credit_err != current.credit_err)
727 727 {
728 728 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
729 729 hk_lfr_last_er_code = CODE_CREDIT;
730 730 update_hk_lfr_last_er = 1;
731 731 }
732 732 // write_sync_err
733 733 if (previous.write_sync_err != current.write_sync_err)
734 734 {
735 735 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
736 736 hk_lfr_last_er_code = CODE_WRITE_SYNC;
737 737 update_hk_lfr_last_er = 1;
738 738 }
739 739 // disconnect_err
740 740 if (previous.disconnect_err != current.disconnect_err)
741 741 {
742 742 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
743 743 hk_lfr_last_er_code = CODE_DISCONNECT;
744 744 update_hk_lfr_last_er = 1;
745 745 }
746 746 // early_ep
747 747 if (previous.early_ep != current.early_ep)
748 748 {
749 749 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
750 750 hk_lfr_last_er_code = CODE_EARLY_EOP_EEP;
751 751 update_hk_lfr_last_er = 1;
752 752 }
753 753 // invalid_address
754 754 if (previous.invalid_address != current.invalid_address)
755 755 {
756 756 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
757 757 hk_lfr_last_er_code = CODE_INVALID_ADDRESS;
758 758 update_hk_lfr_last_er = 1;
759 759 }
760 760
761 761 // if a field has changed, update the hk_last_er fields
762 762 if (update_hk_lfr_last_er == 1)
763 763 {
764 764 update_hk_lfr_last_er_fields( hk_lfr_last_er_rid, hk_lfr_last_er_code );
765 765 }
766 766
767 767 previous = current;
768 768 }
769 769
770 770 void update_hk_lfr_last_er_fields(unsigned int rid, unsigned char code)
771 771 {
772 772 unsigned char *coarseTimePtr;
773 773 unsigned char *fineTimePtr;
774 774
775 775 coarseTimePtr = (unsigned char*) &time_management_regs->coarse_time;
776 776 fineTimePtr = (unsigned char*) &time_management_regs->fine_time;
777 777
778 housekeeping_packet.hk_lfr_last_er_rid[0] = (unsigned char) ((rid & 0xff00) >> 8 );
779 housekeeping_packet.hk_lfr_last_er_rid[1] = (unsigned char) (rid & 0x00ff);
778 housekeeping_packet.hk_lfr_last_er_rid[0] = (unsigned char) ((rid & BYTE0_MASK) >> SHIFT_1_BYTE );
779 housekeeping_packet.hk_lfr_last_er_rid[1] = (unsigned char) (rid & BYTE1_MASK);
780 780 housekeeping_packet.hk_lfr_last_er_code = code;
781 781 housekeeping_packet.hk_lfr_last_er_time[0] = coarseTimePtr[0];
782 782 housekeeping_packet.hk_lfr_last_er_time[1] = coarseTimePtr[1];
783 housekeeping_packet.hk_lfr_last_er_time[2] = coarseTimePtr[2];
784 housekeeping_packet.hk_lfr_last_er_time[3] = coarseTimePtr[3];
785 housekeeping_packet.hk_lfr_last_er_time[4] = fineTimePtr[2];
786 housekeeping_packet.hk_lfr_last_er_time[5] = fineTimePtr[3];
783 housekeeping_packet.hk_lfr_last_er_time[BYTE_2] = coarseTimePtr[BYTE_2];
784 housekeeping_packet.hk_lfr_last_er_time[BYTE_3] = coarseTimePtr[BYTE_3];
785 housekeeping_packet.hk_lfr_last_er_time[BYTE_4] = fineTimePtr[BYTE_2];
786 housekeeping_packet.hk_lfr_last_er_time[BYTE_5] = fineTimePtr[BYTE_3];
787 787 }
788 788
789 789 void update_hk_with_grspw_stats( void )
790 790 {
791 791 //****************************
792 792 // DPU_SPACEWIRE_IF_STATISTICS
793 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (grspw_stats.packets_received >> 8);
793 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (grspw_stats.packets_received >> SHIFT_1_BYTE);
794 794 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (grspw_stats.packets_received);
795 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (grspw_stats.packets_sent >> 8);
795 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (grspw_stats.packets_sent >> SHIFT_1_BYTE);
796 796 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (grspw_stats.packets_sent);
797 797
798 798 //******************************************
799 799 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
800 800 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) grspw_stats.parity_err;
801 801 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) grspw_stats.disconnect_err;
802 802 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) grspw_stats.escape_err;
803 803 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) grspw_stats.credit_err;
804 804 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) grspw_stats.write_sync_err;
805 805
806 806 //*********************************************
807 807 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
808 808 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) grspw_stats.early_ep;
809 809 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) grspw_stats.invalid_address;
810 810 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) grspw_stats.rx_eep_err;
811 811 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) grspw_stats.rx_truncated;
812 812 }
813 813
814 814 void spacewire_update_hk_lfr_link_state( unsigned char *hk_lfr_status_word_0 )
815 815 {
816 816 unsigned int *statusRegisterPtr;
817 817 unsigned char linkState;
818 818
819 819 statusRegisterPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_STATUS_REGISTER);
820 linkState = (unsigned char) ( ( (*statusRegisterPtr) >> 21) & 0x07); // [0000 0111]
820 linkState =
821 (unsigned char) ( ( (*statusRegisterPtr) >> SPW_LINK_STAT_POS) & STATUS_WORD_LINK_STATE_BITS); // [0000 0111]
821 822
822 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 & 0xf8; // [1111 1000] set link state to 0
823 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 & STATUS_WORD_LINK_STATE_MASK; // [1111 1000] set link state to 0
823 824
824 825 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 | linkState; // update hk_lfr_dpu_spw_link_state
825 826 }
826 827
827 828 void increase_unsigned_char_counter( unsigned char *counter )
828 829 {
829 830 // update the number of valid timecodes that have been received
830 if (*counter == 255)
831 if (*counter == UINT8_MAX)
831 832 {
832 833 *counter = 0;
833 834 }
834 835 else
835 836 {
836 837 *counter = *counter + 1;
837 838 }
838 839 }
839 840
840 841 unsigned int check_timecode_and_previous_timecode_coherency(unsigned char currentTimecodeCtr)
841 842 {
842 843 /** This function checks the coherency between the incoming timecode and the last valid timecode.
843 844 *
844 845 * @param currentTimecodeCtr is the incoming timecode
845 846 *
846 847 * @return returned codes::
847 848 * - LFR_DEFAULT
848 849 * - LFR_SUCCESSFUL
849 850 *
850 851 */
851 852
852 853 static unsigned char firstTickout = 1;
853 854 unsigned char ret;
854 855
855 856 ret = LFR_DEFAULT;
856 857
857 858 if (firstTickout == 0)
858 859 {
859 860 if (currentTimecodeCtr == 0)
860 861 {
861 if (previousTimecodeCtr == 63)
862 if (previousTimecodeCtr == SPW_TIMECODE_MAX)
862 863 {
863 864 ret = LFR_SUCCESSFUL;
864 865 }
865 866 else
866 867 {
867 868 ret = LFR_DEFAULT;
868 869 }
869 870 }
870 871 else
871 872 {
872 873 if (currentTimecodeCtr == (previousTimecodeCtr +1))
873 874 {
874 875 ret = LFR_SUCCESSFUL;
875 876 }
876 877 else
877 878 {
878 879 ret = LFR_DEFAULT;
879 880 }
880 881 }
881 882 }
882 883 else
883 884 {
884 885 firstTickout = 0;
885 886 ret = LFR_SUCCESSFUL;
886 887 }
887 888
888 889 return ret;
889 890 }
890 891
891 892 unsigned int check_timecode_and_internal_time_coherency(unsigned char timecode, unsigned char internalTime)
892 893 {
893 894 unsigned int ret;
894 895
895 896 ret = LFR_DEFAULT;
896 897
897 898 if (timecode == internalTime)
898 899 {
899 900 ret = LFR_SUCCESSFUL;
900 901 }
901 902 else
902 903 {
903 904 ret = LFR_DEFAULT;
904 905 }
905 906
906 907 return ret;
907 908 }
908 909
909 910 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
910 911 {
911 912 // a tickout has been emitted, perform actions on the incoming timecode
912 913
913 914 unsigned char incomingTimecode;
914 915 unsigned char updateTime;
915 916 unsigned char internalTime;
916 917 rtems_status_code status;
917 918
918 919 incomingTimecode = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
919 920 updateTime = time_management_regs->coarse_time_load & TIMECODE_MASK;
920 921 internalTime = time_management_regs->coarse_time & TIMECODE_MASK;
921 922
922 923 housekeeping_packet.hk_lfr_dpu_spw_last_timc = incomingTimecode;
923 924
924 925 // update the number of tickout that have been generated
925 926 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt );
926 927
927 928 //**************************
928 929 // HK_LFR_TIMECODE_ERRONEOUS
929 930 // MISSING and INVALID are handled by the timecode_timer_routine service routine
930 931 if (check_timecode_and_previous_timecode_coherency( incomingTimecode ) == LFR_DEFAULT)
931 932 {
932 933 // this is unexpected but a tickout could have been raised despite of the timecode being erroneous
933 934 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_erroneous );
934 935 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_ERRONEOUS );
935 936 }
936 937
937 938 //************************
938 939 // HK_LFR_TIME_TIMECODE_IT
939 940 // check the coherency between the SpaceWire timecode and the Internal Time
940 941 if (check_timecode_and_internal_time_coherency( incomingTimecode, internalTime ) == LFR_DEFAULT)
941 942 {
942 943 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_it );
943 944 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_IT );
944 945 }
945 946
946 947 //********************
947 948 // HK_LFR_TIMECODE_CTR
948 949 // check the value of the timecode with respect to the last TC_LFR_UPDATE_TIME => SSS-CP-FS-370
949 950 if (oneTcLfrUpdateTimeReceived == 1)
950 951 {
951 952 if ( incomingTimecode != updateTime )
952 953 {
953 954 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_ctr );
954 955 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_CTR );
955 956 }
956 957 }
957 958
958 959 // launch the timecode timer to detect missing or invalid timecodes
959 960 previousTimecodeCtr = incomingTimecode; // update the previousTimecodeCtr value
960 961 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT, timecode_timer_routine, NULL );
961 962 if (status != RTEMS_SUCCESSFUL)
962 963 {
963 964 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_14 );
964 965 }
965 966 }
966 967
967 968 rtems_timer_service_routine timecode_timer_routine( rtems_id timer_id, void *user_data )
968 969 {
969 970 static unsigned char initStep = 1;
970 971
971 972 unsigned char currentTimecodeCtr;
972 973
973 974 currentTimecodeCtr = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
974 975
975 976 if (initStep == 1)
976 977 {
977 978 if (currentTimecodeCtr == previousTimecodeCtr)
978 979 {
979 980 //************************
980 981 // HK_LFR_TIMECODE_MISSING
981 982 // the timecode value has not changed, no valid timecode has been received, the timecode is MISSING
982 983 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
983 984 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING );
984 985 }
985 986 else if (currentTimecodeCtr == (previousTimecodeCtr+1))
986 987 {
987 988 // the timecode value has changed and the value is valid, this is unexpected because
988 989 // the timer should not have fired, the timecode_irq_handler should have been raised
989 990 }
990 991 else
991 992 {
992 993 //************************
993 994 // HK_LFR_TIMECODE_INVALID
994 995 // the timecode value has changed and the value is not valid, no tickout has been generated
995 996 // this is why the timer has fired
996 997 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_invalid );
997 998 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_INVALID );
998 999 }
999 1000 }
1000 1001 else
1001 1002 {
1002 1003 initStep = 1;
1003 1004 //************************
1004 1005 // HK_LFR_TIMECODE_MISSING
1005 1006 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
1006 1007 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING );
1007 1008 }
1008 1009
1009 1010 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_13 );
1010 1011 }
1011 1012
1012 1013 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
1013 1014 {
1014 1015 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1015 1016 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1016 1017 header->reserved = DEFAULT_RESERVED;
1017 1018 header->userApplication = CCSDS_USER_APP;
1018 1019 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
1019 1020 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
1020 header->packetLength[0] = 0x00;
1021 header->packetLength[1] = 0x00;
1021 header->packetLength[0] = INIT_CHAR;
1022 header->packetLength[1] = INIT_CHAR;
1022 1023 // DATA FIELD HEADER
1023 1024 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1024 1025 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1025 1026 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
1026 1027 header->destinationID = TM_DESTINATION_ID_GROUND;
1027 header->time[0] = 0x00;
1028 header->time[0] = 0x00;
1029 header->time[0] = 0x00;
1030 header->time[0] = 0x00;
1031 header->time[0] = 0x00;
1032 header->time[0] = 0x00;
1028 header->time[BYTE_0] = INIT_CHAR;
1029 header->time[BYTE_1] = INIT_CHAR;
1030 header->time[BYTE_2] = INIT_CHAR;
1031 header->time[BYTE_3] = INIT_CHAR;
1032 header->time[BYTE_4] = INIT_CHAR;
1033 header->time[BYTE_5] = INIT_CHAR;
1033 1034 // AUXILIARY DATA HEADER
1034 header->sid = 0x00;
1035 header->sid = INIT_CHAR;
1035 1036 header->pa_bia_status_info = DEFAULT_HKBIA;
1036 header->blkNr[0] = 0x00;
1037 header->blkNr[1] = 0x00;
1037 header->blkNr[0] = INIT_CHAR;
1038 header->blkNr[1] = INIT_CHAR;
1038 1039 }
1039 1040
1040 1041 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
1041 1042 {
1042 1043 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1043 1044 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1044 1045 header->reserved = DEFAULT_RESERVED;
1045 1046 header->userApplication = CCSDS_USER_APP;
1046 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1047 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1047 1048 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1048 1049 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1049 1050 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1050 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
1051 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> SHIFT_1_BYTE);
1051 1052 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
1052 1053 // DATA FIELD HEADER
1053 1054 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1054 1055 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1055 1056 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
1056 1057 header->destinationID = TM_DESTINATION_ID_GROUND;
1057 header->time[0] = 0x00;
1058 header->time[0] = 0x00;
1059 header->time[0] = 0x00;
1060 header->time[0] = 0x00;
1061 header->time[0] = 0x00;
1062 header->time[0] = 0x00;
1058 header->time[BYTE_0] = INIT_CHAR;
1059 header->time[BYTE_1] = INIT_CHAR;
1060 header->time[BYTE_2] = INIT_CHAR;
1061 header->time[BYTE_3] = INIT_CHAR;
1062 header->time[BYTE_4] = INIT_CHAR;
1063 header->time[BYTE_5] = INIT_CHAR;
1063 1064 // AUXILIARY DATA HEADER
1064 header->sid = 0x00;
1065 header->sid = INIT_CHAR;
1065 1066 header->pa_bia_status_info = DEFAULT_HKBIA;
1066 header->pktCnt = DEFAULT_PKTCNT; // PKT_CNT
1067 header->pktNr = 0x00;
1068 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
1067 header->pktCnt = PKTCNT_SWF; // PKT_CNT
1068 header->pktNr = INIT_CHAR;
1069 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> SHIFT_1_BYTE);
1069 1070 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
1070 1071 }
1071 1072
1072 1073 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
1073 1074 {
1074 1075 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1075 1076 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1076 1077 header->reserved = DEFAULT_RESERVED;
1077 1078 header->userApplication = CCSDS_USER_APP;
1078 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1079 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1079 1080 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1080 1081 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1081 1082 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1082 header->packetLength[0] = 0x00;
1083 header->packetLength[1] = 0x00;
1083 header->packetLength[0] = INIT_CHAR;
1084 header->packetLength[1] = INIT_CHAR;
1084 1085 // DATA FIELD HEADER
1085 1086 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1086 1087 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1087 1088 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
1088 1089 header->destinationID = TM_DESTINATION_ID_GROUND;
1089 header->time[0] = 0x00;
1090 header->time[0] = 0x00;
1091 header->time[0] = 0x00;
1092 header->time[0] = 0x00;
1093 header->time[0] = 0x00;
1094 header->time[0] = 0x00;
1090 header->time[BYTE_0] = INIT_CHAR;
1091 header->time[BYTE_1] = INIT_CHAR;
1092 header->time[BYTE_2] = INIT_CHAR;
1093 header->time[BYTE_3] = INIT_CHAR;
1094 header->time[BYTE_4] = INIT_CHAR;
1095 header->time[BYTE_5] = INIT_CHAR;
1095 1096 // AUXILIARY DATA HEADER
1096 header->sid = 0x00;
1097 header->pa_bia_status_info = 0x00;
1098 header->pa_lfr_pkt_cnt_asm = 0x00;
1099 header->pa_lfr_pkt_nr_asm = 0x00;
1100 header->pa_lfr_asm_blk_nr[0] = 0x00;
1101 header->pa_lfr_asm_blk_nr[1] = 0x00;
1097 header->sid = INIT_CHAR;
1098 header->pa_bia_status_info = INIT_CHAR;
1099 header->pa_lfr_pkt_cnt_asm = INIT_CHAR;
1100 header->pa_lfr_pkt_nr_asm = INIT_CHAR;
1101 header->pa_lfr_asm_blk_nr[0] = INIT_CHAR;
1102 header->pa_lfr_asm_blk_nr[1] = INIT_CHAR;
1102 1103 }
1103 1104
1104 1105 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
1105 1106 Header_TM_LFR_SCIENCE_CWF_t *header )
1106 1107 {
1107 1108 /** This function sends CWF CCSDS packets (F2, F1 or F0).
1108 1109 *
1109 1110 * @param waveform points to the buffer containing the data that will be send.
1110 1111 * @param sid is the source identifier of the data that will be sent.
1111 1112 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1112 1113 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1113 1114 * contain information to setup the transmission of the data packets.
1114 1115 *
1115 1116 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1116 1117 *
1117 1118 */
1118 1119
1119 1120 unsigned int i;
1120 1121 int ret;
1121 1122 unsigned int coarseTime;
1122 1123 unsigned int fineTime;
1123 1124 rtems_status_code status;
1124 1125 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1125 1126 int *dataPtr;
1126 1127 unsigned char sid;
1127 1128
1128 1129 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1129 1130 spw_ioctl_send_CWF.options = 0;
1130 1131
1131 1132 ret = LFR_DEFAULT;
1132 1133 sid = (unsigned char) ring_node_to_send->sid;
1133 1134
1134 1135 coarseTime = ring_node_to_send->coarseTime;
1135 1136 fineTime = ring_node_to_send->fineTime;
1136 1137 dataPtr = (int*) ring_node_to_send->buffer_address;
1137 1138
1138 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
1139 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> SHIFT_1_BYTE);
1139 1140 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
1140 1141 header->pa_bia_status_info = pa_bia_status_info;
1141 1142 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1142 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
1143 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> SHIFT_1_BYTE);
1143 1144 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
1144 1145
1145 1146 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
1146 1147 {
1147 1148 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
1148 1149 spw_ioctl_send_CWF.hdr = (char*) header;
1149 1150 // BUILD THE DATA
1150 1151 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
1151 1152
1152 1153 // SET PACKET SEQUENCE CONTROL
1153 1154 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1154 1155
1155 1156 // SET SID
1156 1157 header->sid = sid;
1157 1158
1158 1159 // SET PACKET TIME
1159 1160 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
1160 1161 //
1161 1162 header->time[0] = header->acquisitionTime[0];
1162 1163 header->time[1] = header->acquisitionTime[1];
1163 header->time[2] = header->acquisitionTime[2];
1164 header->time[3] = header->acquisitionTime[3];
1165 header->time[4] = header->acquisitionTime[4];
1166 header->time[5] = header->acquisitionTime[5];
1164 header->time[BYTE_2] = header->acquisitionTime[BYTE_2];
1165 header->time[BYTE_3] = header->acquisitionTime[BYTE_3];
1166 header->time[BYTE_4] = header->acquisitionTime[BYTE_4];
1167 header->time[BYTE_5] = header->acquisitionTime[BYTE_5];
1167 1168
1168 1169 // SET PACKET ID
1169 1170 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
1170 1171 {
1171 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8);
1172 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> SHIFT_1_BYTE);
1172 1173 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
1173 1174 }
1174 1175 else
1175 1176 {
1176 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1177 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1177 1178 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1178 1179 }
1179 1180
1180 1181 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1181 1182 if (status != RTEMS_SUCCESSFUL) {
1182 1183 ret = LFR_DEFAULT;
1183 1184 }
1184 1185 }
1185 1186
1186 1187 return ret;
1187 1188 }
1188 1189
1189 1190 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
1190 1191 Header_TM_LFR_SCIENCE_SWF_t *header )
1191 1192 {
1192 1193 /** This function sends SWF CCSDS packets (F2, F1 or F0).
1193 1194 *
1194 1195 * @param waveform points to the buffer containing the data that will be send.
1195 1196 * @param sid is the source identifier of the data that will be sent.
1196 1197 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
1197 1198 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1198 1199 * contain information to setup the transmission of the data packets.
1199 1200 *
1200 1201 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1201 1202 *
1202 1203 */
1203 1204
1204 1205 unsigned int i;
1205 1206 int ret;
1206 1207 unsigned int coarseTime;
1207 1208 unsigned int fineTime;
1208 1209 rtems_status_code status;
1209 1210 spw_ioctl_pkt_send spw_ioctl_send_SWF;
1210 1211 int *dataPtr;
1211 1212 unsigned char sid;
1212 1213
1213 1214 spw_ioctl_send_SWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_SWF;
1214 1215 spw_ioctl_send_SWF.options = 0;
1215 1216
1216 1217 ret = LFR_DEFAULT;
1217 1218
1218 1219 coarseTime = ring_node_to_send->coarseTime;
1219 1220 fineTime = ring_node_to_send->fineTime;
1220 1221 dataPtr = (int*) ring_node_to_send->buffer_address;
1221 1222 sid = ring_node_to_send->sid;
1222 1223
1223 1224 header->pa_bia_status_info = pa_bia_status_info;
1224 1225 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1225 1226
1226 for (i=0; i<7; i++) // send waveform
1227 for (i=0; i<PKTCNT_SWF; i++) // send waveform
1227 1228 {
1228 1229 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
1229 1230 spw_ioctl_send_SWF.hdr = (char*) header;
1230 1231
1231 1232 // SET PACKET SEQUENCE CONTROL
1232 1233 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1233 1234
1234 1235 // SET PACKET LENGTH AND BLKNR
1235 if (i == 6)
1236 if (i == (PKTCNT_SWF-1))
1236 1237 {
1237 1238 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
1238 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8);
1239 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> SHIFT_1_BYTE);
1239 1240 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
1240 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> 8);
1241 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> SHIFT_1_BYTE);
1241 1242 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
1242 1243 }
1243 1244 else
1244 1245 {
1245 1246 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
1246 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8);
1247 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> SHIFT_1_BYTE);
1247 1248 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
1248 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> 8);
1249 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> SHIFT_1_BYTE);
1249 1250 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
1250 1251 }
1251 1252
1252 1253 // SET PACKET TIME
1253 1254 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
1254 1255 //
1255 header->time[0] = header->acquisitionTime[0];
1256 header->time[1] = header->acquisitionTime[1];
1257 header->time[2] = header->acquisitionTime[2];
1258 header->time[3] = header->acquisitionTime[3];
1259 header->time[4] = header->acquisitionTime[4];
1260 header->time[5] = header->acquisitionTime[5];
1256 header->time[BYTE_0] = header->acquisitionTime[BYTE_0];
1257 header->time[BYTE_1] = header->acquisitionTime[BYTE_1];
1258 header->time[BYTE_2] = header->acquisitionTime[BYTE_2];
1259 header->time[BYTE_3] = header->acquisitionTime[BYTE_3];
1260 header->time[BYTE_4] = header->acquisitionTime[BYTE_4];
1261 header->time[BYTE_5] = header->acquisitionTime[BYTE_5];
1261 1262
1262 1263 // SET SID
1263 1264 header->sid = sid;
1264 1265
1265 1266 // SET PKTNR
1266 1267 header->pktNr = i+1; // PKT_NR
1267 1268
1268 1269 // SEND PACKET
1269 1270 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
1270 1271 if (status != RTEMS_SUCCESSFUL) {
1271 1272 ret = LFR_DEFAULT;
1272 1273 }
1273 1274 }
1274 1275
1275 1276 return ret;
1276 1277 }
1277 1278
1278 1279 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
1279 1280 Header_TM_LFR_SCIENCE_CWF_t *header )
1280 1281 {
1281 1282 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
1282 1283 *
1283 1284 * @param waveform points to the buffer containing the data that will be send.
1284 1285 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1285 1286 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1286 1287 * contain information to setup the transmission of the data packets.
1287 1288 *
1288 1289 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
1289 1290 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
1290 1291 *
1291 1292 */
1292 1293
1293 1294 unsigned int i;
1294 1295 int ret;
1295 1296 unsigned int coarseTime;
1296 1297 unsigned int fineTime;
1297 1298 rtems_status_code status;
1298 1299 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1299 1300 char *dataPtr;
1300 1301 unsigned char sid;
1301 1302
1302 1303 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1303 1304 spw_ioctl_send_CWF.options = 0;
1304 1305
1305 1306 ret = LFR_DEFAULT;
1306 1307 sid = ring_node_to_send->sid;
1307 1308
1308 1309 coarseTime = ring_node_to_send->coarseTime;
1309 1310 fineTime = ring_node_to_send->fineTime;
1310 1311 dataPtr = (char*) ring_node_to_send->buffer_address;
1311 1312
1312 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> 8);
1313 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> SHIFT_1_BYTE);
1313 1314 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
1314 1315 header->pa_bia_status_info = pa_bia_status_info;
1315 1316 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1316 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> 8);
1317 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> SHIFT_1_BYTE);
1317 1318 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
1318 1319
1319 1320 //*********************
1320 1321 // SEND CWF3_light DATA
1321 1322 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
1322 1323 {
1323 1324 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
1324 1325 spw_ioctl_send_CWF.hdr = (char*) header;
1325 1326 // BUILD THE DATA
1326 1327 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
1327 1328
1328 1329 // SET PACKET SEQUENCE COUNTER
1329 1330 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1330 1331
1331 1332 // SET SID
1332 1333 header->sid = sid;
1333 1334
1334 1335 // SET PACKET TIME
1335 1336 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1336 1337 //
1337 header->time[0] = header->acquisitionTime[0];
1338 header->time[1] = header->acquisitionTime[1];
1339 header->time[2] = header->acquisitionTime[2];
1340 header->time[3] = header->acquisitionTime[3];
1341 header->time[4] = header->acquisitionTime[4];
1342 header->time[5] = header->acquisitionTime[5];
1338 header->time[BYTE_0] = header->acquisitionTime[BYTE_0];
1339 header->time[BYTE_1] = header->acquisitionTime[BYTE_1];
1340 header->time[BYTE_2] = header->acquisitionTime[BYTE_2];
1341 header->time[BYTE_3] = header->acquisitionTime[BYTE_3];
1342 header->time[BYTE_4] = header->acquisitionTime[BYTE_4];
1343 header->time[BYTE_5] = header->acquisitionTime[BYTE_5];
1343 1344
1344 1345 // SET PACKET ID
1345 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1346 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1346 1347 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1347 1348
1348 1349 // SEND PACKET
1349 1350 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1350 1351 if (status != RTEMS_SUCCESSFUL) {
1351 1352 ret = LFR_DEFAULT;
1352 1353 }
1353 1354 }
1354 1355
1355 1356 return ret;
1356 1357 }
1357 1358
1358 1359 void spw_send_asm_f0( ring_node *ring_node_to_send,
1359 1360 Header_TM_LFR_SCIENCE_ASM_t *header )
1360 1361 {
1361 1362 unsigned int i;
1362 1363 unsigned int length = 0;
1363 1364 rtems_status_code status;
1364 1365 unsigned int sid;
1365 1366 float *spectral_matrix;
1366 1367 int coarseTime;
1367 1368 int fineTime;
1368 1369 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1369 1370
1370 1371 sid = ring_node_to_send->sid;
1371 1372 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1372 1373 coarseTime = ring_node_to_send->coarseTime;
1373 1374 fineTime = ring_node_to_send->fineTime;
1374 1375
1375 1376 header->pa_bia_status_info = pa_bia_status_info;
1376 1377 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1377 1378
1378 for (i=0; i<3; i++)
1379 for (i=0; i<PKTCNT_ASM; i++)
1379 1380 {
1380 1381 if ((i==0) || (i==1))
1381 1382 {
1382 1383 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_1;
1383 1384 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1384 1385 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1385 1386 ];
1386 1387 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_1;
1387 1388 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1388 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> 8 ); // BLK_NR MSB
1389 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> SHIFT_1_BYTE ); // BLK_NR MSB
1389 1390 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_1); // BLK_NR LSB
1390 1391 }
1391 1392 else
1392 1393 {
1393 1394 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_2;
1394 1395 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1395 1396 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1396 1397 ];
1397 1398 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_2;
1398 1399 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1399 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> 8 ); // BLK_NR MSB
1400 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> SHIFT_1_BYTE ); // BLK_NR MSB
1400 1401 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_2); // BLK_NR LSB
1401 1402 }
1402 1403
1403 1404 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1404 1405 spw_ioctl_send_ASM.hdr = (char *) header;
1405 1406 spw_ioctl_send_ASM.options = 0;
1406 1407
1407 1408 // (2) BUILD THE HEADER
1408 1409 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1409 header->packetLength[0] = (unsigned char) (length>>8);
1410 header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE);
1410 1411 header->packetLength[1] = (unsigned char) (length);
1411 1412 header->sid = (unsigned char) sid; // SID
1412 header->pa_lfr_pkt_cnt_asm = 3;
1413 header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM;
1413 1414 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1414 1415
1415 1416 // (3) SET PACKET TIME
1416 header->time[0] = (unsigned char) (coarseTime>>24);
1417 header->time[1] = (unsigned char) (coarseTime>>16);
1418 header->time[2] = (unsigned char) (coarseTime>>8);
1419 header->time[3] = (unsigned char) (coarseTime);
1420 header->time[4] = (unsigned char) (fineTime>>8);
1421 header->time[5] = (unsigned char) (fineTime);
1417 header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES);
1418 header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES);
1419 header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE);
1420 header->time[BYTE_3] = (unsigned char) (coarseTime);
1421 header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE);
1422 header->time[BYTE_5] = (unsigned char) (fineTime);
1422 1423 //
1423 header->acquisitionTime[0] = header->time[0];
1424 header->acquisitionTime[1] = header->time[1];
1425 header->acquisitionTime[2] = header->time[2];
1426 header->acquisitionTime[3] = header->time[3];
1427 header->acquisitionTime[4] = header->time[4];
1428 header->acquisitionTime[5] = header->time[5];
1424 header->acquisitionTime[BYTE_0] = header->time[BYTE_0];
1425 header->acquisitionTime[BYTE_1] = header->time[BYTE_1];
1426 header->acquisitionTime[BYTE_2] = header->time[BYTE_2];
1427 header->acquisitionTime[BYTE_3] = header->time[BYTE_3];
1428 header->acquisitionTime[BYTE_4] = header->time[BYTE_4];
1429 header->acquisitionTime[BYTE_5] = header->time[BYTE_5];
1429 1430
1430 1431 // (4) SEND PACKET
1431 1432 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1432 1433 if (status != RTEMS_SUCCESSFUL) {
1433 1434 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1434 1435 }
1435 1436 }
1436 1437 }
1437 1438
1438 1439 void spw_send_asm_f1( ring_node *ring_node_to_send,
1439 1440 Header_TM_LFR_SCIENCE_ASM_t *header )
1440 1441 {
1441 1442 unsigned int i;
1442 1443 unsigned int length = 0;
1443 1444 rtems_status_code status;
1444 1445 unsigned int sid;
1445 1446 float *spectral_matrix;
1446 1447 int coarseTime;
1447 1448 int fineTime;
1448 1449 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1449 1450
1450 1451 sid = ring_node_to_send->sid;
1451 1452 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1452 1453 coarseTime = ring_node_to_send->coarseTime;
1453 1454 fineTime = ring_node_to_send->fineTime;
1454 1455
1455 1456 header->pa_bia_status_info = pa_bia_status_info;
1456 1457 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1457 1458
1458 for (i=0; i<3; i++)
1459 for (i=0; i<PKTCNT_ASM; i++)
1459 1460 {
1460 1461 if ((i==0) || (i==1))
1461 1462 {
1462 1463 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_1;
1463 1464 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1464 1465 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1465 1466 ];
1466 1467 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_1;
1467 1468 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1468 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> 8 ); // BLK_NR MSB
1469 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> SHIFT_1_BYTE ); // BLK_NR MSB
1469 1470 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_1); // BLK_NR LSB
1470 1471 }
1471 1472 else
1472 1473 {
1473 1474 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_2;
1474 1475 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1475 1476 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1476 1477 ];
1477 1478 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_2;
1478 1479 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1479 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> 8 ); // BLK_NR MSB
1480 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> SHIFT_1_BYTE ); // BLK_NR MSB
1480 1481 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_2); // BLK_NR LSB
1481 1482 }
1482 1483
1483 1484 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1484 1485 spw_ioctl_send_ASM.hdr = (char *) header;
1485 1486 spw_ioctl_send_ASM.options = 0;
1486 1487
1487 1488 // (2) BUILD THE HEADER
1488 1489 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1489 header->packetLength[0] = (unsigned char) (length>>8);
1490 header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE);
1490 1491 header->packetLength[1] = (unsigned char) (length);
1491 1492 header->sid = (unsigned char) sid; // SID
1492 header->pa_lfr_pkt_cnt_asm = 3;
1493 header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM;
1493 1494 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1494 1495
1495 1496 // (3) SET PACKET TIME
1496 header->time[0] = (unsigned char) (coarseTime>>24);
1497 header->time[1] = (unsigned char) (coarseTime>>16);
1498 header->time[2] = (unsigned char) (coarseTime>>8);
1499 header->time[3] = (unsigned char) (coarseTime);
1500 header->time[4] = (unsigned char) (fineTime>>8);
1501 header->time[5] = (unsigned char) (fineTime);
1497 header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES);
1498 header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES);
1499 header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE);
1500 header->time[BYTE_3] = (unsigned char) (coarseTime);
1501 header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE);
1502 header->time[BYTE_5] = (unsigned char) (fineTime);
1502 1503 //
1503 header->acquisitionTime[0] = header->time[0];
1504 header->acquisitionTime[1] = header->time[1];
1505 header->acquisitionTime[2] = header->time[2];
1506 header->acquisitionTime[3] = header->time[3];
1507 header->acquisitionTime[4] = header->time[4];
1508 header->acquisitionTime[5] = header->time[5];
1504 header->acquisitionTime[BYTE_0] = header->time[BYTE_0];
1505 header->acquisitionTime[BYTE_1] = header->time[BYTE_1];
1506 header->acquisitionTime[BYTE_2] = header->time[BYTE_2];
1507 header->acquisitionTime[BYTE_3] = header->time[BYTE_3];
1508 header->acquisitionTime[BYTE_4] = header->time[BYTE_4];
1509 header->acquisitionTime[BYTE_5] = header->time[BYTE_5];
1509 1510
1510 1511 // (4) SEND PACKET
1511 1512 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1512 1513 if (status != RTEMS_SUCCESSFUL) {
1513 1514 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1514 1515 }
1515 1516 }
1516 1517 }
1517 1518
1518 1519 void spw_send_asm_f2( ring_node *ring_node_to_send,
1519 1520 Header_TM_LFR_SCIENCE_ASM_t *header )
1520 1521 {
1521 1522 unsigned int i;
1522 1523 unsigned int length = 0;
1523 1524 rtems_status_code status;
1524 1525 unsigned int sid;
1525 1526 float *spectral_matrix;
1526 1527 int coarseTime;
1527 1528 int fineTime;
1528 1529 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1529 1530
1530 1531 sid = ring_node_to_send->sid;
1531 1532 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1532 1533 coarseTime = ring_node_to_send->coarseTime;
1533 1534 fineTime = ring_node_to_send->fineTime;
1534 1535
1535 1536 header->pa_bia_status_info = pa_bia_status_info;
1536 1537 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1537 1538
1538 for (i=0; i<3; i++)
1539 for (i=0; i<PKTCNT_ASM; i++)
1539 1540 {
1540 1541
1541 1542 spw_ioctl_send_ASM.dlen = DLEN_ASM_F2_PKT;
1542 1543 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1543 1544 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM )
1544 1545 ];
1545 1546 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1546 1547 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1547 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB
1548 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> SHIFT_1_BYTE ); // BLK_NR MSB
1548 1549 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1549 1550
1550 1551 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1551 1552 spw_ioctl_send_ASM.hdr = (char *) header;
1552 1553 spw_ioctl_send_ASM.options = 0;
1553 1554
1554 1555 // (2) BUILD THE HEADER
1555 1556 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1556 header->packetLength[0] = (unsigned char) (length>>8);
1557 header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE);
1557 1558 header->packetLength[1] = (unsigned char) (length);
1558 1559 header->sid = (unsigned char) sid; // SID
1559 header->pa_lfr_pkt_cnt_asm = 3;
1560 header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM;
1560 1561 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1561 1562
1562 1563 // (3) SET PACKET TIME
1563 header->time[0] = (unsigned char) (coarseTime>>24);
1564 header->time[1] = (unsigned char) (coarseTime>>16);
1565 header->time[2] = (unsigned char) (coarseTime>>8);
1566 header->time[3] = (unsigned char) (coarseTime);
1567 header->time[4] = (unsigned char) (fineTime>>8);
1568 header->time[5] = (unsigned char) (fineTime);
1564 header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES);
1565 header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES);
1566 header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE);
1567 header->time[BYTE_3] = (unsigned char) (coarseTime);
1568 header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE);
1569 header->time[BYTE_5] = (unsigned char) (fineTime);
1569 1570 //
1570 header->acquisitionTime[0] = header->time[0];
1571 header->acquisitionTime[1] = header->time[1];
1572 header->acquisitionTime[2] = header->time[2];
1573 header->acquisitionTime[3] = header->time[3];
1574 header->acquisitionTime[4] = header->time[4];
1575 header->acquisitionTime[5] = header->time[5];
1571 header->acquisitionTime[BYTE_0] = header->time[BYTE_0];
1572 header->acquisitionTime[BYTE_1] = header->time[BYTE_1];
1573 header->acquisitionTime[BYTE_2] = header->time[BYTE_2];
1574 header->acquisitionTime[BYTE_3] = header->time[BYTE_3];
1575 header->acquisitionTime[BYTE_4] = header->time[BYTE_4];
1576 header->acquisitionTime[BYTE_5] = header->time[BYTE_5];
1576 1577
1577 1578 // (4) SEND PACKET
1578 1579 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1579 1580 if (status != RTEMS_SUCCESSFUL) {
1580 1581 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1581 1582 }
1582 1583 }
1583 1584 }
1584 1585
1585 1586 void spw_send_k_dump( ring_node *ring_node_to_send )
1586 1587 {
1587 1588 rtems_status_code status;
1588 1589 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump;
1589 1590 unsigned int packetLength;
1590 1591 unsigned int size;
1591 1592
1592 1593 PRINTF("spw_send_k_dump\n")
1593 1594
1594 1595 kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address;
1595 1596
1596 packetLength = kcoefficients_dump->packetLength[0] * 256 + kcoefficients_dump->packetLength[1];
1597 packetLength = (kcoefficients_dump->packetLength[0] * CONST_256) + kcoefficients_dump->packetLength[1];
1597 1598
1598 1599 size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
1599 1600
1600 1601 PRINTF2("packetLength %d, size %d\n", packetLength, size )
1601 1602
1602 1603 status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size );
1603 1604
1604 1605 if (status == -1){
1605 1606 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
1606 1607 }
1607 1608
1608 ring_node_to_send->status = 0x00;
1609 ring_node_to_send->status = INIT_CHAR;
1609 1610 }
Show file before | Show file after | Hide comments
@@ -1,115 +1,116
1 1 /*
2 2 * CPU Usage Reporter
3 3 *
4 4 * COPYRIGHT (c) 1989-2009
5 5 * On-Line Applications Research Corporation (OAR).
6 6 *
7 7 * The license and distribution terms for this file may be
8 8 * found in the file LICENSE in this distribution or at
9 9 * http://www.rtems.com/license/LICENSE.
10 10 *
11 11 * $Id$
12 12 */
13 13
14 14 #include "lfr_cpu_usage_report.h"
15 15
16 16 unsigned char lfr_rtems_cpu_usage_report( void )
17 17 {
18 18 uint32_t api_index;
19 19 Thread_Control *the_thread;
20 20 Objects_Information *information;
21 uint32_t ival, fval;
21 uint32_t ival;
22 uint32_t fval;
22 23 #ifndef __RTEMS_USE_TICKS_FOR_STATISTICS__
23 24 Timestamp_Control uptime;
24 25 Timestamp_Control total;
25 26 Timestamp_Control ran;
26 27 #else
27 28 uint32_t total_units = 0;
28 29 #endif
29 30
30 31 unsigned char cpu_load;
31 32 cpu_load = 0;
32 33
33 34 /*
34 35 * When not using nanosecond CPU usage resolution, we have to count
35 36 * the number of "ticks" we gave credit for to give the user a rough
36 37 * guideline as to what each number means proportionally.
37 38 */
38 39 #ifndef __RTEMS_USE_TICKS_FOR_STATISTICS__
39 40 _TOD_Get_uptime( &uptime );
40 41 _Timestamp_Subtract( &CPU_usage_Uptime_at_last_reset, &uptime, &total );
41 42 #else
42 43 for ( api_index = 1 ; api_index <= OBJECTS_APIS_LAST ; api_index++ ) {
43 44 if ( !_Objects_Information_table[ api_index ] ) { }
44 45 else
45 46 {
46 47 information = _Objects_Information_table[ api_index ][ 1 ];
47 48 if ( information != NULL )
48 49 {
49 50 for ( i=1 ; i <= information->maximum ; i++ ) {
50 51 the_thread = (Thread_Control *)information->local_table[ i ];
51 52
52 53 if ( the_thread != NULL ) {
53 54 total_units += the_thread->cpu_time_used; }
54 55 }
55 56 }
56 57 }
57 58 }
58 59 #endif
59 60
60 61 for ( api_index = 1 ; api_index <= OBJECTS_APIS_LAST ; api_index++ )
61 62 {
62 63 if ( !_Objects_Information_table[ api_index ] ) { }
63 64 else
64 65 {
65 66 information = _Objects_Information_table[ api_index ][ 1 ];
66 67 if ( information != NULL )
67 68 {
68 69 the_thread = (Thread_Control *)information->local_table[ 1 ];
69 70
70 71 if ( the_thread == NULL ) { }
71 72 else
72 73 {
73 74 #ifndef __RTEMS_USE_TICKS_FOR_STATISTICS__
74 75 /*
75 76 * If this is the currently executing thread, account for time
76 77 * since the last context switch.
77 78 */
78 79 ran = the_thread->cpu_time_used;
79 80 if ( _Thread_Executing->Object.id == the_thread->Object.id )
80 81 {
81 82 Timestamp_Control used;
82 83 _Timestamp_Subtract(
83 84 &_Thread_Time_of_last_context_switch, &uptime, &used
84 85 );
85 86 _Timestamp_Add_to( &ran, &used );
86 87 }
87 88 _Timestamp_Divide( &ran, &total, &ival, &fval );
88 89
89 90 #else
90 91 if (total_units != 0)
91 92 {
92 93 uint64_t ival_64;
93 94
94 95 ival_64 = the_thread->cpu_time_used;
95 ival_64 *= 100000;
96 ival_64 *= CONST_100000;
96 97 ival = ival_64 / total_units;
97 98 }
98 99 else
99 100 {
100 101 ival = 0;
101 102 }
102 103
103 fval = ival % 1000;
104 ival /= 1000;
104 fval = ival % CONST_1000;
105 ival /= CONST_1000;
105 106 #endif
106 107 }
107 108 }
108 109 }
109 110 }
110 cpu_load = (unsigned char) (100 - ival);
111 cpu_load = (unsigned char) (CONST_100 - ival);
111 112
112 113 return cpu_load;
113 114 }
114 115
115 116
Show file before | Show file after | Hide comments
@@ -1,414 +1,414
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 char asm_f0_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ];
28 27 float compressed_sm_norm_f0[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F0];
29 28 float compressed_sm_sbm_f0 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F0 ];
30 29
31 30 float k_coeff_intercalib_f0_norm[ NB_BINS_COMPRESSED_SM_F0 * NB_K_COEFF_PER_BIN ]; // 11 * 32 = 352
32 31 float k_coeff_intercalib_f0_sbm[ NB_BINS_COMPRESSED_SM_SBM_F0 * NB_K_COEFF_PER_BIN ]; // 22 * 32 = 704
33 32
34 33 //************
35 34 // RTEMS TASKS
36 35
37 36 rtems_task avf0_task( rtems_task_argument lfrRequestedMode )
38 37 {
39 38 int i;
40 39
41 40 rtems_event_set event_out;
42 41 rtems_status_code status;
43 42 rtems_id queue_id_prc0;
44 43 asm_msg msgForPRC;
45 44 ring_node *nodeForAveraging;
46 ring_node *ring_node_tab[8];
45 ring_node *ring_node_tab[NB_SM_BEFORE_AVF0_F1];
47 46 ring_node_asm *current_ring_node_asm_burst_sbm_f0;
48 47 ring_node_asm *current_ring_node_asm_norm_f0;
49 48
50 49 unsigned int nb_norm_bp1;
51 50 unsigned int nb_norm_bp2;
52 51 unsigned int nb_norm_asm;
53 52 unsigned int nb_sbm_bp1;
54 53 unsigned int nb_sbm_bp2;
55 54
56 55 nb_norm_bp1 = 0;
57 56 nb_norm_bp2 = 0;
58 57 nb_norm_asm = 0;
59 58 nb_sbm_bp1 = 0;
60 59 nb_sbm_bp2 = 0;
61 60
62 61 reset_nb_sm_f0( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
63 62 ASM_generic_init_ring( asm_ring_norm_f0, NB_RING_NODES_ASM_NORM_F0 );
64 63 ASM_generic_init_ring( asm_ring_burst_sbm_f0, NB_RING_NODES_ASM_BURST_SBM_F0 );
65 64 current_ring_node_asm_norm_f0 = asm_ring_norm_f0;
66 65 current_ring_node_asm_burst_sbm_f0 = asm_ring_burst_sbm_f0;
67 66
68 67 BOOT_PRINTF1("in AVFO *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
69 68
70 69 status = get_message_queue_id_prc0( &queue_id_prc0 );
71 70 if (status != RTEMS_SUCCESSFUL)
72 71 {
73 72 PRINTF1("in MATR *** ERR get_message_queue_id_prc0 %d\n", status)
74 73 }
75 74
76 75 while(1){
77 76 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
78 77
79 78 //****************************************
80 79 // initialize the mesage for the MATR task
81 80 msgForPRC.norm = current_ring_node_asm_norm_f0;
82 81 msgForPRC.burst_sbm = current_ring_node_asm_burst_sbm_f0;
83 msgForPRC.event = 0x00; // this composite event will be sent to the PRC0 task
82 msgForPRC.event = EVENT_SETS_NONE_PENDING; // this composite event will be sent to the PRC0 task
84 83 //
85 84 //****************************************
86 85
87 86 nodeForAveraging = getRingNodeForAveraging( 0 );
88 87
89 ring_node_tab[NB_SM_BEFORE_AVF0-1] = nodeForAveraging;
90 for ( i = 2; i < (NB_SM_BEFORE_AVF0+1); i++ )
88 ring_node_tab[NB_SM_BEFORE_AVF0_F1-1] = nodeForAveraging;
89 for ( i = 1; i < (NB_SM_BEFORE_AVF0_F1); i++ )
91 90 {
92 91 nodeForAveraging = nodeForAveraging->previous;
93 ring_node_tab[NB_SM_BEFORE_AVF0-i] = nodeForAveraging;
92 ring_node_tab[NB_SM_BEFORE_AVF0_F1-i] = nodeForAveraging;
94 93 }
95 94
96 95 // compute the average and store it in the averaged_sm_f1 buffer
97 96 SM_average( current_ring_node_asm_norm_f0->matrix,
98 97 current_ring_node_asm_burst_sbm_f0->matrix,
99 98 ring_node_tab,
100 99 nb_norm_bp1, nb_sbm_bp1,
101 100 &msgForPRC, 0 ); // 0 => frequency channel 0
102 101
103 102 // update nb_average
104 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0;
105 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0;
106 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0;
107 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0;
108 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0;
103 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0_F1;
104 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0_F1;
105 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0_F1;
106 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0_F1;
107 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0_F1;
109 108
110 109 if (nb_sbm_bp1 == nb_sm_before_f0.burst_sbm_bp1)
111 110 {
112 111 nb_sbm_bp1 = 0;
113 112 // set another ring for the ASM storage
114 113 current_ring_node_asm_burst_sbm_f0 = current_ring_node_asm_burst_sbm_f0->next;
115 114 if ( lfrCurrentMode == LFR_MODE_BURST )
116 115 {
117 116 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP1_F0;
118 117 }
119 118 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
120 119 {
121 120 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_SBM_BP1_F0;
122 121 }
123 122 }
124 123
125 124 if (nb_sbm_bp2 == nb_sm_before_f0.burst_sbm_bp2)
126 125 {
127 126 nb_sbm_bp2 = 0;
128 127 if ( lfrCurrentMode == LFR_MODE_BURST )
129 128 {
130 129 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP2_F0;
131 130 }
132 131 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
133 132 {
134 133 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_SBM_BP2_F0;
135 134 }
136 135 }
137 136
138 137 if (nb_norm_bp1 == nb_sm_before_f0.norm_bp1)
139 138 {
140 139 nb_norm_bp1 = 0;
141 140 // set another ring for the ASM storage
142 141 current_ring_node_asm_norm_f0 = current_ring_node_asm_norm_f0->next;
143 142 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
144 143 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
145 144 {
146 145 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP1_F0;
147 146 }
148 147 }
149 148
150 149 if (nb_norm_bp2 == nb_sm_before_f0.norm_bp2)
151 150 {
152 151 nb_norm_bp2 = 0;
153 152 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
154 153 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
155 154 {
156 155 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP2_F0;
157 156 }
158 157 }
159 158
160 159 if (nb_norm_asm == nb_sm_before_f0.norm_asm)
161 160 {
162 161 nb_norm_asm = 0;
163 162 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
164 163 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
165 164 {
166 165 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_ASM_F0;
167 166 }
168 167 }
169 168
170 169 //*************************
171 170 // send the message to PRC
172 if (msgForPRC.event != 0x00)
171 if (msgForPRC.event != EVENT_SETS_NONE_PENDING)
173 172 {
174 173 status = rtems_message_queue_send( queue_id_prc0, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC0);
175 174 }
176 175
177 176 if (status != RTEMS_SUCCESSFUL) {
178 177 PRINTF1("in AVF0 *** Error sending message to PRC, code %d\n", status)
179 178 }
180 179 }
181 180 }
182 181
183 182 rtems_task prc0_task( rtems_task_argument lfrRequestedMode )
184 183 {
185 184 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
186 185 size_t size; // size of the incoming TC packet
187 186 asm_msg *incomingMsg;
188 187 //
189 188 unsigned char sid;
190 189 rtems_status_code status;
191 190 rtems_id queue_id;
192 191 rtems_id queue_id_q_p0;
193 192 bp_packet_with_spare packet_norm_bp1;
194 193 bp_packet packet_norm_bp2;
195 194 bp_packet packet_sbm_bp1;
196 195 bp_packet packet_sbm_bp2;
197 196 ring_node *current_ring_node_to_send_asm_f0;
198 197 float nbSMInASMNORM;
199 198 float nbSMInASMSBM;
200 199
201 200 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
202 201 init_ring( ring_to_send_asm_f0, NB_RING_NODES_ASM_F0, (volatile int*) buffer_asm_f0, TOTAL_SIZE_SM );
203 202 current_ring_node_to_send_asm_f0 = ring_to_send_asm_f0;
204 203
205 204 //*************
206 205 // NORM headers
207 206 BP_init_header_with_spare( &packet_norm_bp1,
208 207 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F0,
209 208 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0, NB_BINS_COMPRESSED_SM_F0 );
210 209 BP_init_header( &packet_norm_bp2,
211 210 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F0,
212 211 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0, NB_BINS_COMPRESSED_SM_F0);
213 212
214 213 //****************************
215 214 // BURST SBM1 and SBM2 headers
216 215 if ( lfrRequestedMode == LFR_MODE_BURST )
217 216 {
218 217 BP_init_header( &packet_sbm_bp1,
219 218 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F0,
220 219 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
221 220 BP_init_header( &packet_sbm_bp2,
222 221 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F0,
223 222 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
224 223 }
225 224 else if ( lfrRequestedMode == LFR_MODE_SBM1 )
226 225 {
227 226 BP_init_header( &packet_sbm_bp1,
228 227 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP1_F0,
229 228 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
230 229 BP_init_header( &packet_sbm_bp2,
231 230 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP2_F0,
232 231 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
233 232 }
234 233 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
235 234 {
236 235 BP_init_header( &packet_sbm_bp1,
237 236 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F0,
238 237 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
239 238 BP_init_header( &packet_sbm_bp2,
240 239 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F0,
241 240 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
242 241 }
243 242 else
244 243 {
245 244 PRINTF1("in PRC0 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
246 245 }
247 246
248 247 status = get_message_queue_id_send( &queue_id );
249 248 if (status != RTEMS_SUCCESSFUL)
250 249 {
251 250 PRINTF1("in PRC0 *** ERR get_message_queue_id_send %d\n", status)
252 251 }
253 252 status = get_message_queue_id_prc0( &queue_id_q_p0);
254 253 if (status != RTEMS_SUCCESSFUL)
255 254 {
256 255 PRINTF1("in PRC0 *** ERR get_message_queue_id_prc0 %d\n", status)
257 256 }
258 257
259 258 BOOT_PRINTF1("in PRC0 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
260 259
261 260 while(1){
262 261 status = rtems_message_queue_receive( queue_id_q_p0, incomingData, &size, //************************************
263 262 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
264 263
265 264 incomingMsg = (asm_msg*) incomingData;
266 265
267 266 ASM_patch( incomingMsg->norm->matrix, asm_f0_patched_norm );
268 267 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f0_patched_burst_sbm );
269 268
270 269 nbSMInASMNORM = incomingMsg->numberOfSMInASMNORM;
271 270 nbSMInASMSBM = incomingMsg->numberOfSMInASMSBM;
272 271
273 272 //****************
274 273 //****************
275 274 // BURST SBM1 SBM2
276 275 //****************
277 276 //****************
278 277 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F0 ) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F0 ) )
279 278 {
280 279 sid = getSID( incomingMsg->event );
281 280 // 1) compress the matrix for Basic Parameters calculation
282 281 ASM_compress_reorganize_and_divide_mask( asm_f0_patched_burst_sbm, compressed_sm_sbm_f0,
283 282 nbSMInASMSBM,
284 283 NB_BINS_COMPRESSED_SM_SBM_F0, NB_BINS_TO_AVERAGE_ASM_SBM_F0,
285 284 ASM_F0_INDICE_START, CHANNELF0);
286 285 // 2) compute the BP1 set
287 286 BP1_set( compressed_sm_sbm_f0, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp1.data );
288 287 // 3) send the BP1 set
289 288 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
290 289 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
291 290 packet_sbm_bp1.pa_bia_status_info = pa_bia_status_info;
292 291 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
293 292 BP_send_s1_s2( (char *) &packet_sbm_bp1, queue_id,
294 293 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0 + PACKET_LENGTH_DELTA,
295 294 sid);
296 295 // 4) compute the BP2 set if needed
297 296 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F0) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F0) )
298 297 {
299 298 // 1) compute the BP2 set
300 299 BP2_set( compressed_sm_sbm_f0, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp2.data );
301 300 // 2) send the BP2 set
302 301 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
303 302 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
304 303 packet_sbm_bp2.pa_bia_status_info = pa_bia_status_info;
305 304 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
306 305 BP_send_s1_s2( (char *) &packet_sbm_bp2, queue_id,
307 306 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0 + PACKET_LENGTH_DELTA,
308 307 sid);
309 308 }
310 309 }
311 310
312 311 //*****
313 312 //*****
314 313 // NORM
315 314 //*****
316 315 //*****
317 316 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F0)
318 317 {
319 318 // 1) compress the matrix for Basic Parameters calculation
320 319 ASM_compress_reorganize_and_divide_mask( asm_f0_patched_norm, compressed_sm_norm_f0,
321 320 nbSMInASMNORM,
322 321 NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0,
323 322 ASM_F0_INDICE_START, CHANNELF0 );
324 323 // 2) compute the BP1 set
325 324 BP1_set( compressed_sm_norm_f0, k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp1.data );
326 325 // 3) send the BP1 set
327 326 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
328 327 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
329 328 packet_norm_bp1.pa_bia_status_info = pa_bia_status_info;
330 329 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
331 330 BP_send( (char *) &packet_norm_bp1, queue_id,
332 331 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0 + PACKET_LENGTH_DELTA,
333 332 SID_NORM_BP1_F0 );
334 333 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F0)
335 334 {
336 335 // 1) compute the BP2 set using the same ASM as the one used for BP1
337 336 BP2_set( compressed_sm_norm_f0, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp2.data );
338 337 // 2) send the BP2 set
339 338 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
340 339 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
341 340 packet_norm_bp2.pa_bia_status_info = pa_bia_status_info;
342 341 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
343 342 BP_send( (char *) &packet_norm_bp2, queue_id,
344 343 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0 + PACKET_LENGTH_DELTA,
345 344 SID_NORM_BP2_F0);
346 345 }
347 346 }
348 347
349 348 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F0)
350 349 {
351 350 // 1) reorganize the ASM and divide
352 351 ASM_reorganize_and_divide( asm_f0_patched_norm,
353 352 (float*) current_ring_node_to_send_asm_f0->buffer_address,
354 353 nbSMInASMNORM );
355 354 current_ring_node_to_send_asm_f0->coarseTime = incomingMsg->coarseTimeNORM;
356 355 current_ring_node_to_send_asm_f0->fineTime = incomingMsg->fineTimeNORM;
357 356 current_ring_node_to_send_asm_f0->sid = SID_NORM_ASM_F0;
358 357
359 358 // 3) send the spectral matrix packets
360 359 status = rtems_message_queue_send( queue_id, &current_ring_node_to_send_asm_f0, sizeof( ring_node* ) );
361 360
362 361 // change asm ring node
363 362 current_ring_node_to_send_asm_f0 = current_ring_node_to_send_asm_f0->next;
364 363 }
365 364
366 365 update_queue_max_count( queue_id_q_p0, &hk_lfr_q_p0_fifo_size_max );
367 366
368 367 }
369 368 }
370 369
371 370 //**********
372 371 // FUNCTIONS
373 372
374 373 void reset_nb_sm_f0( unsigned char lfrMode )
375 374 {
376 nb_sm_before_f0.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 96;
377 nb_sm_before_f0.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 96;
378 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;
379 nb_sm_before_f0.sbm1_bp1 = parameter_dump_packet.sy_lfr_s1_bp_p0 * 24; // 0.25 s per digit
380 nb_sm_before_f0.sbm1_bp2 = parameter_dump_packet.sy_lfr_s1_bp_p1 * 96;
381 nb_sm_before_f0.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 96;
382 nb_sm_before_f0.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 96;
383 nb_sm_before_f0.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 96;
384 nb_sm_before_f0.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 96;
375 nb_sm_before_f0.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * NB_SM_PER_S_F0;
376 nb_sm_before_f0.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * NB_SM_PER_S_F0;
377 nb_sm_before_f0.norm_asm =
378 ( (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256) + parameter_dump_packet.sy_lfr_n_asm_p[1]) * NB_SM_PER_S_F0;
379 nb_sm_before_f0.sbm1_bp1 = parameter_dump_packet.sy_lfr_s1_bp_p0 * NB_SM_PER_S1_BP_P0; // 0.25 s per digit
380 nb_sm_before_f0.sbm1_bp2 = parameter_dump_packet.sy_lfr_s1_bp_p1 * NB_SM_PER_S_F0;
381 nb_sm_before_f0.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * NB_SM_PER_S_F0;
382 nb_sm_before_f0.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * NB_SM_PER_S_F0;
383 nb_sm_before_f0.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * NB_SM_PER_S_F0;
384 nb_sm_before_f0.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * NB_SM_PER_S_F0;
385 385
386 386 if (lfrMode == LFR_MODE_SBM1)
387 387 {
388 388 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm1_bp1;
389 389 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm1_bp2;
390 390 }
391 391 else if (lfrMode == LFR_MODE_SBM2)
392 392 {
393 393 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm2_bp1;
394 394 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm2_bp2;
395 395 }
396 396 else if (lfrMode == LFR_MODE_BURST)
397 397 {
398 398 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
399 399 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
400 400 }
401 401 else
402 402 {
403 403 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
404 404 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
405 405 }
406 406 }
407 407
408 408 void init_k_coefficients_prc0( void )
409 409 {
410 410 init_k_coefficients( k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0 );
411 411
412 412 init_kcoeff_sbm_from_kcoeff_norm( k_coeff_intercalib_f0_norm, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_F0);
413 413 }
414 414
Show file before | Show file after | Hide comments
@@ -1,398 +1,398
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 char asm_f1_char [ TOTAL_SIZE_SM * 2 ];
29 28 float compressed_sm_norm_f1[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F1];
30 29 float compressed_sm_sbm_f1 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 ];
31 30
32 31 float k_coeff_intercalib_f1_norm[ NB_BINS_COMPRESSED_SM_F1 * NB_K_COEFF_PER_BIN ]; // 13 * 32 = 416
33 32 float k_coeff_intercalib_f1_sbm[ NB_BINS_COMPRESSED_SM_SBM_F1 * NB_K_COEFF_PER_BIN ]; // 26 * 32 = 832
34 33
35 34 //************
36 35 // RTEMS TASKS
37 36
38 37 rtems_task avf1_task( rtems_task_argument lfrRequestedMode )
39 38 {
40 39 int i;
41 40
42 41 rtems_event_set event_out;
43 42 rtems_status_code status;
44 43 rtems_id queue_id_prc1;
45 44 asm_msg msgForPRC;
46 45 ring_node *nodeForAveraging;
47 ring_node *ring_node_tab[NB_SM_BEFORE_AVF0];
46 ring_node *ring_node_tab[NB_SM_BEFORE_AVF0_F1];
48 47 ring_node_asm *current_ring_node_asm_burst_sbm_f1;
49 48 ring_node_asm *current_ring_node_asm_norm_f1;
50 49
51 50 unsigned int nb_norm_bp1;
52 51 unsigned int nb_norm_bp2;
53 52 unsigned int nb_norm_asm;
54 53 unsigned int nb_sbm_bp1;
55 54 unsigned int nb_sbm_bp2;
56 55
57 56 nb_norm_bp1 = 0;
58 57 nb_norm_bp2 = 0;
59 58 nb_norm_asm = 0;
60 59 nb_sbm_bp1 = 0;
61 60 nb_sbm_bp2 = 0;
62 61
63 62 reset_nb_sm_f1( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
64 63 ASM_generic_init_ring( asm_ring_norm_f1, NB_RING_NODES_ASM_NORM_F1 );
65 64 ASM_generic_init_ring( asm_ring_burst_sbm_f1, NB_RING_NODES_ASM_BURST_SBM_F1 );
66 65 current_ring_node_asm_norm_f1 = asm_ring_norm_f1;
67 66 current_ring_node_asm_burst_sbm_f1 = asm_ring_burst_sbm_f1;
68 67
69 68 BOOT_PRINTF1("in AVF1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
70 69
71 70 status = get_message_queue_id_prc1( &queue_id_prc1 );
72 71 if (status != RTEMS_SUCCESSFUL)
73 72 {
74 73 PRINTF1("in AVF1 *** ERR get_message_queue_id_prc1 %d\n", status)
75 74 }
76 75
77 76 while(1){
78 77 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
79 78
80 79 //****************************************
81 80 // initialize the mesage for the MATR task
82 81 msgForPRC.norm = current_ring_node_asm_norm_f1;
83 82 msgForPRC.burst_sbm = current_ring_node_asm_burst_sbm_f1;
84 msgForPRC.event = 0x00; // this composite event will be sent to the PRC1 task
83 msgForPRC.event = EVENT_SETS_NONE_PENDING; // this composite event will be sent to the PRC1 task
85 84 //
86 85 //****************************************
87 86
88 87 nodeForAveraging = getRingNodeForAveraging( 1 );
89 88
90 ring_node_tab[NB_SM_BEFORE_AVF1-1] = nodeForAveraging;
91 for ( i = 2; i < (NB_SM_BEFORE_AVF1+1); i++ )
89 ring_node_tab[NB_SM_BEFORE_AVF0_F1-1] = nodeForAveraging;
90 for ( i = 1; i < (NB_SM_BEFORE_AVF0_F1); i++ )
92 91 {
93 92 nodeForAveraging = nodeForAveraging->previous;
94 ring_node_tab[NB_SM_BEFORE_AVF1-i] = nodeForAveraging;
93 ring_node_tab[NB_SM_BEFORE_AVF0_F1-i] = nodeForAveraging;
95 94 }
96 95
97 96 // compute the average and store it in the averaged_sm_f1 buffer
98 97 SM_average( current_ring_node_asm_norm_f1->matrix,
99 98 current_ring_node_asm_burst_sbm_f1->matrix,
100 99 ring_node_tab,
101 100 nb_norm_bp1, nb_sbm_bp1,
102 101 &msgForPRC, 1 ); // 1 => frequency channel 1
103 102
104 103 // update nb_average
105 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF1;
106 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF1;
107 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF1;
108 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF1;
109 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF1;
104 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0_F1;
105 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0_F1;
106 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0_F1;
107 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0_F1;
108 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0_F1;
110 109
111 110 if (nb_sbm_bp1 == nb_sm_before_f1.burst_sbm_bp1)
112 111 {
113 112 nb_sbm_bp1 = 0;
114 113 // set another ring for the ASM storage
115 114 current_ring_node_asm_burst_sbm_f1 = current_ring_node_asm_burst_sbm_f1->next;
116 115 if ( lfrCurrentMode == LFR_MODE_BURST )
117 116 {
118 117 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP1_F1;
119 118 }
120 119 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
121 120 {
122 121 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_SBM_BP1_F1;
123 122 }
124 123 }
125 124
126 125 if (nb_sbm_bp2 == nb_sm_before_f1.burst_sbm_bp2)
127 126 {
128 127 nb_sbm_bp2 = 0;
129 128 if ( lfrCurrentMode == LFR_MODE_BURST )
130 129 {
131 130 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP2_F1;
132 131 }
133 132 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
134 133 {
135 134 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_SBM_BP2_F1;
136 135 }
137 136 }
138 137
139 138 if (nb_norm_bp1 == nb_sm_before_f1.norm_bp1)
140 139 {
141 140 nb_norm_bp1 = 0;
142 141 // set another ring for the ASM storage
143 142 current_ring_node_asm_norm_f1 = current_ring_node_asm_norm_f1->next;
144 143 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
145 144 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
146 145 {
147 146 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP1_F1;
148 147 }
149 148 }
150 149
151 150 if (nb_norm_bp2 == nb_sm_before_f1.norm_bp2)
152 151 {
153 152 nb_norm_bp2 = 0;
154 153 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
155 154 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
156 155 {
157 156 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP2_F1;
158 157 }
159 158 }
160 159
161 160 if (nb_norm_asm == nb_sm_before_f1.norm_asm)
162 161 {
163 162 nb_norm_asm = 0;
164 163 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
165 164 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
166 165 {
167 166 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_ASM_F1;
168 167 }
169 168 }
170 169
171 170 //*************************
172 171 // send the message to PRC
173 if (msgForPRC.event != 0x00)
172 if (msgForPRC.event != EVENT_SETS_NONE_PENDING)
174 173 {
175 174 status = rtems_message_queue_send( queue_id_prc1, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC1);
176 175 }
177 176
178 177 if (status != RTEMS_SUCCESSFUL) {
179 178 PRINTF1("in AVF1 *** Error sending message to PRC1, code %d\n", status)
180 179 }
181 180 }
182 181 }
183 182
184 183 rtems_task prc1_task( rtems_task_argument lfrRequestedMode )
185 184 {
186 185 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
187 186 size_t size; // size of the incoming TC packet
188 187 asm_msg *incomingMsg;
189 188 //
190 189 unsigned char sid;
191 190 rtems_status_code status;
192 191 rtems_id queue_id_send;
193 192 rtems_id queue_id_q_p1;
194 193 bp_packet_with_spare packet_norm_bp1;
195 194 bp_packet packet_norm_bp2;
196 195 bp_packet packet_sbm_bp1;
197 196 bp_packet packet_sbm_bp2;
198 197 ring_node *current_ring_node_to_send_asm_f1;
199 198 float nbSMInASMNORM;
200 199 float nbSMInASMSBM;
201 200
202 201 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
203 202 init_ring( ring_to_send_asm_f1, NB_RING_NODES_ASM_F1, (volatile int*) buffer_asm_f1, TOTAL_SIZE_SM );
204 203 current_ring_node_to_send_asm_f1 = ring_to_send_asm_f1;
205 204
206 205 //*************
207 206 // NORM headers
208 207 BP_init_header_with_spare( &packet_norm_bp1,
209 208 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F1,
210 209 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1, NB_BINS_COMPRESSED_SM_F1 );
211 210 BP_init_header( &packet_norm_bp2,
212 211 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F1,
213 212 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1, NB_BINS_COMPRESSED_SM_F1);
214 213
215 214 //***********************
216 215 // BURST and SBM2 headers
217 216 if ( lfrRequestedMode == LFR_MODE_BURST )
218 217 {
219 218 BP_init_header( &packet_sbm_bp1,
220 219 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F1,
221 220 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
222 221 BP_init_header( &packet_sbm_bp2,
223 222 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F1,
224 223 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
225 224 }
226 225 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
227 226 {
228 227 BP_init_header( &packet_sbm_bp1,
229 228 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F1,
230 229 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
231 230 BP_init_header( &packet_sbm_bp2,
232 231 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F1,
233 232 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
234 233 }
235 234 else
236 235 {
237 236 PRINTF1("in PRC1 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
238 237 }
239 238
240 239 status = get_message_queue_id_send( &queue_id_send );
241 240 if (status != RTEMS_SUCCESSFUL)
242 241 {
243 242 PRINTF1("in PRC1 *** ERR get_message_queue_id_send %d\n", status)
244 243 }
245 244 status = get_message_queue_id_prc1( &queue_id_q_p1);
246 245 if (status != RTEMS_SUCCESSFUL)
247 246 {
248 247 PRINTF1("in PRC1 *** ERR get_message_queue_id_prc1 %d\n", status)
249 248 }
250 249
251 250 BOOT_PRINTF1("in PRC1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
252 251
253 252 while(1){
254 253 status = rtems_message_queue_receive( queue_id_q_p1, incomingData, &size, //************************************
255 254 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
256 255
257 256 incomingMsg = (asm_msg*) incomingData;
258 257
259 258 ASM_patch( incomingMsg->norm->matrix, asm_f1_patched_norm );
260 259 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f1_patched_burst_sbm );
261 260
262 261 nbSMInASMNORM = incomingMsg->numberOfSMInASMNORM;
263 262 nbSMInASMSBM = incomingMsg->numberOfSMInASMSBM;
264 263
265 264 //***********
266 265 //***********
267 266 // BURST SBM2
268 267 //***********
269 268 //***********
270 269 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F1) )
271 270 {
272 271 sid = getSID( incomingMsg->event );
273 272 // 1) compress the matrix for Basic Parameters calculation
274 273 ASM_compress_reorganize_and_divide_mask( asm_f1_patched_burst_sbm, compressed_sm_sbm_f1,
275 274 nbSMInASMSBM,
276 275 NB_BINS_COMPRESSED_SM_SBM_F1, NB_BINS_TO_AVERAGE_ASM_SBM_F1,
277 276 ASM_F1_INDICE_START, CHANNELF1);
278 277 // 2) compute the BP1 set
279 278 BP1_set( compressed_sm_sbm_f1, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp1.data );
280 279 // 3) send the BP1 set
281 280 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
282 281 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
283 282 packet_sbm_bp1.pa_bia_status_info = pa_bia_status_info;
284 283 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
285 284 BP_send_s1_s2( (char *) &packet_sbm_bp1, queue_id_send,
286 285 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 + PACKET_LENGTH_DELTA,
287 286 sid );
288 287 // 4) compute the BP2 set if needed
289 288 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F1) )
290 289 {
291 290 // 1) compute the BP2 set
292 291 BP2_set( compressed_sm_sbm_f1, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp2.data );
293 292 // 2) send the BP2 set
294 293 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
295 294 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
296 295 packet_sbm_bp2.pa_bia_status_info = pa_bia_status_info;
297 296 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
298 297 BP_send_s1_s2( (char *) &packet_sbm_bp2, queue_id_send,
299 298 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1 + PACKET_LENGTH_DELTA,
300 299 sid );
301 300 }
302 301 }
303 302
304 303 //*****
305 304 //*****
306 305 // NORM
307 306 //*****
308 307 //*****
309 308 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F1)
310 309 {
311 310 // 1) compress the matrix for Basic Parameters calculation
312 311 ASM_compress_reorganize_and_divide_mask( asm_f1_patched_norm, compressed_sm_norm_f1,
313 312 nbSMInASMNORM,
314 313 NB_BINS_COMPRESSED_SM_F1, NB_BINS_TO_AVERAGE_ASM_F1,
315 314 ASM_F1_INDICE_START, CHANNELF1 );
316 315 // 2) compute the BP1 set
317 316 BP1_set( compressed_sm_norm_f1, k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp1.data );
318 317 // 3) send the BP1 set
319 318 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
320 319 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
321 320 packet_norm_bp1.pa_bia_status_info = pa_bia_status_info;
322 321 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
323 322 BP_send( (char *) &packet_norm_bp1, queue_id_send,
324 323 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 + PACKET_LENGTH_DELTA,
325 324 SID_NORM_BP1_F1 );
326 325 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F1)
327 326 {
328 327 // 1) compute the BP2 set
329 328 BP2_set( compressed_sm_norm_f1, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp2.data );
330 329 // 2) send the BP2 set
331 330 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
332 331 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
333 332 packet_norm_bp2.pa_bia_status_info = pa_bia_status_info;
334 333 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
335 334 BP_send( (char *) &packet_norm_bp2, queue_id_send,
336 335 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 + PACKET_LENGTH_DELTA,
337 336 SID_NORM_BP2_F1 );
338 337 }
339 338 }
340 339
341 340 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F1)
342 341 {
343 342 // 1) reorganize the ASM and divide
344 343 ASM_reorganize_and_divide( asm_f1_patched_norm,
345 344 (float*) current_ring_node_to_send_asm_f1->buffer_address,
346 345 nbSMInASMNORM );
347 346 current_ring_node_to_send_asm_f1->coarseTime = incomingMsg->coarseTimeNORM;
348 347 current_ring_node_to_send_asm_f1->fineTime = incomingMsg->fineTimeNORM;
349 348 current_ring_node_to_send_asm_f1->sid = SID_NORM_ASM_F1;
350 349
351 350 // 3) send the spectral matrix packets
352 351 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f1, sizeof( ring_node* ) );
353 352
354 353 // change asm ring node
355 354 current_ring_node_to_send_asm_f1 = current_ring_node_to_send_asm_f1->next;
356 355 }
357 356
358 357 update_queue_max_count( queue_id_q_p1, &hk_lfr_q_p1_fifo_size_max );
359 358
360 359 }
361 360 }
362 361
363 362 //**********
364 363 // FUNCTIONS
365 364
366 365 void reset_nb_sm_f1( unsigned char lfrMode )
367 366 {
368 nb_sm_before_f1.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 16;
369 nb_sm_before_f1.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 16;
370 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;
371 nb_sm_before_f1.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 16;
372 nb_sm_before_f1.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 16;
373 nb_sm_before_f1.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 16;
374 nb_sm_before_f1.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 16;
367 nb_sm_before_f1.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * NB_SM_PER_S_F1;
368 nb_sm_before_f1.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * NB_SM_PER_S_F1;
369 nb_sm_before_f1.norm_asm =
370 ( (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256) + parameter_dump_packet.sy_lfr_n_asm_p[1]) * NB_SM_PER_S_F1;
371 nb_sm_before_f1.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * NB_SM_PER_S_F1;
372 nb_sm_before_f1.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * NB_SM_PER_S_F1;
373 nb_sm_before_f1.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * NB_SM_PER_S_F1;
374 nb_sm_before_f1.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * NB_SM_PER_S_F1;
375 375
376 376 if (lfrMode == LFR_MODE_SBM2)
377 377 {
378 378 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.sbm2_bp1;
379 379 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.sbm2_bp2;
380 380 }
381 381 else if (lfrMode == LFR_MODE_BURST)
382 382 {
383 383 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
384 384 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
385 385 }
386 386 else
387 387 {
388 388 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
389 389 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
390 390 }
391 391 }
392 392
393 393 void init_k_coefficients_prc1( void )
394 394 {
395 395 init_k_coefficients( k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1 );
396 396
397 397 init_kcoeff_sbm_from_kcoeff_norm( k_coeff_intercalib_f1_norm, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_F1);
398 398 }
Show file before | Show file after | Hide comments
@@ -1,327 +1,326
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 char asm_f2_char [ TOTAL_SIZE_SM * 2 ];
27 26 float compressed_sm_norm_f2[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F2];
28 27
29 28 float k_coeff_intercalib_f2[ NB_BINS_COMPRESSED_SM_F2 * NB_K_COEFF_PER_BIN ]; // 12 * 32 = 384
30 29
31 30 //************
32 31 // RTEMS TASKS
33 32
34 33 //***
35 34 // F2
36 35 rtems_task avf2_task( rtems_task_argument argument )
37 36 {
38 37 rtems_event_set event_out;
39 38 rtems_status_code status;
40 39 rtems_id queue_id_prc2;
41 40 asm_msg msgForPRC;
42 41 ring_node *nodeForAveraging;
43 42 ring_node_asm *current_ring_node_asm_norm_f2;
44 43
45 44 unsigned int nb_norm_bp1;
46 45 unsigned int nb_norm_bp2;
47 46 unsigned int nb_norm_asm;
48 47
49 48 nb_norm_bp1 = 0;
50 49 nb_norm_bp2 = 0;
51 50 nb_norm_asm = 0;
52 51
53 52 reset_nb_sm_f2( ); // reset the sm counters that drive the BP and ASM computations / transmissions
54 53 ASM_generic_init_ring( asm_ring_norm_f2, NB_RING_NODES_ASM_NORM_F2 );
55 54 current_ring_node_asm_norm_f2 = asm_ring_norm_f2;
56 55
57 56 BOOT_PRINTF("in AVF2 ***\n")
58 57
59 58 status = get_message_queue_id_prc2( &queue_id_prc2 );
60 59 if (status != RTEMS_SUCCESSFUL)
61 60 {
62 61 PRINTF1("in AVF2 *** ERR get_message_queue_id_prc2 %d\n", status)
63 62 }
64 63
65 64 while(1){
66 65 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
67 66
68 67 //****************************************
69 68 // initialize the mesage for the MATR task
70 69 msgForPRC.norm = current_ring_node_asm_norm_f2;
71 70 msgForPRC.burst_sbm = NULL;
72 msgForPRC.event = 0x00; // this composite event will be sent to the PRC2 task
71 msgForPRC.event = EVENT_SETS_NONE_PENDING; // this composite event will be sent to the PRC2 task
73 72 //
74 73 //****************************************
75 74
76 nodeForAveraging = getRingNodeForAveraging( 2 );
75 nodeForAveraging = getRingNodeForAveraging( CHANNELF2 );
77 76
78 77 // compute the average and store it in the averaged_sm_f2 buffer
79 78 SM_average_f2( current_ring_node_asm_norm_f2->matrix,
80 79 nodeForAveraging,
81 80 nb_norm_bp1,
82 81 &msgForPRC );
83 82
84 83 // update nb_average
85 84 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF2;
86 85 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF2;
87 86 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF2;
88 87
89 88 if (nb_norm_bp1 == nb_sm_before_f2.norm_bp1)
90 89 {
91 90 nb_norm_bp1 = 0;
92 91 // set another ring for the ASM storage
93 92 current_ring_node_asm_norm_f2 = current_ring_node_asm_norm_f2->next;
94 93 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
95 94 || (lfrCurrentMode == LFR_MODE_SBM2) )
96 95 {
97 96 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP1_F2;
98 97 }
99 98 }
100 99
101 100 if (nb_norm_bp2 == nb_sm_before_f2.norm_bp2)
102 101 {
103 102 nb_norm_bp2 = 0;
104 103 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
105 104 || (lfrCurrentMode == LFR_MODE_SBM2) )
106 105 {
107 106 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP2_F2;
108 107 }
109 108 }
110 109
111 110 if (nb_norm_asm == nb_sm_before_f2.norm_asm)
112 111 {
113 112 nb_norm_asm = 0;
114 113 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
115 114 || (lfrCurrentMode == LFR_MODE_SBM2) )
116 115 {
117 116 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_ASM_F2;
118 117 }
119 118 }
120 119
121 120 //*************************
122 121 // send the message to PRC2
123 if (msgForPRC.event != 0x00)
122 if (msgForPRC.event != EVENT_SETS_NONE_PENDING)
124 123 {
125 124 status = rtems_message_queue_send( queue_id_prc2, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC2);
126 125 }
127 126
128 127 if (status != RTEMS_SUCCESSFUL) {
129 128 PRINTF1("in AVF2 *** Error sending message to PRC2, code %d\n", status)
130 129 }
131 130 }
132 131 }
133 132
134 133 rtems_task prc2_task( rtems_task_argument argument )
135 134 {
136 135 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
137 136 size_t size; // size of the incoming TC packet
138 137 asm_msg *incomingMsg;
139 138 //
140 139 rtems_status_code status;
141 140 rtems_id queue_id_send;
142 141 rtems_id queue_id_q_p2;
143 142 bp_packet packet_norm_bp1;
144 143 bp_packet packet_norm_bp2;
145 144 ring_node *current_ring_node_to_send_asm_f2;
146 145 float nbSMInASMNORM;
147 146
148 147 unsigned long long int localTime;
149 148
150 149 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
151 150 init_ring( ring_to_send_asm_f2, NB_RING_NODES_ASM_F2, (volatile int*) buffer_asm_f2, TOTAL_SIZE_SM );
152 151 current_ring_node_to_send_asm_f2 = ring_to_send_asm_f2;
153 152
154 153 //*************
155 154 // NORM headers
156 155 BP_init_header( &packet_norm_bp1,
157 156 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F2,
158 157 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2, NB_BINS_COMPRESSED_SM_F2 );
159 158 BP_init_header( &packet_norm_bp2,
160 159 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F2,
161 160 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2, NB_BINS_COMPRESSED_SM_F2 );
162 161
163 162 status = get_message_queue_id_send( &queue_id_send );
164 163 if (status != RTEMS_SUCCESSFUL)
165 164 {
166 165 PRINTF1("in PRC2 *** ERR get_message_queue_id_send %d\n", status)
167 166 }
168 167 status = get_message_queue_id_prc2( &queue_id_q_p2);
169 168 if (status != RTEMS_SUCCESSFUL)
170 169 {
171 170 PRINTF1("in PRC2 *** ERR get_message_queue_id_prc2 %d\n", status)
172 171 }
173 172
174 173 BOOT_PRINTF("in PRC2 ***\n")
175 174
176 175 while(1){
177 176 status = rtems_message_queue_receive( queue_id_q_p2, incomingData, &size, //************************************
178 177 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF2
179 178
180 179 incomingMsg = (asm_msg*) incomingData;
181 180
182 181 ASM_patch( incomingMsg->norm->matrix, asm_f2_patched_norm );
183 182
184 183 localTime = getTimeAsUnsignedLongLongInt( );
185 184
186 185 nbSMInASMNORM = incomingMsg->numberOfSMInASMNORM;
187 186
188 187 //*****
189 188 //*****
190 189 // NORM
191 190 //*****
192 191 //*****
193 192 // 1) compress the matrix for Basic Parameters calculation
194 193 ASM_compress_reorganize_and_divide_mask( asm_f2_patched_norm, compressed_sm_norm_f2,
195 194 nbSMInASMNORM,
196 195 NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2,
197 196 ASM_F2_INDICE_START, CHANNELF2 );
198 197 // BP1_F2
199 198 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F2)
200 199 {
201 200 // 1) compute the BP1 set
202 201 BP1_set( compressed_sm_norm_f2, k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp1.data );
203 202 // 2) send the BP1 set
204 203 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
205 204 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
206 205 packet_norm_bp1.pa_bia_status_info = pa_bia_status_info;
207 206 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
208 207 BP_send( (char *) &packet_norm_bp1, queue_id_send,
209 208 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2 + PACKET_LENGTH_DELTA,
210 209 SID_NORM_BP1_F2 );
211 210 }
212 211 // BP2_F2
213 212 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F2)
214 213 {
215 214 // 1) compute the BP2 set
216 215 BP2_set( compressed_sm_norm_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp2.data );
217 216 // 2) send the BP2 set
218 217 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
219 218 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
220 219 packet_norm_bp2.pa_bia_status_info = pa_bia_status_info;
221 220 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
222 221 BP_send( (char *) &packet_norm_bp2, queue_id_send,
223 222 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2 + PACKET_LENGTH_DELTA,
224 223 SID_NORM_BP2_F2 );
225 224 }
226 225
227 226 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F2)
228 227 {
229 228 // 1) reorganize the ASM and divide
230 229 ASM_reorganize_and_divide( asm_f2_patched_norm,
231 230 (float*) current_ring_node_to_send_asm_f2->buffer_address,
232 231 nb_sm_before_f2.norm_bp1 );
233 232 current_ring_node_to_send_asm_f2->coarseTime = incomingMsg->coarseTimeNORM;
234 233 current_ring_node_to_send_asm_f2->fineTime = incomingMsg->fineTimeNORM;
235 234 current_ring_node_to_send_asm_f2->sid = SID_NORM_ASM_F2;
236 235
237 236 // 3) send the spectral matrix packets
238 237 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f2, sizeof( ring_node* ) );
239 238
240 239 // change asm ring node
241 240 current_ring_node_to_send_asm_f2 = current_ring_node_to_send_asm_f2->next;
242 241 }
243 242
244 243 update_queue_max_count( queue_id_q_p2, &hk_lfr_q_p2_fifo_size_max );
245 244
246 245 }
247 246 }
248 247
249 248 //**********
250 249 // FUNCTIONS
251 250
252 251 void reset_nb_sm_f2( void )
253 252 {
254 253 nb_sm_before_f2.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0;
255 254 nb_sm_before_f2.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1;
256 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];
255 nb_sm_before_f2.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_asm_p[1];
257 256 }
258 257
259 258 void SM_average_f2( float *averaged_spec_mat_f2,
260 259 ring_node *ring_node,
261 260 unsigned int nbAverageNormF2,
262 261 asm_msg *msgForMATR )
263 262 {
264 263 float sum;
265 264 unsigned int i;
266 265 unsigned char keepMatrix;
267 266
268 267 // test acquisitionTime validity
269 keepMatrix = acquisitionTimeIsValid( ring_node->coarseTime, ring_node->fineTime, 2 );
268 keepMatrix = acquisitionTimeIsValid( ring_node->coarseTime, ring_node->fineTime, CHANNELF2 );
270 269
271 270 for(i=0; i<TOTAL_SIZE_SM; i++)
272 271 {
273 272 sum = ( (int *) (ring_node->buffer_address) ) [ i ];
274 273 if ( (nbAverageNormF2 == 0) ) // average initialization
275 274 {
276 275 if (keepMatrix == 1) // keep the matrix and add it to the average
277 276 {
278 277 averaged_spec_mat_f2[ i ] = sum;
279 278 }
280 279 else // drop the matrix and initialize the average
281 280 {
282 averaged_spec_mat_f2[ i ] = 0.;
281 averaged_spec_mat_f2[ i ] = INIT_FLOAT;
283 282 }
284 283 msgForMATR->coarseTimeNORM = ring_node->coarseTime;
285 284 msgForMATR->fineTimeNORM = ring_node->fineTime;
286 285 }
287 286 else
288 287 {
289 288 if (keepMatrix == 1) // keep the matrix and add it to the average
290 289 {
291 290 averaged_spec_mat_f2[ i ] = ( averaged_spec_mat_f2[ i ] + sum );
292 291 }
293 292 else
294 293 {
295 294 // nothing to do, the matrix is not valid
296 295 }
297 296 }
298 297 }
299 298
300 299 if (keepMatrix == 1)
301 300 {
302 301 if ( (nbAverageNormF2 == 0) )
303 302 {
304 303 msgForMATR->numberOfSMInASMNORM = 1;
305 304 }
306 305 else
307 306 {
308 307 msgForMATR->numberOfSMInASMNORM++;
309 308 }
310 309 }
311 310 else
312 311 {
313 312 if ( (nbAverageNormF2 == 0) )
314 313 {
315 314 msgForMATR->numberOfSMInASMNORM = 0;
316 315 }
317 316 else
318 317 {
319 318 // nothing to do
320 319 }
321 320 }
322 321 }
323 322
324 323 void init_k_coefficients_prc2( void )
325 324 {
326 325 init_k_coefficients( k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2);
327 326 }
Show file before | Show file after | Hide comments
@@ -1,792 +1,794
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 12 #include "fsw_init.h"
13 13
14 14 unsigned int nb_sm_f0;
15 15 unsigned int nb_sm_f0_aux_f1;
16 16 unsigned int nb_sm_f1;
17 17 unsigned int nb_sm_f0_aux_f2;
18 18
19 19 typedef enum restartState_t
20 20 {
21 21 WAIT_FOR_F2,
22 22 WAIT_FOR_F1,
23 23 WAIT_FOR_F0
24 24 } restartState;
25 25
26 26 //************************
27 27 // spectral matrices rings
28 28 ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ];
29 29 ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ];
30 30 ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ];
31 31 ring_node *current_ring_node_sm_f0;
32 32 ring_node *current_ring_node_sm_f1;
33 33 ring_node *current_ring_node_sm_f2;
34 34 ring_node *ring_node_for_averaging_sm_f0;
35 35 ring_node *ring_node_for_averaging_sm_f1;
36 36 ring_node *ring_node_for_averaging_sm_f2;
37 37
38 38 //
39 39 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
40 40 {
41 41 ring_node *node;
42 42
43 43 node = NULL;
44 44 switch ( frequencyChannel ) {
45 case 0:
45 case CHANNELF0:
46 46 node = ring_node_for_averaging_sm_f0;
47 47 break;
48 case 1:
48 case CHANNELF1:
49 49 node = ring_node_for_averaging_sm_f1;
50 50 break;
51 case 2:
51 case CHANNELF2:
52 52 node = ring_node_for_averaging_sm_f2;
53 53 break;
54 54 default:
55 55 break;
56 56 }
57 57
58 58 return node;
59 59 }
60 60
61 61 //***********************************************************
62 62 // Interrupt Service Routine for spectral matrices processing
63 63
64 64 void spectral_matrices_isr_f0( int statusReg )
65 65 {
66 66 unsigned char status;
67 67 rtems_status_code status_code;
68 68 ring_node *full_ring_node;
69 69
70 status = (unsigned char) (statusReg & 0x03); // [0011] get the status_ready_matrix_f0_x bits
70 status = (unsigned char) (statusReg & BITS_STATUS_F0); // [0011] get the status_ready_matrix_f0_x bits
71 71
72 72 switch(status)
73 73 {
74 74 case 0:
75 75 break;
76 case 3:
76 case BIT_READY_0_1:
77 77 // UNEXPECTED VALUE
78 spectral_matrix_regs->status = 0x03; // [0011]
78 spectral_matrix_regs->status = BIT_READY_0_1; // [0011]
79 79 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
80 80 break;
81 case 1:
81 case BIT_READY_0:
82 82 full_ring_node = current_ring_node_sm_f0->previous;
83 83 full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
84 84 full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time;
85 85 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
86 86 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
87 87 // if there are enough ring nodes ready, wake up an AVFx task
88 88 nb_sm_f0 = nb_sm_f0 + 1;
89 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
89 if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
90 90 {
91 91 ring_node_for_averaging_sm_f0 = full_ring_node;
92 92 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
93 93 {
94 94 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
95 95 }
96 96 nb_sm_f0 = 0;
97 97 }
98 spectral_matrix_regs->status = 0x01; // [0000 0001]
98 spectral_matrix_regs->status = BIT_READY_0; // [0000 0001]
99 99 break;
100 case 2:
100 case BIT_READY_1:
101 101 full_ring_node = current_ring_node_sm_f0->previous;
102 102 full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
103 103 full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time;
104 104 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
105 105 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
106 106 // if there are enough ring nodes ready, wake up an AVFx task
107 107 nb_sm_f0 = nb_sm_f0 + 1;
108 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
108 if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
109 109 {
110 110 ring_node_for_averaging_sm_f0 = full_ring_node;
111 111 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
112 112 {
113 113 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
114 114 }
115 115 nb_sm_f0 = 0;
116 116 }
117 spectral_matrix_regs->status = 0x02; // [0000 0010]
117 spectral_matrix_regs->status = BIT_READY_1; // [0000 0010]
118 118 break;
119 119 }
120 120 }
121 121
122 122 void spectral_matrices_isr_f1( int statusReg )
123 123 {
124 124 rtems_status_code status_code;
125 125 unsigned char status;
126 126 ring_node *full_ring_node;
127 127
128 status = (unsigned char) ((statusReg & 0x0c) >> 2); // [1100] get the status_ready_matrix_f1_x bits
128 status = (unsigned char) ((statusReg & BITS_STATUS_F1) >> SHIFT_2_BITS); // [1100] get the status_ready_matrix_f1_x bits
129 129
130 130 switch(status)
131 131 {
132 132 case 0:
133 133 break;
134 case 3:
134 case BIT_READY_0_1:
135 135 // UNEXPECTED VALUE
136 spectral_matrix_regs->status = 0xc0; // [1100]
136 spectral_matrix_regs->status = BITS_STATUS_F1; // [1100]
137 137 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
138 138 break;
139 case 1:
139 case BIT_READY_0:
140 140 full_ring_node = current_ring_node_sm_f1->previous;
141 141 full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
142 142 full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
143 143 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
144 144 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
145 145 // if there are enough ring nodes ready, wake up an AVFx task
146 146 nb_sm_f1 = nb_sm_f1 + 1;
147 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
147 if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
148 148 {
149 149 ring_node_for_averaging_sm_f1 = full_ring_node;
150 150 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
151 151 {
152 152 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
153 153 }
154 154 nb_sm_f1 = 0;
155 155 }
156 spectral_matrix_regs->status = 0x04; // [0000 0100]
156 spectral_matrix_regs->status = BIT_STATUS_F1_0; // [0000 0100]
157 157 break;
158 case 2:
158 case BIT_READY_1:
159 159 full_ring_node = current_ring_node_sm_f1->previous;
160 160 full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
161 161 full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
162 162 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
163 163 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
164 164 // if there are enough ring nodes ready, wake up an AVFx task
165 165 nb_sm_f1 = nb_sm_f1 + 1;
166 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
166 if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
167 167 {
168 168 ring_node_for_averaging_sm_f1 = full_ring_node;
169 169 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
170 170 {
171 171 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
172 172 }
173 173 nb_sm_f1 = 0;
174 174 }
175 spectral_matrix_regs->status = 0x08; // [1000 0000]
175 spectral_matrix_regs->status = BIT_STATUS_F1_1; // [1000 0000]
176 176 break;
177 177 }
178 178 }
179 179
180 180 void spectral_matrices_isr_f2( int statusReg )
181 181 {
182 182 unsigned char status;
183 183 rtems_status_code status_code;
184 184
185 status = (unsigned char) ((statusReg & 0x30) >> 4); // [0011 0000] get the status_ready_matrix_f2_x bits
185 status = (unsigned char) ((statusReg & BITS_STATUS_F2) >> SHIFT_4_BITS); // [0011 0000] get the status_ready_matrix_f2_x bits
186 186
187 187 switch(status)
188 188 {
189 189 case 0:
190 190 break;
191 case 3:
191 case BIT_READY_0_1:
192 192 // UNEXPECTED VALUE
193 spectral_matrix_regs->status = 0x30; // [0011 0000]
193 spectral_matrix_regs->status = BITS_STATUS_F2; // [0011 0000]
194 194 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
195 195 break;
196 case 1:
196 case BIT_READY_0:
197 197 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
198 198 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
199 199 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
200 200 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
201 201 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
202 spectral_matrix_regs->status = 0x10; // [0001 0000]
202 spectral_matrix_regs->status = BIT_STATUS_F2_0; // [0001 0000]
203 203 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
204 204 {
205 205 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
206 206 }
207 207 break;
208 case 2:
208 case BIT_READY_1:
209 209 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
210 210 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
211 211 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
212 212 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
213 213 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
214 spectral_matrix_regs->status = 0x20; // [0010 0000]
214 spectral_matrix_regs->status = BIT_STATUS_F2_1; // [0010 0000]
215 215 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
216 216 {
217 217 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
218 218 }
219 219 break;
220 220 }
221 221 }
222 222
223 223 void spectral_matrix_isr_error_handler( int statusReg )
224 224 {
225 225 // STATUS REGISTER
226 226 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
227 227 // 10 9 8
228 228 // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
229 229 // 7 6 5 4 3 2 1 0
230 230 // [bad_component_err] not defined in the last version of the VHDL code
231 231
232 232 rtems_status_code status_code;
233 233
234 234 //***************************************************
235 235 // the ASM status register is copied in the HK packet
236 housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) (statusReg & 0x780 >> 7); // [0111 1000 0000]
236 housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) ((statusReg & BITS_HK_AA_SM) >> SHIFT_7_BITS); // [0111 1000 0000]
237 237
238 if (statusReg & 0x7c0) // [0111 1100 0000]
238 if (statusReg & BITS_SM_ERR) // [0111 1100 0000]
239 239 {
240 240 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
241 241 }
242 242
243 spectral_matrix_regs->status = spectral_matrix_regs->status & 0x7c0;
243 spectral_matrix_regs->status = spectral_matrix_regs->status & BITS_SM_ERR;
244 244
245 245 }
246 246
247 247 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
248 248 {
249 249 // STATUS REGISTER
250 250 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
251 251 // 10 9 8
252 252 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
253 253 // 7 6 5 4 3 2 1 0
254 254
255 255 int statusReg;
256 256
257 257 static restartState state = WAIT_FOR_F2;
258 258
259 259 statusReg = spectral_matrix_regs->status;
260 260
261 261 if (thisIsAnASMRestart == 0)
262 262 { // this is not a restart sequence, process incoming matrices normally
263 263 spectral_matrices_isr_f0( statusReg );
264 264
265 265 spectral_matrices_isr_f1( statusReg );
266 266
267 267 spectral_matrices_isr_f2( statusReg );
268 268 }
269 269 else
270 270 { // a restart sequence has to be launched
271 271 switch (state) {
272 272 case WAIT_FOR_F2:
273 if ((statusReg & 0x30) != 0x00) // [0011 0000] check the status_ready_matrix_f2_x bits
273 if ((statusReg & BITS_STATUS_F2) != INIT_CHAR) // [0011 0000] check the status_ready_matrix_f2_x bits
274 274 {
275 275 state = WAIT_FOR_F1;
276 276 }
277 277 break;
278 278 case WAIT_FOR_F1:
279 if ((statusReg & 0x0c) != 0x00) // [0000 1100] check the status_ready_matrix_f1_x bits
279 if ((statusReg & BITS_STATUS_F1) != INIT_CHAR) // [0000 1100] check the status_ready_matrix_f1_x bits
280 280 {
281 281 state = WAIT_FOR_F0;
282 282 }
283 283 break;
284 284 case WAIT_FOR_F0:
285 if ((statusReg & 0x03) != 0x00) // [0000 0011] check the status_ready_matrix_f0_x bits
285 if ((statusReg & BITS_STATUS_F0) != INIT_CHAR) // [0000 0011] check the status_ready_matrix_f0_x bits
286 286 {
287 287 state = WAIT_FOR_F2;
288 288 thisIsAnASMRestart = 0;
289 289 }
290 290 break;
291 291 default:
292 292 break;
293 293 }
294 294 reset_sm_status();
295 295 }
296 296
297 297 spectral_matrix_isr_error_handler( statusReg );
298 298
299 299 }
300 300
301 301 //******************
302 302 // Spectral Matrices
303 303
304 304 void reset_nb_sm( void )
305 305 {
306 306 nb_sm_f0 = 0;
307 307 nb_sm_f0_aux_f1 = 0;
308 308 nb_sm_f0_aux_f2 = 0;
309 309
310 310 nb_sm_f1 = 0;
311 311 }
312 312
313 313 void SM_init_rings( void )
314 314 {
315 315 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
316 316 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
317 317 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
318 318
319 319 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
320 320 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
321 321 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
322 322 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
323 323 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
324 324 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
325 325 }
326 326
327 327 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
328 328 {
329 329 unsigned char i;
330 330
331 331 ring[ nbNodes - 1 ].next
332 332 = (ring_node_asm*) &ring[ 0 ];
333 333
334 334 for(i=0; i<nbNodes-1; i++)
335 335 {
336 336 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
337 337 }
338 338 }
339 339
340 340 void SM_reset_current_ring_nodes( void )
341 341 {
342 342 current_ring_node_sm_f0 = sm_ring_f0[0].next;
343 343 current_ring_node_sm_f1 = sm_ring_f1[0].next;
344 344 current_ring_node_sm_f2 = sm_ring_f2[0].next;
345 345
346 346 ring_node_for_averaging_sm_f0 = NULL;
347 347 ring_node_for_averaging_sm_f1 = NULL;
348 348 ring_node_for_averaging_sm_f2 = NULL;
349 349 }
350 350
351 351 //*****************
352 352 // Basic Parameters
353 353
354 354 void BP_init_header( bp_packet *packet,
355 355 unsigned int apid, unsigned char sid,
356 356 unsigned int packetLength, unsigned char blkNr )
357 357 {
358 358 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
359 359 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
360 packet->reserved = 0x00;
360 packet->reserved = INIT_CHAR;
361 361 packet->userApplication = CCSDS_USER_APP;
362 packet->packetID[0] = (unsigned char) (apid >> 8);
362 packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
363 363 packet->packetID[1] = (unsigned char) (apid);
364 364 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
365 packet->packetSequenceControl[1] = 0x00;
366 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
365 packet->packetSequenceControl[1] = INIT_CHAR;
366 packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
367 367 packet->packetLength[1] = (unsigned char) (packetLength);
368 368 // DATA FIELD HEADER
369 packet->spare1_pusVersion_spare2 = 0x10;
369 packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
370 370 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
371 371 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
372 372 packet->destinationID = TM_DESTINATION_ID_GROUND;
373 packet->time[0] = 0x00;
374 packet->time[1] = 0x00;
375 packet->time[2] = 0x00;
376 packet->time[3] = 0x00;
377 packet->time[4] = 0x00;
378 packet->time[5] = 0x00;
373 packet->time[BYTE_0] = INIT_CHAR;
374 packet->time[BYTE_1] = INIT_CHAR;
375 packet->time[BYTE_2] = INIT_CHAR;
376 packet->time[BYTE_3] = INIT_CHAR;
377 packet->time[BYTE_4] = INIT_CHAR;
378 packet->time[BYTE_5] = INIT_CHAR;
379 379 // AUXILIARY DATA HEADER
380 380 packet->sid = sid;
381 packet->pa_bia_status_info = 0x00;
382 packet->sy_lfr_common_parameters_spare = 0x00;
383 packet->sy_lfr_common_parameters = 0x00;
384 packet->acquisitionTime[0] = 0x00;
385 packet->acquisitionTime[1] = 0x00;
386 packet->acquisitionTime[2] = 0x00;
387 packet->acquisitionTime[3] = 0x00;
388 packet->acquisitionTime[4] = 0x00;
389 packet->acquisitionTime[5] = 0x00;
390 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
381 packet->pa_bia_status_info = INIT_CHAR;
382 packet->sy_lfr_common_parameters_spare = INIT_CHAR;
383 packet->sy_lfr_common_parameters = INIT_CHAR;
384 packet->acquisitionTime[BYTE_0] = INIT_CHAR;
385 packet->acquisitionTime[BYTE_1] = INIT_CHAR;
386 packet->acquisitionTime[BYTE_2] = INIT_CHAR;
387 packet->acquisitionTime[BYTE_3] = INIT_CHAR;
388 packet->acquisitionTime[BYTE_4] = INIT_CHAR;
389 packet->acquisitionTime[BYTE_5] = INIT_CHAR;
390 packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB
391 391 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
392 392 }
393 393
394 394 void BP_init_header_with_spare( bp_packet_with_spare *packet,
395 395 unsigned int apid, unsigned char sid,
396 396 unsigned int packetLength , unsigned char blkNr)
397 397 {
398 398 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
399 399 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
400 packet->reserved = 0x00;
400 packet->reserved = INIT_CHAR;
401 401 packet->userApplication = CCSDS_USER_APP;
402 packet->packetID[0] = (unsigned char) (apid >> 8);
402 packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
403 403 packet->packetID[1] = (unsigned char) (apid);
404 404 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
405 packet->packetSequenceControl[1] = 0x00;
406 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
405 packet->packetSequenceControl[1] = INIT_CHAR;
406 packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
407 407 packet->packetLength[1] = (unsigned char) (packetLength);
408 408 // DATA FIELD HEADER
409 packet->spare1_pusVersion_spare2 = 0x10;
409 packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
410 410 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
411 411 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
412 412 packet->destinationID = TM_DESTINATION_ID_GROUND;
413 413 // AUXILIARY DATA HEADER
414 414 packet->sid = sid;
415 packet->pa_bia_status_info = 0x00;
416 packet->sy_lfr_common_parameters_spare = 0x00;
417 packet->sy_lfr_common_parameters = 0x00;
418 packet->time[0] = 0x00;
419 packet->time[0] = 0x00;
420 packet->time[0] = 0x00;
421 packet->time[0] = 0x00;
422 packet->time[0] = 0x00;
423 packet->time[0] = 0x00;
424 packet->source_data_spare = 0x00;
425 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
415 packet->pa_bia_status_info = INIT_CHAR;
416 packet->sy_lfr_common_parameters_spare = INIT_CHAR;
417 packet->sy_lfr_common_parameters = INIT_CHAR;
418 packet->time[BYTE_0] = INIT_CHAR;
419 packet->time[BYTE_1] = INIT_CHAR;
420 packet->time[BYTE_2] = INIT_CHAR;
421 packet->time[BYTE_3] = INIT_CHAR;
422 packet->time[BYTE_4] = INIT_CHAR;
423 packet->time[BYTE_5] = INIT_CHAR;
424 packet->source_data_spare = INIT_CHAR;
425 packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB
426 426 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
427 427 }
428 428
429 429 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
430 430 {
431 431 rtems_status_code status;
432 432
433 433 // SEND PACKET
434 434 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
435 435 if (status != RTEMS_SUCCESSFUL)
436 436 {
437 437 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
438 438 }
439 439 }
440 440
441 441 void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
442 442 {
443 443 /** This function is used to send the BP paquets when needed.
444 444 *
445 445 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
446 446 *
447 447 * @return void
448 448 *
449 449 * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition.
450 450 * BURST paquets are sent everytime.
451 451 *
452 452 */
453 453
454 454 rtems_status_code status;
455 455
456 456 // SEND PACKET
457 457 // before lastValidTransitionDate, the data are drops even if they are ready
458 458 // this guarantees that no SBM packets will be received before the requested enter mode time
459 459 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
460 460 {
461 461 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
462 462 if (status != RTEMS_SUCCESSFUL)
463 463 {
464 464 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
465 465 }
466 466 }
467 467 }
468 468
469 469 //******************
470 470 // general functions
471 471
472 472 void reset_sm_status( void )
473 473 {
474 474 // error
475 475 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
476 476 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
477 477 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
478 478 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
479 479
480 spectral_matrix_regs->status = 0x7ff; // [0111 1111 1111]
480 spectral_matrix_regs->status = BITS_STATUS_REG; // [0111 1111 1111]
481 481 }
482 482
483 483 void reset_spectral_matrix_regs( void )
484 484 {
485 485 /** This function resets the spectral matrices module registers.
486 486 *
487 487 * The registers affected by this function are located at the following offset addresses:
488 488 *
489 489 * - 0x00 config
490 490 * - 0x04 status
491 491 * - 0x08 matrixF0_Address0
492 492 * - 0x10 matrixFO_Address1
493 493 * - 0x14 matrixF1_Address
494 494 * - 0x18 matrixF2_Address
495 495 *
496 496 */
497 497
498 498 set_sm_irq_onError( 0 );
499 499
500 500 set_sm_irq_onNewMatrix( 0 );
501 501
502 502 reset_sm_status();
503 503
504 504 // F1
505 505 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
506 506 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
507 507 // F2
508 508 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
509 509 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
510 510 // F3
511 511 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
512 512 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
513 513
514 spectral_matrix_regs->matrix_length = 0xc8; // 25 * 128 / 16 = 200 = 0xc8
514 spectral_matrix_regs->matrix_length = DEFAULT_MATRIX_LENGTH; // 25 * 128 / 16 = 200 = 0xc8
515 515 }
516 516
517 517 void set_time( unsigned char *time, unsigned char * timeInBuffer )
518 518 {
519 time[0] = timeInBuffer[0];
520 time[1] = timeInBuffer[1];
521 time[2] = timeInBuffer[2];
522 time[3] = timeInBuffer[3];
523 time[4] = timeInBuffer[6];
524 time[5] = timeInBuffer[7];
519 time[BYTE_0] = timeInBuffer[BYTE_0];
520 time[BYTE_1] = timeInBuffer[BYTE_1];
521 time[BYTE_2] = timeInBuffer[BYTE_2];
522 time[BYTE_3] = timeInBuffer[BYTE_3];
523 time[BYTE_4] = timeInBuffer[BYTE_6];
524 time[BYTE_5] = timeInBuffer[BYTE_7];
525 525 }
526 526
527 527 unsigned long long int get_acquisition_time( unsigned char *timePtr )
528 528 {
529 529 unsigned long long int acquisitionTimeAslong;
530 acquisitionTimeAslong = 0x00;
531 acquisitionTimeAslong = ( (unsigned long long int) (timePtr[0] & 0x7f) << 40 ) // [0111 1111] mask the synchronization bit
532 + ( (unsigned long long int) timePtr[1] << 32 )
533 + ( (unsigned long long int) timePtr[2] << 24 )
534 + ( (unsigned long long int) timePtr[3] << 16 )
535 + ( (unsigned long long int) timePtr[6] << 8 )
536 + ( (unsigned long long int) timePtr[7] );
530 acquisitionTimeAslong = INIT_CHAR;
531 acquisitionTimeAslong =
532 ( (unsigned long long int) (timePtr[BYTE_0] & SYNC_BIT_MASK) << SHIFT_5_BYTES ) // [0111 1111] mask the synchronization bit
533 + ( (unsigned long long int) timePtr[BYTE_1] << SHIFT_4_BYTES )
534 + ( (unsigned long long int) timePtr[BYTE_2] << SHIFT_3_BYTES )
535 + ( (unsigned long long int) timePtr[BYTE_3] << SHIFT_2_BYTES )
536 + ( (unsigned long long int) timePtr[BYTE_6] << SHIFT_1_BYTE )
537 + ( (unsigned long long int) timePtr[BYTE_7] );
537 538 return acquisitionTimeAslong;
538 539 }
539 540
540 541 unsigned char getSID( rtems_event_set event )
541 542 {
542 543 unsigned char sid;
543 544
544 545 rtems_event_set eventSetBURST;
545 546 rtems_event_set eventSetSBM;
546 547
547 548 //******
548 549 // BURST
549 550 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
550 551 | RTEMS_EVENT_BURST_BP1_F1
551 552 | RTEMS_EVENT_BURST_BP2_F0
552 553 | RTEMS_EVENT_BURST_BP2_F1;
553 554
554 555 //****
555 556 // SBM
556 557 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
557 558 | RTEMS_EVENT_SBM_BP1_F1
558 559 | RTEMS_EVENT_SBM_BP2_F0
559 560 | RTEMS_EVENT_SBM_BP2_F1;
560 561
561 562 if (event & eventSetBURST)
562 563 {
563 564 sid = SID_BURST_BP1_F0;
564 565 }
565 566 else if (event & eventSetSBM)
566 567 {
567 568 sid = SID_SBM1_BP1_F0;
568 569 }
569 570 else
570 571 {
571 572 sid = 0;
572 573 }
573 574
574 575 return sid;
575 576 }
576 577
577 578 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
578 579 {
579 580 unsigned int i;
580 581 float re;
581 582 float im;
582 583
583 584 for (i=0; i<NB_BINS_PER_SM; i++){
584 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 ];
585 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 + 1];
586 outputASM[ (asmComponent *NB_BINS_PER_SM) + i] = re;
587 outputASM[ (asmComponent+1)*NB_BINS_PER_SM + i] = im;
585 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) ];
586 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) + 1];
587 outputASM[ ( asmComponent *NB_BINS_PER_SM) + i] = re;
588 outputASM[ ((asmComponent+1)*NB_BINS_PER_SM) + i] = im;
588 589 }
589 590 }
590 591
591 592 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
592 593 {
593 594 unsigned int i;
594 595 float re;
595 596
596 597 for (i=0; i<NB_BINS_PER_SM; i++){
597 598 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
598 599 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
599 600 }
600 601 }
601 602
602 603 void ASM_patch( float *inputASM, float *outputASM )
603 604 {
604 extractReImVectors( inputASM, outputASM, 1); // b1b2
605 extractReImVectors( inputASM, outputASM, 3 ); // b1b3
606 extractReImVectors( inputASM, outputASM, 5 ); // b1e1
607 extractReImVectors( inputASM, outputASM, 7 ); // b1e2
608 extractReImVectors( inputASM, outputASM, 10 ); // b2b3
609 extractReImVectors( inputASM, outputASM, 12 ); // b2e1
610 extractReImVectors( inputASM, outputASM, 14 ); // b2e2
611 extractReImVectors( inputASM, outputASM, 17 ); // b3e1
612 extractReImVectors( inputASM, outputASM, 19 ); // b3e2
613 extractReImVectors( inputASM, outputASM, 22 ); // e1e2
605 extractReImVectors( inputASM, outputASM, ASM_COMP_B1B2); // b1b2
606 extractReImVectors( inputASM, outputASM, ASM_COMP_B1B3 ); // b1b3
607 extractReImVectors( inputASM, outputASM, ASM_COMP_B1E1 ); // b1e1
608 extractReImVectors( inputASM, outputASM, ASM_COMP_B1E2 ); // b1e2
609 extractReImVectors( inputASM, outputASM, ASM_COMP_B2B3 ); // b2b3
610 extractReImVectors( inputASM, outputASM, ASM_COMP_B2E1 ); // b2e1
611 extractReImVectors( inputASM, outputASM, ASM_COMP_B2E2 ); // b2e2
612 extractReImVectors( inputASM, outputASM, ASM_COMP_B3E1 ); // b3e1
613 extractReImVectors( inputASM, outputASM, ASM_COMP_B3E2 ); // b3e2
614 extractReImVectors( inputASM, outputASM, ASM_COMP_E1E2 ); // e1e2
614 615
615 copyReVectors(inputASM, outputASM, 0 ); // b1b1
616 copyReVectors(inputASM, outputASM, 9 ); // b2b2
617 copyReVectors(inputASM, outputASM, 16); // b3b3
618 copyReVectors(inputASM, outputASM, 21); // e1e1
619 copyReVectors(inputASM, outputASM, 24); // e2e2
616 copyReVectors(inputASM, outputASM, ASM_COMP_B1B1 ); // b1b1
617 copyReVectors(inputASM, outputASM, ASM_COMP_B2B2 ); // b2b2
618 copyReVectors(inputASM, outputASM, ASM_COMP_B3B3); // b3b3
619 copyReVectors(inputASM, outputASM, ASM_COMP_E1E1); // e1e1
620 copyReVectors(inputASM, outputASM, ASM_COMP_E2E2); // e2e2
620 621 }
621 622
622 623 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
623 624 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage,
624 625 unsigned char ASMIndexStart,
625 626 unsigned char channel )
626 627 {
627 628 //*************
628 629 // input format
629 630 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
630 631 //**************
631 632 // output format
632 633 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
633 634 //************
634 635 // compression
635 636 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
636 637 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
637 638
638 639 int frequencyBin;
639 640 int asmComponent;
640 641 int offsetASM;
641 642 int offsetCompressed;
642 643 int offsetFBin;
643 644 int fBinMask;
644 645 int k;
645 646
646 647 // BUILD DATA
647 648 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
648 649 {
649 650 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
650 651 {
651 652 offsetCompressed = // NO TIME OFFSET
652 frequencyBin * NB_VALUES_PER_SM
653 (frequencyBin * NB_VALUES_PER_SM)
653 654 + asmComponent;
654 655 offsetASM = // NO TIME OFFSET
655 asmComponent * NB_BINS_PER_SM
656 (asmComponent * NB_BINS_PER_SM)
656 657 + ASMIndexStart
657 + frequencyBin * nbBinsToAverage;
658 + (frequencyBin * nbBinsToAverage);
658 659 offsetFBin = ASMIndexStart
659 + frequencyBin * nbBinsToAverage;
660 + (frequencyBin * nbBinsToAverage);
660 661 compressed_spec_mat[ offsetCompressed ] = 0;
661 662 for ( k = 0; k < nbBinsToAverage; k++ )
662 663 {
663 664 fBinMask = getFBinMask( offsetFBin + k, channel );
664 compressed_spec_mat[offsetCompressed ] =
665 ( compressed_spec_mat[ offsetCompressed ]
666 + averaged_spec_mat[ offsetASM + k ] * fBinMask );
665 compressed_spec_mat[offsetCompressed ] = compressed_spec_mat[ offsetCompressed ]
666 + (averaged_spec_mat[ offsetASM + k ] * fBinMask);
667 667 }
668 668 if (divider != 0)
669 669 {
670 670 compressed_spec_mat[ offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
671 671 }
672 672 else
673 673 {
674 compressed_spec_mat[ offsetCompressed ] = 0.0;
674 compressed_spec_mat[ offsetCompressed ] = INIT_FLOAT;
675 675 }
676 676 }
677 677 }
678 678
679 679 }
680 680
681 681 int getFBinMask( int index, unsigned char channel )
682 682 {
683 683 unsigned int indexInChar;
684 684 unsigned int indexInTheChar;
685 685 int fbin;
686 686 unsigned char *sy_lfr_fbins_fx_word1;
687 687
688 688 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins.fx.f0_word1;
689 689
690 690 switch(channel)
691 691 {
692 case 0:
692 case CHANNELF0:
693 693 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0;
694 694 break;
695 case 1:
695 case CHANNELF1:
696 696 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1;
697 697 break;
698 case 2:
698 case CHANNELF2:
699 699 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2;
700 700 break;
701 701 default:
702 702 PRINTF("ERR *** in getFBinMask, wrong frequency channel")
703 703 }
704 704
705 indexInChar = index >> 3;
706 indexInTheChar = index - indexInChar * 8;
705 indexInChar = index >> SHIFT_3_BITS;
706 indexInTheChar = index - (indexInChar * BITS_PER_BYTE);
707 707
708 fbin = (int) ((sy_lfr_fbins_fx_word1[ NB_BYTES_PER_FREQ_MASK - 1 - indexInChar] >> indexInTheChar) & 0x1);
708 fbin = (int) ((sy_lfr_fbins_fx_word1[ BYTES_PER_MASK - 1 - indexInChar] >> indexInTheChar) & 1);
709 709
710 710 return fbin;
711 711 }
712 712
713 713 unsigned char acquisitionTimeIsValid( unsigned int coarseTime, unsigned int fineTime, unsigned char channel)
714 714 {
715 715 u_int64_t acquisitionTime;
716 716 u_int64_t timecodeReference;
717 717 u_int64_t offsetInFineTime;
718 718 u_int64_t shiftInFineTime;
719 719 u_int64_t tBadInFineTime;
720 720 u_int64_t acquisitionTimeRangeMin;
721 721 u_int64_t acquisitionTimeRangeMax;
722 722 unsigned char pasFilteringIsEnabled;
723 723 unsigned char ret;
724 724
725 pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 0x01); // [0000 0001]
725 pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 1); // [0000 0001]
726 726 ret = 1;
727 727
728 728 // compute acquisition time from caoarseTime and fineTime
729 acquisitionTime = ( ((u_int64_t)coarseTime) << 16 )
729 acquisitionTime = ( ((u_int64_t)coarseTime) << SHIFT_2_BYTES )
730 730 + (u_int64_t) fineTime;
731 731
732 732 // compute the timecode reference
733 timecodeReference = (u_int64_t) (floor( ((double) coarseTime) / ((double) filterPar.sy_lfr_pas_filter_modulus) )
734 * ((double) filterPar.sy_lfr_pas_filter_modulus)) * 65536;
733 timecodeReference = (u_int64_t) ( (floor( ((double) coarseTime) / ((double) filterPar.sy_lfr_pas_filter_modulus) )
734 * ((double) filterPar.sy_lfr_pas_filter_modulus)) * CONST_65536 );
735 735
736 736 // compute the acquitionTime range
737 offsetInFineTime = ((double) filterPar.sy_lfr_pas_filter_offset) * 65536;
738 shiftInFineTime = ((double) filterPar.sy_lfr_pas_filter_shift) * 65536;
739 tBadInFineTime = ((double) filterPar.sy_lfr_pas_filter_tbad) * 65536;
737 offsetInFineTime = ((double) filterPar.sy_lfr_pas_filter_offset) * CONST_65536;
738 shiftInFineTime = ((double) filterPar.sy_lfr_pas_filter_shift) * CONST_65536;
739 tBadInFineTime = ((double) filterPar.sy_lfr_pas_filter_tbad) * CONST_65536;
740 740
741 741 acquisitionTimeRangeMin =
742 742 timecodeReference
743 743 + offsetInFineTime
744 744 + shiftInFineTime
745 745 - acquisitionDurations[channel];
746 746 acquisitionTimeRangeMax =
747 747 timecodeReference
748 748 + offsetInFineTime
749 749 + shiftInFineTime
750 750 + tBadInFineTime;
751 751
752 752 if ( (acquisitionTime >= acquisitionTimeRangeMin)
753 753 && (acquisitionTime <= acquisitionTimeRangeMax)
754 754 && (pasFilteringIsEnabled == 1) )
755 755 {
756 756 ret = 0; // the acquisition time is INSIDE the range, the matrix shall be ignored
757 757 }
758 758 else
759 759 {
760 760 ret = 1; // the acquisition time is OUTSIDE the range, the matrix can be used for the averaging
761 761 }
762 762
763 763 // printf("coarseTime = %x, fineTime = %x\n",
764 764 // coarseTime,
765 765 // fineTime);
766 766
767 767 // printf("[ret = %d] *** acquisitionTime = %f, Reference = %f",
768 768 // ret,
769 769 // acquisitionTime / 65536.,
770 770 // timecodeReference / 65536.);
771 771
772 772 // printf(", Min = %f, Max = %f\n",
773 773 // acquisitionTimeRangeMin / 65536.,
774 774 // acquisitionTimeRangeMax / 65536.);
775 775
776 776 return ret;
777 777 }
778 778
779 779 void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
780 780 {
781 781 unsigned char bin;
782 782 unsigned char kcoeff;
783 783
784 784 for (bin=0; bin<nb_bins_norm; bin++)
785 785 {
786 786 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
787 787 {
788 output_kcoeff[ (bin*NB_K_COEFF_PER_BIN + kcoeff)*2 ] = input_kcoeff[ bin*NB_K_COEFF_PER_BIN + kcoeff ];
789 output_kcoeff[ (bin*NB_K_COEFF_PER_BIN + kcoeff)*2 + 1 ] = input_kcoeff[ bin*NB_K_COEFF_PER_BIN + kcoeff ];
788 output_kcoeff[ ( ( bin * NB_K_COEFF_PER_BIN ) + kcoeff ) * SBM_COEFF_PER_NORM_COEFF ]
789 = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
790 output_kcoeff[ ( ( bin * NB_K_COEFF_PER_BIN ) + kcoeff ) * SBM_COEFF_PER_NORM_COEFF + 1 ]
791 = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
790 792 }
791 793 }
792 794 }
Show file before | Show file after | Hide comments
@@ -1,474 +1,475
1 1 /** Functions related to TeleCommand acceptance.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TeleCommands parsing.\n
7 7 *
8 8 */
9 9
10 10 #include "tc_acceptance.h"
11 11 #include <stdio.h>
12 12
13 unsigned int lookUpTableForCRC[256];
13 unsigned int lookUpTableForCRC[CONST_256];
14 14
15 15 //**********************
16 16 // GENERAL USE FUNCTIONS
17 17 unsigned int Crc_opt( unsigned char D, unsigned int Chk)
18 18 {
19 19 /** This function generate the CRC for one byte and returns the value of the new syndrome.
20 20 *
21 21 * @param D is the current byte of data.
22 22 * @param Chk is the current syndrom value.
23 23 *
24 24 * @return the value of the new syndrome on two bytes.
25 25 *
26 26 */
27 27
28 return(((Chk << 8) & 0xff00)^lookUpTableForCRC [(((Chk >> 8)^D) & 0x00ff)]);
28 return(((Chk << SHIFT_1_BYTE) & BYTE0_MASK)^lookUpTableForCRC [(((Chk >> SHIFT_1_BYTE)^D) & BYTE1_MASK)]);
29 29 }
30 30
31 31 void initLookUpTableForCRC( void )
32 32 {
33 33 /** This function is used to initiates the look-up table for fast CRC computation.
34 34 *
35 35 * The global table lookUpTableForCRC[256] is initiated.
36 36 *
37 37 */
38 38
39 39 unsigned int i;
40 40 unsigned int tmp;
41 41
42 for (i=0; i<256; i++)
42 for (i=0; i<CONST_256; i++)
43 43 {
44 44 tmp = 0;
45 if((i & 1) != 0) {
46 tmp = tmp ^ 0x1021;
45 if((i & BIT_0) != 0) {
46 tmp = tmp ^ CONST_CRC_0;
47 47 }
48 if((i & 2) != 0) {
49 tmp = tmp ^ 0x2042;
48 if((i & BIT_1) != 0) {
49 tmp = tmp ^ CONST_CRC_1;
50 50 }
51 if((i & 4) != 0) {
52 tmp = tmp ^ 0x4084;
51 if((i & BIT_2) != 0) {
52 tmp = tmp ^ CONST_CRC_2;
53 53 }
54 if((i & 8) != 0) {
55 tmp = tmp ^ 0x8108;
54 if((i & BIT_3) != 0) {
55 tmp = tmp ^ CONST_CRC_3;
56 56 }
57 if((i & 16) != 0) {
58 tmp = tmp ^ 0x1231;
57 if((i & BIT_4) != 0) {
58 tmp = tmp ^ CONST_CRC_4;
59 59 }
60 if((i & 32) != 0) {
61 tmp = tmp ^ 0x2462;
60 if((i & BIT_5) != 0) {
61 tmp = tmp ^ CONST_CRC_5;
62 62 }
63 if((i & 64) != 0) {
64 tmp = tmp ^ 0x48c4;
63 if((i & BIT_6) != 0) {
64 tmp = tmp ^ CONST_CRC_6;
65 65 }
66 if((i & 128) != 0) {
67 tmp = tmp ^ 0x9188;
66 if((i & BIT_7) != 0) {
67 tmp = tmp ^ CONST_CRC_7;
68 68 }
69 69 lookUpTableForCRC[i] = tmp;
70 70 }
71 71 }
72 72
73 73 void GetCRCAsTwoBytes(unsigned char* data, unsigned char* crcAsTwoBytes, unsigned int sizeOfData)
74 74 {
75 75 /** This function calculates a two bytes Cyclic Redundancy Code.
76 76 *
77 77 * @param data points to a buffer containing the data on which to compute the CRC.
78 78 * @param crcAsTwoBytes points points to a two bytes buffer in which the CRC is stored.
79 79 * @param sizeOfData is the number of bytes of *data* used to compute the CRC.
80 80 *
81 81 * The specification of the Cyclic Redundancy Code is described in the following document: ECSS-E-70-41-A.
82 82 *
83 83 */
84 84
85 85 unsigned int Chk;
86 86 int j;
87 Chk = 0xffff; // reset the syndrom to all ones
87 Chk = CRC_RESET; // reset the syndrom to all ones
88 88 for (j=0; j<sizeOfData; j++) {
89 89 Chk = Crc_opt(data[j], Chk);
90 90 }
91 crcAsTwoBytes[0] = (unsigned char) (Chk >> 8);
92 crcAsTwoBytes[1] = (unsigned char) (Chk & 0x00ff);
91 crcAsTwoBytes[0] = (unsigned char) (Chk >> SHIFT_1_BYTE);
92 crcAsTwoBytes[1] = (unsigned char) (Chk & BYTE1_MASK);
93 93 }
94 94
95 95 //*********************
96 96 // ACCEPTANCE FUNCTIONS
97 97 int tc_parser(ccsdsTelecommandPacket_t * TCPacket, unsigned int estimatedPacketLength, unsigned char *computed_CRC)
98 98 {
99 99 /** This function parses TeleCommands.
100 100 *
101 101 * @param TC points to the TeleCommand that will be parsed.
102 102 * @param estimatedPacketLength is the PACKET_LENGTH field calculated from the effective length of the received packet.
103 103 *
104 104 * @return Status code of the parsing.
105 105 *
106 106 * The parsing checks:
107 107 * - process id
108 108 * - category
109 109 * - length: a global check is performed and a per subtype check also
110 110 * - type
111 111 * - subtype
112 112 * - crc
113 113 *
114 114 */
115 115
116 116 int status;
117 117 int status_crc;
118 118 unsigned char pid;
119 119 unsigned char category;
120 120 unsigned int packetLength;
121 121 unsigned char packetType;
122 122 unsigned char packetSubtype;
123 123 unsigned char sid;
124 124
125 125 status = CCSDS_TM_VALID;
126 126
127 127 // APID check *** APID on 2 bytes
128 pid = ((TCPacket->packetID[0] & 0x07)<<4) + ( (TCPacket->packetID[1]>>4) & 0x0f ); // PID = 11 *** 7 bits xxxxx210 7654xxxx
129 category = (TCPacket->packetID[1] & 0x0f); // PACKET_CATEGORY = 12 *** 4 bits xxxxxxxx xxxx3210
130 packetLength = (TCPacket->packetLength[0] * 256) + TCPacket->packetLength[1];
128 pid = ((TCPacket->packetID[0] & BITS_PID_0) << SHIFT_4_BITS)
129 + ( (TCPacket->packetID[1] >> SHIFT_4_BITS) & BITS_PID_1 ); // PID = 11 *** 7 bits xxxxx210 7654xxxx
130 category = (TCPacket->packetID[1] & BITS_CAT); // PACKET_CATEGORY = 12 *** 4 bits xxxxxxxx xxxx3210
131 packetLength = (TCPacket->packetLength[0] * CONST_256) + TCPacket->packetLength[1];
131 132 packetType = TCPacket->serviceType;
132 133 packetSubtype = TCPacket->serviceSubType;
133 134 sid = TCPacket->sourceID;
134 135
135 136 if ( pid != CCSDS_PROCESS_ID ) // CHECK THE PROCESS ID
136 137 {
137 138 status = ILLEGAL_APID;
138 139 }
139 140 if (status == CCSDS_TM_VALID) // CHECK THE CATEGORY
140 141 {
141 142 if ( category != CCSDS_PACKET_CATEGORY )
142 143 {
143 144 status = ILLEGAL_APID;
144 145 }
145 146 }
146 147 if (status == CCSDS_TM_VALID) // CHECK THE PACKET_LENGTH FIELD AND THE ESTIMATED PACKET_LENGTH COMPLIANCE
147 148 {
148 149 if (packetLength != estimatedPacketLength ) {
149 150 status = WRONG_LEN_PKT;
150 151 }
151 152 }
152 153 if (status == CCSDS_TM_VALID) // CHECK THAT THE PACKET DOES NOT EXCEED THE MAX SIZE
153 154 {
154 155 if ( packetLength > CCSDS_TC_PKT_MAX_SIZE ) {
155 156 status = WRONG_LEN_PKT;
156 157 }
157 158 }
158 159 if (status == CCSDS_TM_VALID) // CHECK THE TYPE
159 160 {
160 161 status = tc_check_type( packetType );
161 162 }
162 163 if (status == CCSDS_TM_VALID) // CHECK THE SUBTYPE
163 164 {
164 165 status = tc_check_type_subtype( packetType, packetSubtype );
165 166 }
166 167 if (status == CCSDS_TM_VALID) // CHECK THE SID
167 168 {
168 169 status = tc_check_sid( sid );
169 170 }
170 171 if (status == CCSDS_TM_VALID) // CHECK THE SUBTYPE AND LENGTH COMPLIANCE
171 172 {
172 173 status = tc_check_length( packetSubtype, packetLength );
173 174 }
174 175 status_crc = tc_check_crc( TCPacket, estimatedPacketLength, computed_CRC );
175 176 if (status == CCSDS_TM_VALID ) // CHECK CRC
176 177 {
177 178 status = status_crc;
178 179 }
179 180
180 181 return status;
181 182 }
182 183
183 184 int tc_check_type( unsigned char packetType )
184 185 {
185 186 /** This function checks that the type of a TeleCommand is valid.
186 187 *
187 188 * @param packetType is the type to check.
188 189 *
189 190 * @return Status code CCSDS_TM_VALID or ILL_TYPE.
190 191 *
191 192 */
192 193
193 194 int status;
194 195
195 196 if ( (packetType == TC_TYPE_GEN) || (packetType == TC_TYPE_TIME))
196 197 {
197 198 status = CCSDS_TM_VALID;
198 199 }
199 200 else
200 201 {
201 202 status = ILL_TYPE;
202 203 }
203 204
204 205 return status;
205 206 }
206 207
207 208 int tc_check_type_subtype( unsigned char packetType, unsigned char packetSubType )
208 209 {
209 210 /** This function checks that the subtype of a TeleCommand is valid and coherent with the type.
210 211 *
211 212 * @param packetType is the type of the TC.
212 213 * @param packetSubType is the subtype to check.
213 214 *
214 215 * @return Status code CCSDS_TM_VALID or ILL_SUBTYPE.
215 216 *
216 217 */
217 218
218 219 int status;
219 220
220 221 switch(packetType)
221 222 {
222 223 case TC_TYPE_GEN:
223 224 if ( (packetSubType == TC_SUBTYPE_RESET)
224 225 || (packetSubType == TC_SUBTYPE_LOAD_COMM)
225 226 || (packetSubType == TC_SUBTYPE_LOAD_NORM) || (packetSubType == TC_SUBTYPE_LOAD_BURST)
226 227 || (packetSubType == TC_SUBTYPE_LOAD_SBM1) || (packetSubType == TC_SUBTYPE_LOAD_SBM2)
227 228 || (packetSubType == TC_SUBTYPE_DUMP)
228 229 || (packetSubType == TC_SUBTYPE_ENTER)
229 230 || (packetSubType == TC_SUBTYPE_UPDT_INFO)
230 231 || (packetSubType == TC_SUBTYPE_EN_CAL) || (packetSubType == TC_SUBTYPE_DIS_CAL)
231 232 || (packetSubType == TC_SUBTYPE_LOAD_K) || (packetSubType == TC_SUBTYPE_DUMP_K)
232 233 || (packetSubType == TC_SUBTYPE_LOAD_FBINS)
233 234 || (packetSubType == TC_SUBTYPE_LOAD_FILTER_PAR))
234 235 {
235 236 status = CCSDS_TM_VALID;
236 237 }
237 238 else
238 239 {
239 240 status = ILL_SUBTYPE;
240 241 }
241 242 break;
242 243
243 244 case TC_TYPE_TIME:
244 245 if (packetSubType == TC_SUBTYPE_UPDT_TIME)
245 246 {
246 247 status = CCSDS_TM_VALID;
247 248 }
248 249 else
249 250 {
250 251 status = ILL_SUBTYPE;
251 252 }
252 253 break;
253 254
254 255 default:
255 256 status = ILL_SUBTYPE;
256 257 break;
257 258 }
258 259
259 260 return status;
260 261 }
261 262
262 263 int tc_check_sid( unsigned char sid )
263 264 {
264 265 /** This function checks that the sid of a TeleCommand is valid.
265 266 *
266 267 * @param sid is the sid to check.
267 268 *
268 269 * @return Status code CCSDS_TM_VALID or CORRUPTED.
269 270 *
270 271 */
271 272
272 273 int status;
273 274
274 275 if ( (sid == SID_TC_MISSION_TIMELINE) || (sid == SID_TC_TC_SEQUENCES) || (sid == SID_TC_RECOVERY_ACTION_CMD)
275 276 || (sid == SID_TC_BACKUP_MISSION_TIMELINE)
276 277 || (sid == SID_TC_DIRECT_CMD) || (sid == SID_TC_SPARE_GRD_SRC1) || (sid == SID_TC_SPARE_GRD_SRC2)
277 278 || (sid == SID_TC_OBCP) || (sid == SID_TC_SYSTEM_CONTROL) || (sid == SID_TC_AOCS)
278 279 || (sid == SID_TC_RPW_INTERNAL))
279 280 {
280 281 status = CCSDS_TM_VALID;
281 282 }
282 283 else
283 284 {
284 285 status = WRONG_SRC_ID;
285 286 }
286 287
287 288 return status;
288 289 }
289 290
290 291 int tc_check_length( unsigned char packetSubType, unsigned int length )
291 292 {
292 293 /** This function checks that the subtype and the length are compliant.
293 294 *
294 295 * @param packetSubType is the subtype to check.
295 296 * @param length is the length to check.
296 297 *
297 298 * @return Status code CCSDS_TM_VALID or ILL_TYPE.
298 299 *
299 300 */
300 301
301 302 int status;
302 303
303 304 status = LFR_SUCCESSFUL;
304 305
305 306 switch(packetSubType)
306 307 {
307 308 case TC_SUBTYPE_RESET:
308 309 if (length!=(TC_LEN_RESET-CCSDS_TC_TM_PACKET_OFFSET)) {
309 310 status = WRONG_LEN_PKT;
310 311 }
311 312 else {
312 313 status = CCSDS_TM_VALID;
313 314 }
314 315 break;
315 316 case TC_SUBTYPE_LOAD_COMM:
316 317 if (length!=(TC_LEN_LOAD_COMM-CCSDS_TC_TM_PACKET_OFFSET)) {
317 318 status = WRONG_LEN_PKT;
318 319 }
319 320 else {
320 321 status = CCSDS_TM_VALID;
321 322 }
322 323 break;
323 324 case TC_SUBTYPE_LOAD_NORM:
324 325 if (length!=(TC_LEN_LOAD_NORM-CCSDS_TC_TM_PACKET_OFFSET)) {
325 326 status = WRONG_LEN_PKT;
326 327 }
327 328 else {
328 329 status = CCSDS_TM_VALID;
329 330 }
330 331 break;
331 332 case TC_SUBTYPE_LOAD_BURST:
332 333 if (length!=(TC_LEN_LOAD_BURST-CCSDS_TC_TM_PACKET_OFFSET)) {
333 334 status = WRONG_LEN_PKT;
334 335 }
335 336 else {
336 337 status = CCSDS_TM_VALID;
337 338 }
338 339 break;
339 340 case TC_SUBTYPE_LOAD_SBM1:
340 341 if (length!=(TC_LEN_LOAD_SBM1-CCSDS_TC_TM_PACKET_OFFSET)) {
341 342 status = WRONG_LEN_PKT;
342 343 }
343 344 else {
344 345 status = CCSDS_TM_VALID;
345 346 }
346 347 break;
347 348 case TC_SUBTYPE_LOAD_SBM2:
348 349 if (length!=(TC_LEN_LOAD_SBM2-CCSDS_TC_TM_PACKET_OFFSET)) {
349 350 status = WRONG_LEN_PKT;
350 351 }
351 352 else {
352 353 status = CCSDS_TM_VALID;
353 354 }
354 355 break;
355 356 case TC_SUBTYPE_DUMP:
356 357 if (length!=(TC_LEN_DUMP-CCSDS_TC_TM_PACKET_OFFSET)) {
357 358 status = WRONG_LEN_PKT;
358 359 }
359 360 else {
360 361 status = CCSDS_TM_VALID;
361 362 }
362 363 break;
363 364 case TC_SUBTYPE_ENTER:
364 365 if (length!=(TC_LEN_ENTER-CCSDS_TC_TM_PACKET_OFFSET)) {
365 366 status = WRONG_LEN_PKT;
366 367 }
367 368 else {
368 369 status = CCSDS_TM_VALID;
369 370 }
370 371 break;
371 372 case TC_SUBTYPE_UPDT_INFO:
372 373 if (length!=(TC_LEN_UPDT_INFO-CCSDS_TC_TM_PACKET_OFFSET)) {
373 374 status = WRONG_LEN_PKT;
374 375 }
375 376 else {
376 377 status = CCSDS_TM_VALID;
377 378 }
378 379 break;
379 380 case TC_SUBTYPE_EN_CAL:
380 381 if (length!=(TC_LEN_EN_CAL-CCSDS_TC_TM_PACKET_OFFSET)) {
381 382 status = WRONG_LEN_PKT;
382 383 }
383 384 else {
384 385 status = CCSDS_TM_VALID;
385 386 }
386 387 break;
387 388 case TC_SUBTYPE_DIS_CAL:
388 389 if (length!=(TC_LEN_DIS_CAL-CCSDS_TC_TM_PACKET_OFFSET)) {
389 390 status = WRONG_LEN_PKT;
390 391 }
391 392 else {
392 393 status = CCSDS_TM_VALID;
393 394 }
394 395 break;
395 396 case TC_SUBTYPE_LOAD_K:
396 397 if (length!=(TC_LEN_LOAD_K-CCSDS_TC_TM_PACKET_OFFSET)) {
397 398 status = WRONG_LEN_PKT;
398 399 }
399 400 else {
400 401 status = CCSDS_TM_VALID;
401 402 }
402 403 break;
403 404 case TC_SUBTYPE_DUMP_K:
404 405 if (length!=(TC_LEN_DUMP_K-CCSDS_TC_TM_PACKET_OFFSET)) {
405 406 status = WRONG_LEN_PKT;
406 407 }
407 408 else {
408 409 status = CCSDS_TM_VALID;
409 410 }
410 411 break;
411 412 case TC_SUBTYPE_LOAD_FBINS:
412 413 if (length!=(TC_LEN_LOAD_FBINS-CCSDS_TC_TM_PACKET_OFFSET)) {
413 414 status = WRONG_LEN_PKT;
414 415 }
415 416 else {
416 417 status = CCSDS_TM_VALID;
417 418 }
418 419 break;
419 420 case TC_SUBTYPE_LOAD_FILTER_PAR:
420 421 if (length!=(TC_LEN_LOAD_FILTER_PAR-CCSDS_TC_TM_PACKET_OFFSET)) {
421 422 status = WRONG_LEN_PKT;
422 423 }
423 424 else {
424 425 status = CCSDS_TM_VALID;
425 426 }
426 427 break;
427 428 case TC_SUBTYPE_UPDT_TIME:
428 429 if (length!=(TC_LEN_UPDT_TIME-CCSDS_TC_TM_PACKET_OFFSET)) {
429 430 status = WRONG_LEN_PKT;
430 431 }
431 432 else {
432 433 status = CCSDS_TM_VALID;
433 434 }
434 435 break;
435 436 default: // if the subtype is not a legal value, return ILL_SUBTYPE
436 437 status = ILL_SUBTYPE;
437 438 break ;
438 439 }
439 440
440 441 return status;
441 442 }
442 443
443 444 int tc_check_crc( ccsdsTelecommandPacket_t * TCPacket, unsigned int length, unsigned char *computed_CRC )
444 445 {
445 446 /** This function checks the CRC validity of the corresponding TeleCommand packet.
446 447 *
447 448 * @param TCPacket points to the TeleCommand packet to check.
448 449 * @param length is the length of the TC packet.
449 450 *
450 451 * @return Status code CCSDS_TM_VALID or INCOR_CHECKSUM.
451 452 *
452 453 */
453 454
454 455 int status;
455 456 unsigned char * CCSDSContent;
456 457
457 458 CCSDSContent = (unsigned char*) TCPacket->packetID;
458 GetCRCAsTwoBytes(CCSDSContent, computed_CRC, length + CCSDS_TC_TM_PACKET_OFFSET - 2); // 2 CRC bytes removed from the calculation of the CRC
459 GetCRCAsTwoBytes(CCSDSContent, computed_CRC, length + CCSDS_TC_TM_PACKET_OFFSET - BYTES_PER_CRC); // 2 CRC bytes removed from the calculation of the CRC
459 460
460 if (computed_CRC[0] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -2]) {
461 if (computed_CRC[0] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET - BYTES_PER_CRC]) {
461 462 status = INCOR_CHECKSUM;
462 463 }
463 464 else if (computed_CRC[1] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -1]) {
464 465 status = INCOR_CHECKSUM;
465 466 }
466 467 else {
467 468 status = CCSDS_TM_VALID;
468 469 }
469 470
470 471 return status;
471 472 }
472 473
473 474
474 475
Show file before | Show file after | Hide comments
@@ -1,1645 +1,1654
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 unsigned char time[6];
35 unsigned char time[BYTES_PER_TIME];
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( &TC, 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 result = action_load_kcoefficients( &TC, queue_snd_id, time );
116 116 close_action( &TC, result, queue_snd_id );
117 117 break;
118 118 case TC_SUBTYPE_DUMP_K:
119 119 result = action_dump_kcoefficients( &TC, queue_snd_id, time );
120 120 close_action( &TC, result, queue_snd_id );
121 121 break;
122 122 case TC_SUBTYPE_LOAD_FBINS:
123 123 result = action_load_fbins_mask( &TC, queue_snd_id, time );
124 124 close_action( &TC, result, queue_snd_id );
125 125 break;
126 126 case TC_SUBTYPE_LOAD_FILTER_PAR:
127 127 result = action_load_filter_par( &TC, queue_snd_id, time );
128 128 close_action( &TC, result, queue_snd_id );
129 129 break;
130 130 case TC_SUBTYPE_UPDT_TIME:
131 131 result = action_update_time( &TC );
132 132 close_action( &TC, result, queue_snd_id );
133 133 break;
134 134 default:
135 135 break;
136 136 }
137 137 }
138 138 }
139 139 }
140 140
141 141 //***********
142 142 // TC ACTIONS
143 143
144 144 int action_reset(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
145 145 {
146 146 /** This function executes specific actions when a TC_LFR_RESET TeleCommand has been received.
147 147 *
148 148 * @param TC points to the TeleCommand packet that is being processed
149 149 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
150 150 *
151 151 */
152 152
153 153 PRINTF("this is the end!!!\n");
154 154 exit(0);
155 155
156 156 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
157 157
158 158 return LFR_DEFAULT;
159 159 }
160 160
161 161 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
162 162 {
163 163 /** This function executes specific actions when a TC_LFR_ENTER_MODE TeleCommand has been received.
164 164 *
165 165 * @param TC points to the TeleCommand packet that is being processed
166 166 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
167 167 *
168 168 */
169 169
170 170 rtems_status_code status;
171 171 unsigned char requestedMode;
172 172 unsigned int *transitionCoarseTime_ptr;
173 173 unsigned int transitionCoarseTime;
174 174 unsigned char * bytePosPtr;
175 175
176 176 bytePosPtr = (unsigned char *) &TC->packetID;
177 177
178 178 requestedMode = bytePosPtr[ BYTE_POS_CP_MODE_LFR_SET ];
179 179 transitionCoarseTime_ptr = (unsigned int *) ( &bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME ] );
180 transitionCoarseTime = (*transitionCoarseTime_ptr) & 0x7fffffff;
180 transitionCoarseTime = (*transitionCoarseTime_ptr) & COARSE_TIME_MASK;
181 181
182 182 status = check_mode_value( requestedMode );
183 183
184 184 if ( status != LFR_SUCCESSFUL ) // the mode value is inconsistent
185 185 {
186 186 send_tm_lfr_tc_exe_inconsistent( TC, queue_id, BYTE_POS_CP_MODE_LFR_SET, requestedMode );
187 187 }
188 188
189 189 else // the mode value is valid, check the transition
190 190 {
191 191 status = check_mode_transition(requestedMode);
192 192 if (status != LFR_SUCCESSFUL)
193 193 {
194 194 PRINTF("ERR *** in action_enter_mode *** check_mode_transition\n")
195 195 send_tm_lfr_tc_exe_not_executable( TC, queue_id );
196 196 }
197 197 }
198 198
199 199 if ( status == LFR_SUCCESSFUL ) // the transition is valid, check the date
200 200 {
201 201 status = check_transition_date( transitionCoarseTime );
202 202 if (status != LFR_SUCCESSFUL)
203 203 {
204 204 PRINTF("ERR *** in action_enter_mode *** check_transition_date\n");
205 205 send_tm_lfr_tc_exe_not_executable(TC, queue_id );
206 206 }
207 207 }
208 208
209 209 if ( status == LFR_SUCCESSFUL ) // the date is valid, enter the mode
210 210 {
211 211 PRINTF1("OK *** in action_enter_mode *** enter mode %d\n", requestedMode);
212 212
213 213 switch(requestedMode)
214 214 {
215 215 case LFR_MODE_STANDBY:
216 216 status = enter_mode_standby();
217 217 break;
218 218 case LFR_MODE_NORMAL:
219 219 status = enter_mode_normal( transitionCoarseTime );
220 220 break;
221 221 case LFR_MODE_BURST:
222 222 status = enter_mode_burst( transitionCoarseTime );
223 223 break;
224 224 case LFR_MODE_SBM1:
225 225 status = enter_mode_sbm1( transitionCoarseTime );
226 226 break;
227 227 case LFR_MODE_SBM2:
228 228 status = enter_mode_sbm2( transitionCoarseTime );
229 229 break;
230 230 default:
231 231 break;
232 232 }
233 233
234 234 if (status != RTEMS_SUCCESSFUL)
235 235 {
236 236 status = LFR_EXE_ERROR;
237 237 }
238 238 }
239 239
240 240 return status;
241 241 }
242 242
243 243 int action_update_info(ccsdsTelecommandPacket_t *TC, rtems_id queue_id)
244 244 {
245 245 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
246 246 *
247 247 * @param TC points to the TeleCommand packet that is being processed
248 248 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
249 249 *
250 250 * @return LFR directive status code:
251 251 * - LFR_DEFAULT
252 252 * - LFR_SUCCESSFUL
253 253 *
254 254 */
255 255
256 256 unsigned int val;
257 257 int result;
258 258 unsigned int status;
259 259 unsigned char mode;
260 260 unsigned char * bytePosPtr;
261 261
262 262 bytePosPtr = (unsigned char *) &TC->packetID;
263 263
264 264 // check LFR mode
265 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET5 ] & 0x1e) >> 1;
265 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET5 ] & BITS_LFR_MODE) >> SHIFT_LFR_MODE;
266 266 status = check_update_info_hk_lfr_mode( mode );
267 267 if (status == LFR_SUCCESSFUL) // check TDS mode
268 268 {
269 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0xf0) >> 4;
269 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & BITS_TDS_MODE) >> SHIFT_TDS_MODE;
270 270 status = check_update_info_hk_tds_mode( mode );
271 271 }
272 272 if (status == LFR_SUCCESSFUL) // check THR mode
273 273 {
274 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0x0f);
274 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & BITS_THR_MODE);
275 275 status = check_update_info_hk_thr_mode( mode );
276 276 }
277 277 if (status == LFR_SUCCESSFUL) // if the parameter check is successful
278 278 {
279 val = housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * 256
279 val = (housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * CONST_256)
280 280 + housekeeping_packet.hk_lfr_update_info_tc_cnt[1];
281 281 val++;
282 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> 8);
282 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
283 283 housekeeping_packet.hk_lfr_update_info_tc_cnt[1] = (unsigned char) (val);
284 284 }
285 285
286 286 // pa_bia_status_info
287 287 // => pa_bia_mode_mux_set 3 bits
288 288 // => pa_bia_mode_hv_enabled 1 bit
289 289 // => pa_bia_mode_bias1_enabled 1 bit
290 290 // => pa_bia_mode_bias2_enabled 1 bit
291 291 // => pa_bia_mode_bias3_enabled 1 bit
292 292 // => pa_bia_on_off (cp_dpu_bias_on_off)
293 pa_bia_status_info = bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET2 ] & 0xfe; // [1111 1110]
293 pa_bia_status_info = bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET2 ] & BITS_BIA; // [1111 1110]
294 294 pa_bia_status_info = pa_bia_status_info
295 | (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET1 ] & 0x1);
295 | (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET1 ] & 1);
296 296
297 297 // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets)
298 298
299 299 cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ];
300 300 getReactionWheelsFrequencies( TC );
301 301 build_sy_lfr_rw_masks();
302 302
303 303 // once the masks are built, they have to be merged with the fbins_mask
304 304 merge_fbins_masks();
305 305
306 306 result = status;
307 307
308 308 return result;
309 309 }
310 310
311 311 int action_enable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
312 312 {
313 313 /** This function executes specific actions when a TC_LFR_ENABLE_CALIBRATION TeleCommand has been received.
314 314 *
315 315 * @param TC points to the TeleCommand packet that is being processed
316 316 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
317 317 *
318 318 */
319 319
320 320 int result;
321 321
322 322 result = LFR_DEFAULT;
323 323
324 324 setCalibration( true );
325 325
326 326 result = LFR_SUCCESSFUL;
327 327
328 328 return result;
329 329 }
330 330
331 331 int action_disable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
332 332 {
333 333 /** This function executes specific actions when a TC_LFR_DISABLE_CALIBRATION TeleCommand has been received.
334 334 *
335 335 * @param TC points to the TeleCommand packet that is being processed
336 336 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
337 337 *
338 338 */
339 339
340 340 int result;
341 341
342 342 result = LFR_DEFAULT;
343 343
344 344 setCalibration( false );
345 345
346 346 result = LFR_SUCCESSFUL;
347 347
348 348 return result;
349 349 }
350 350
351 351 int action_update_time(ccsdsTelecommandPacket_t *TC)
352 352 {
353 353 /** This function executes specific actions when a TC_LFR_UPDATE_TIME TeleCommand has been received.
354 354 *
355 355 * @param TC points to the TeleCommand packet that is being processed
356 356 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
357 357 *
358 358 * @return LFR_SUCCESSFUL
359 359 *
360 360 */
361 361
362 362 unsigned int val;
363 363
364 time_management_regs->coarse_time_load = (TC->dataAndCRC[0] << 24)
365 + (TC->dataAndCRC[1] << 16)
366 + (TC->dataAndCRC[2] << 8)
367 + TC->dataAndCRC[3];
364 time_management_regs->coarse_time_load = (TC->dataAndCRC[BYTE_0] << SHIFT_3_BYTES)
365 + (TC->dataAndCRC[BYTE_1] << SHIFT_2_BYTES)
366 + (TC->dataAndCRC[BYTE_2] << SHIFT_1_BYTE)
367 + TC->dataAndCRC[BYTE_3];
368 368
369 val = housekeeping_packet.hk_lfr_update_time_tc_cnt[0] * 256
369 val = (housekeeping_packet.hk_lfr_update_time_tc_cnt[0] * CONST_256)
370 370 + housekeeping_packet.hk_lfr_update_time_tc_cnt[1];
371 371 val++;
372 housekeeping_packet.hk_lfr_update_time_tc_cnt[0] = (unsigned char) (val >> 8);
372 housekeeping_packet.hk_lfr_update_time_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
373 373 housekeeping_packet.hk_lfr_update_time_tc_cnt[1] = (unsigned char) (val);
374 374
375 375 oneTcLfrUpdateTimeReceived = 1;
376 376
377 377 return LFR_SUCCESSFUL;
378 378 }
379 379
380 380 //*******************
381 381 // ENTERING THE MODES
382 382 int check_mode_value( unsigned char requestedMode )
383 383 {
384 384 int status;
385 385
386 386 if ( (requestedMode != LFR_MODE_STANDBY)
387 387 && (requestedMode != LFR_MODE_NORMAL) && (requestedMode != LFR_MODE_BURST)
388 388 && (requestedMode != LFR_MODE_SBM1) && (requestedMode != LFR_MODE_SBM2) )
389 389 {
390 390 status = LFR_DEFAULT;
391 391 }
392 392 else
393 393 {
394 394 status = LFR_SUCCESSFUL;
395 395 }
396 396
397 397 return status;
398 398 }
399 399
400 400 int check_mode_transition( unsigned char requestedMode )
401 401 {
402 402 /** This function checks the validity of the transition requested by the TC_LFR_ENTER_MODE.
403 403 *
404 404 * @param requestedMode is the mode requested by the TC_LFR_ENTER_MODE
405 405 *
406 406 * @return LFR directive status codes:
407 407 * - LFR_SUCCESSFUL - the transition is authorized
408 408 * - LFR_DEFAULT - the transition is not authorized
409 409 *
410 410 */
411 411
412 412 int status;
413 413
414 414 switch (requestedMode)
415 415 {
416 416 case LFR_MODE_STANDBY:
417 417 if ( lfrCurrentMode == LFR_MODE_STANDBY ) {
418 418 status = LFR_DEFAULT;
419 419 }
420 420 else
421 421 {
422 422 status = LFR_SUCCESSFUL;
423 423 }
424 424 break;
425 425 case LFR_MODE_NORMAL:
426 426 if ( lfrCurrentMode == LFR_MODE_NORMAL ) {
427 427 status = LFR_DEFAULT;
428 428 }
429 429 else {
430 430 status = LFR_SUCCESSFUL;
431 431 }
432 432 break;
433 433 case LFR_MODE_BURST:
434 434 if ( lfrCurrentMode == LFR_MODE_BURST ) {
435 435 status = LFR_DEFAULT;
436 436 }
437 437 else {
438 438 status = LFR_SUCCESSFUL;
439 439 }
440 440 break;
441 441 case LFR_MODE_SBM1:
442 442 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
443 443 status = LFR_DEFAULT;
444 444 }
445 445 else {
446 446 status = LFR_SUCCESSFUL;
447 447 }
448 448 break;
449 449 case LFR_MODE_SBM2:
450 450 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
451 451 status = LFR_DEFAULT;
452 452 }
453 453 else {
454 454 status = LFR_SUCCESSFUL;
455 455 }
456 456 break;
457 457 default:
458 458 status = LFR_DEFAULT;
459 459 break;
460 460 }
461 461
462 462 return status;
463 463 }
464 464
465 465 void update_last_valid_transition_date( unsigned int transitionCoarseTime )
466 466 {
467 467 if (transitionCoarseTime == 0)
468 468 {
469 469 lastValidEnterModeTime = time_management_regs->coarse_time + 1;
470 470 PRINTF1("lastValidEnterModeTime = 0x%x (transitionCoarseTime = 0 => coarse_time+1)\n", lastValidEnterModeTime);
471 471 }
472 472 else
473 473 {
474 474 lastValidEnterModeTime = transitionCoarseTime;
475 475 PRINTF1("lastValidEnterModeTime = 0x%x\n", transitionCoarseTime);
476 476 }
477 477 }
478 478
479 479 int check_transition_date( unsigned int transitionCoarseTime )
480 480 {
481 481 int status;
482 482 unsigned int localCoarseTime;
483 483 unsigned int deltaCoarseTime;
484 484
485 485 status = LFR_SUCCESSFUL;
486 486
487 487 if (transitionCoarseTime == 0) // transition time = 0 means an instant transition
488 488 {
489 489 status = LFR_SUCCESSFUL;
490 490 }
491 491 else
492 492 {
493 localCoarseTime = time_management_regs->coarse_time & 0x7fffffff;
493 localCoarseTime = time_management_regs->coarse_time & COARSE_TIME_MASK;
494 494
495 495 PRINTF2("localTime = %x, transitionTime = %x\n", localCoarseTime, transitionCoarseTime);
496 496
497 497 if ( transitionCoarseTime <= localCoarseTime ) // SSS-CP-EQS-322
498 498 {
499 499 status = LFR_DEFAULT;
500 500 PRINTF("ERR *** in check_transition_date *** transitionCoarseTime <= localCoarseTime\n");
501 501 }
502 502
503 503 if (status == LFR_SUCCESSFUL)
504 504 {
505 505 deltaCoarseTime = transitionCoarseTime - localCoarseTime;
506 if ( deltaCoarseTime > 3 ) // SSS-CP-EQS-323
506 if ( deltaCoarseTime > MAX_DELTA_COARSE_TIME ) // SSS-CP-EQS-323
507 507 {
508 508 status = LFR_DEFAULT;
509 509 PRINTF1("ERR *** in check_transition_date *** deltaCoarseTime = %x\n", deltaCoarseTime)
510 510 }
511 511 }
512 512 }
513 513
514 514 return status;
515 515 }
516 516
517 517 int restart_asm_activities( unsigned char lfrRequestedMode )
518 518 {
519 519 rtems_status_code status;
520 520
521 521 status = stop_spectral_matrices();
522 522
523 523 thisIsAnASMRestart = 1;
524 524
525 525 status = restart_asm_tasks( lfrRequestedMode );
526 526
527 527 launch_spectral_matrix();
528 528
529 529 return status;
530 530 }
531 531
532 532 int stop_spectral_matrices( void )
533 533 {
534 534 /** This function stops and restarts the current mode average spectral matrices activities.
535 535 *
536 536 * @return RTEMS directive status codes:
537 537 * - RTEMS_SUCCESSFUL - task restarted successfully
538 538 * - RTEMS_INVALID_ID - task id invalid
539 539 * - RTEMS_ALREADY_SUSPENDED - task already suspended
540 540 *
541 541 */
542 542
543 543 rtems_status_code status;
544 544
545 545 status = RTEMS_SUCCESSFUL;
546 546
547 547 // (1) mask interruptions
548 548 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // mask spectral matrix interrupt
549 549
550 550 // (2) reset spectral matrices registers
551 551 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
552 552 reset_sm_status();
553 553
554 554 // (3) clear interruptions
555 555 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
556 556
557 557 // suspend several tasks
558 558 if (lfrCurrentMode != LFR_MODE_STANDBY) {
559 559 status = suspend_asm_tasks();
560 560 }
561 561
562 562 if (status != RTEMS_SUCCESSFUL)
563 563 {
564 564 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
565 565 }
566 566
567 567 return status;
568 568 }
569 569
570 570 int stop_current_mode( void )
571 571 {
572 572 /** This function stops the current mode by masking interrupt lines and suspending science tasks.
573 573 *
574 574 * @return RTEMS directive status codes:
575 575 * - RTEMS_SUCCESSFUL - task restarted successfully
576 576 * - RTEMS_INVALID_ID - task id invalid
577 577 * - RTEMS_ALREADY_SUSPENDED - task already suspended
578 578 *
579 579 */
580 580
581 581 rtems_status_code status;
582 582
583 583 status = RTEMS_SUCCESSFUL;
584 584
585 585 // (1) mask interruptions
586 586 LEON_Mask_interrupt( IRQ_WAVEFORM_PICKER ); // mask waveform picker interrupt
587 587 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
588 588
589 589 // (2) reset waveform picker registers
590 590 reset_wfp_burst_enable(); // reset burst and enable bits
591 591 reset_wfp_status(); // reset all the status bits
592 592
593 593 // (3) reset spectral matrices registers
594 594 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
595 595 reset_sm_status();
596 596
597 597 // reset lfr VHDL module
598 598 reset_lfr();
599 599
600 600 reset_extractSWF(); // reset the extractSWF flag to false
601 601
602 602 // (4) clear interruptions
603 603 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER ); // clear waveform picker interrupt
604 604 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
605 605
606 606 // suspend several tasks
607 607 if (lfrCurrentMode != LFR_MODE_STANDBY) {
608 608 status = suspend_science_tasks();
609 609 }
610 610
611 611 if (status != RTEMS_SUCCESSFUL)
612 612 {
613 613 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
614 614 }
615 615
616 616 return status;
617 617 }
618 618
619 619 int enter_mode_standby( void )
620 620 {
621 621 /** This function is used to put LFR in the STANDBY mode.
622 622 *
623 623 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
624 624 *
625 625 * @return RTEMS directive status codes:
626 626 * - RTEMS_SUCCESSFUL - task restarted successfully
627 627 * - RTEMS_INVALID_ID - task id invalid
628 628 * - RTEMS_INCORRECT_STATE - task never started
629 629 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
630 630 *
631 631 * The STANDBY mode does not depends on a specific transition date, the effect of the TC_LFR_ENTER_MODE
632 632 * is immediate.
633 633 *
634 634 */
635 635
636 636 int status;
637 637
638 638 status = stop_current_mode(); // STOP THE CURRENT MODE
639 639
640 640 #ifdef PRINT_TASK_STATISTICS
641 641 rtems_cpu_usage_report();
642 642 #endif
643 643
644 644 #ifdef PRINT_STACK_REPORT
645 645 PRINTF("stack report selected\n")
646 646 rtems_stack_checker_report_usage();
647 647 #endif
648 648
649 649 return status;
650 650 }
651 651
652 652 int enter_mode_normal( unsigned int transitionCoarseTime )
653 653 {
654 654 /** This function is used to start the NORMAL mode.
655 655 *
656 656 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
657 657 *
658 658 * @return RTEMS directive status codes:
659 659 * - RTEMS_SUCCESSFUL - task restarted successfully
660 660 * - RTEMS_INVALID_ID - task id invalid
661 661 * - RTEMS_INCORRECT_STATE - task never started
662 662 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
663 663 *
664 664 * The way the NORMAL mode is started depends on the LFR current mode. If LFR is in SBM1 or SBM2,
665 665 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected.
666 666 *
667 667 */
668 668
669 669 int status;
670 670
671 671 #ifdef PRINT_TASK_STATISTICS
672 672 rtems_cpu_usage_reset();
673 673 #endif
674 674
675 675 status = RTEMS_UNSATISFIED;
676 676
677 677 switch( lfrCurrentMode )
678 678 {
679 679 case LFR_MODE_STANDBY:
680 680 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart science tasks
681 681 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
682 682 {
683 683 launch_spectral_matrix( );
684 684 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
685 685 }
686 686 break;
687 687 case LFR_MODE_BURST:
688 688 status = stop_current_mode(); // stop the current mode
689 689 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart the science tasks
690 690 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
691 691 {
692 692 launch_spectral_matrix( );
693 693 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
694 694 }
695 695 break;
696 696 case LFR_MODE_SBM1:
697 697 status = restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
698 698 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
699 699 update_last_valid_transition_date( transitionCoarseTime );
700 700 break;
701 701 case LFR_MODE_SBM2:
702 702 status = restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
703 703 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
704 704 update_last_valid_transition_date( transitionCoarseTime );
705 705 break;
706 706 default:
707 707 break;
708 708 }
709 709
710 710 if (status != RTEMS_SUCCESSFUL)
711 711 {
712 712 PRINTF1("ERR *** in enter_mode_normal *** status = %d\n", status)
713 713 status = RTEMS_UNSATISFIED;
714 714 }
715 715
716 716 return status;
717 717 }
718 718
719 719 int enter_mode_burst( unsigned int transitionCoarseTime )
720 720 {
721 721 /** This function is used to start the BURST mode.
722 722 *
723 723 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
724 724 *
725 725 * @return RTEMS directive status codes:
726 726 * - RTEMS_SUCCESSFUL - task restarted successfully
727 727 * - RTEMS_INVALID_ID - task id invalid
728 728 * - RTEMS_INCORRECT_STATE - task never started
729 729 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
730 730 *
731 731 * The way the BURST mode is started does not depend on the LFR current mode.
732 732 *
733 733 */
734 734
735 735
736 736 int status;
737 737
738 738 #ifdef PRINT_TASK_STATISTICS
739 739 rtems_cpu_usage_reset();
740 740 #endif
741 741
742 742 status = stop_current_mode(); // stop the current mode
743 743 status = restart_science_tasks( LFR_MODE_BURST ); // restart the science tasks
744 744 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
745 745 {
746 746 launch_spectral_matrix( );
747 747 launch_waveform_picker( LFR_MODE_BURST, transitionCoarseTime );
748 748 }
749 749
750 750 if (status != RTEMS_SUCCESSFUL)
751 751 {
752 752 PRINTF1("ERR *** in enter_mode_burst *** status = %d\n", status)
753 753 status = RTEMS_UNSATISFIED;
754 754 }
755 755
756 756 return status;
757 757 }
758 758
759 759 int enter_mode_sbm1( unsigned int transitionCoarseTime )
760 760 {
761 761 /** This function is used to start the SBM1 mode.
762 762 *
763 763 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
764 764 *
765 765 * @return RTEMS directive status codes:
766 766 * - RTEMS_SUCCESSFUL - task restarted successfully
767 767 * - RTEMS_INVALID_ID - task id invalid
768 768 * - RTEMS_INCORRECT_STATE - task never started
769 769 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
770 770 *
771 771 * The way the SBM1 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM2,
772 772 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
773 773 * cases, the acquisition is completely restarted.
774 774 *
775 775 */
776 776
777 777 int status;
778 778
779 779 #ifdef PRINT_TASK_STATISTICS
780 780 rtems_cpu_usage_reset();
781 781 #endif
782 782
783 783 status = RTEMS_UNSATISFIED;
784 784
785 785 switch( lfrCurrentMode )
786 786 {
787 787 case LFR_MODE_STANDBY:
788 788 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart science tasks
789 789 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
790 790 {
791 791 launch_spectral_matrix( );
792 792 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
793 793 }
794 794 break;
795 795 case LFR_MODE_NORMAL: // lfrCurrentMode will be updated after the execution of close_action
796 796 status = restart_asm_activities( LFR_MODE_SBM1 );
797 797 status = LFR_SUCCESSFUL;
798 798 update_last_valid_transition_date( transitionCoarseTime );
799 799 break;
800 800 case LFR_MODE_BURST:
801 801 status = stop_current_mode(); // stop the current mode
802 802 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart the science tasks
803 803 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
804 804 {
805 805 launch_spectral_matrix( );
806 806 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
807 807 }
808 808 break;
809 809 case LFR_MODE_SBM2:
810 810 status = restart_asm_activities( LFR_MODE_SBM1 );
811 811 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
812 812 update_last_valid_transition_date( transitionCoarseTime );
813 813 break;
814 814 default:
815 815 break;
816 816 }
817 817
818 818 if (status != RTEMS_SUCCESSFUL)
819 819 {
820 820 PRINTF1("ERR *** in enter_mode_sbm1 *** status = %d\n", status);
821 821 status = RTEMS_UNSATISFIED;
822 822 }
823 823
824 824 return status;
825 825 }
826 826
827 827 int enter_mode_sbm2( unsigned int transitionCoarseTime )
828 828 {
829 829 /** This function is used to start the SBM2 mode.
830 830 *
831 831 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
832 832 *
833 833 * @return RTEMS directive status codes:
834 834 * - RTEMS_SUCCESSFUL - task restarted successfully
835 835 * - RTEMS_INVALID_ID - task id invalid
836 836 * - RTEMS_INCORRECT_STATE - task never started
837 837 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
838 838 *
839 839 * The way the SBM2 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM1,
840 840 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
841 841 * cases, the acquisition is completely restarted.
842 842 *
843 843 */
844 844
845 845 int status;
846 846
847 847 #ifdef PRINT_TASK_STATISTICS
848 848 rtems_cpu_usage_reset();
849 849 #endif
850 850
851 851 status = RTEMS_UNSATISFIED;
852 852
853 853 switch( lfrCurrentMode )
854 854 {
855 855 case LFR_MODE_STANDBY:
856 856 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart science tasks
857 857 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
858 858 {
859 859 launch_spectral_matrix( );
860 860 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
861 861 }
862 862 break;
863 863 case LFR_MODE_NORMAL:
864 864 status = restart_asm_activities( LFR_MODE_SBM2 );
865 865 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
866 866 update_last_valid_transition_date( transitionCoarseTime );
867 867 break;
868 868 case LFR_MODE_BURST:
869 869 status = stop_current_mode(); // stop the current mode
870 870 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart the science tasks
871 871 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
872 872 {
873 873 launch_spectral_matrix( );
874 874 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
875 875 }
876 876 break;
877 877 case LFR_MODE_SBM1:
878 878 status = restart_asm_activities( LFR_MODE_SBM2 );
879 879 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
880 880 update_last_valid_transition_date( transitionCoarseTime );
881 881 break;
882 882 default:
883 883 break;
884 884 }
885 885
886 886 if (status != RTEMS_SUCCESSFUL)
887 887 {
888 888 PRINTF1("ERR *** in enter_mode_sbm2 *** status = %d\n", status)
889 889 status = RTEMS_UNSATISFIED;
890 890 }
891 891
892 892 return status;
893 893 }
894 894
895 895 int restart_science_tasks( unsigned char lfrRequestedMode )
896 896 {
897 897 /** This function is used to restart all science tasks.
898 898 *
899 899 * @return RTEMS directive status codes:
900 900 * - RTEMS_SUCCESSFUL - task restarted successfully
901 901 * - RTEMS_INVALID_ID - task id invalid
902 902 * - RTEMS_INCORRECT_STATE - task never started
903 903 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
904 904 *
905 905 * Science tasks are AVF0, PRC0, WFRM, CWF3, CW2, CWF1
906 906 *
907 907 */
908 908
909 rtems_status_code status[10];
909 rtems_status_code status[NB_SCIENCE_TASKS];
910 910 rtems_status_code ret;
911 911
912 912 ret = RTEMS_SUCCESSFUL;
913 913
914 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
915 if (status[0] != RTEMS_SUCCESSFUL)
914 status[STATUS_0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
915 if (status[STATUS_0] != RTEMS_SUCCESSFUL)
916 916 {
917 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
917 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[STATUS_0])
918 918 }
919 919
920 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
921 if (status[1] != RTEMS_SUCCESSFUL)
920 status[STATUS_1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
921 if (status[STATUS_1] != RTEMS_SUCCESSFUL)
922 922 {
923 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
923 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[STATUS_1])
924 924 }
925 925
926 status[2] = rtems_task_restart( Task_id[TASKID_WFRM],1 );
927 if (status[2] != RTEMS_SUCCESSFUL)
926 status[STATUS_2] = rtems_task_restart( Task_id[TASKID_WFRM],1 );
927 if (status[STATUS_2] != RTEMS_SUCCESSFUL)
928 928 {
929 PRINTF1("in restart_science_task *** WFRM ERR %d\n", status[2])
929 PRINTF1("in restart_science_task *** WFRM ERR %d\n", status[STATUS_2])
930 930 }
931 931
932 status[3] = rtems_task_restart( Task_id[TASKID_CWF3],1 );
933 if (status[3] != RTEMS_SUCCESSFUL)
932 status[STATUS_3] = rtems_task_restart( Task_id[TASKID_CWF3],1 );
933 if (status[STATUS_3] != RTEMS_SUCCESSFUL)
934 934 {
935 PRINTF1("in restart_science_task *** CWF3 ERR %d\n", status[3])
935 PRINTF1("in restart_science_task *** CWF3 ERR %d\n", status[STATUS_3])
936 936 }
937 937
938 status[4] = rtems_task_restart( Task_id[TASKID_CWF2],1 );
939 if (status[4] != RTEMS_SUCCESSFUL)
938 status[STATUS_4] = rtems_task_restart( Task_id[TASKID_CWF2],1 );
939 if (status[STATUS_4] != RTEMS_SUCCESSFUL)
940 940 {
941 PRINTF1("in restart_science_task *** CWF2 ERR %d\n", status[4])
941 PRINTF1("in restart_science_task *** CWF2 ERR %d\n", status[STATUS_4])
942 942 }
943 943
944 status[5] = rtems_task_restart( Task_id[TASKID_CWF1],1 );
945 if (status[5] != RTEMS_SUCCESSFUL)
944 status[STATUS_5] = rtems_task_restart( Task_id[TASKID_CWF1],1 );
945 if (status[STATUS_5] != RTEMS_SUCCESSFUL)
946 946 {
947 PRINTF1("in restart_science_task *** CWF1 ERR %d\n", status[5])
947 PRINTF1("in restart_science_task *** CWF1 ERR %d\n", status[STATUS_5])
948 948 }
949 949
950 status[6] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
951 if (status[6] != RTEMS_SUCCESSFUL)
950 status[STATUS_6] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
951 if (status[STATUS_6] != RTEMS_SUCCESSFUL)
952 952 {
953 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[6])
953 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[STATUS_6])
954 954 }
955 955
956 status[7] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
957 if (status[7] != RTEMS_SUCCESSFUL)
956 status[STATUS_7] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
957 if (status[STATUS_7] != RTEMS_SUCCESSFUL)
958 958 {
959 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[7])
959 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[STATUS_7])
960 960 }
961 961
962 status[8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
963 if (status[8] != RTEMS_SUCCESSFUL)
962 status[STATUS_8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
963 if (status[STATUS_8] != RTEMS_SUCCESSFUL)
964 964 {
965 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[8])
965 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[STATUS_8])
966 966 }
967 967
968 status[9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
969 if (status[9] != RTEMS_SUCCESSFUL)
968 status[STATUS_9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
969 if (status[STATUS_9] != RTEMS_SUCCESSFUL)
970 970 {
971 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[9])
971 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[STATUS_9])
972 972 }
973 973
974 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
975 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
976 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) ||
977 (status[6] != RTEMS_SUCCESSFUL) || (status[7] != RTEMS_SUCCESSFUL) ||
978 (status[8] != RTEMS_SUCCESSFUL) || (status[9] != RTEMS_SUCCESSFUL) )
974 if ( (status[STATUS_0] != RTEMS_SUCCESSFUL) || (status[STATUS_1] != RTEMS_SUCCESSFUL) ||
975 (status[STATUS_2] != RTEMS_SUCCESSFUL) || (status[STATUS_3] != RTEMS_SUCCESSFUL) ||
976 (status[STATUS_4] != RTEMS_SUCCESSFUL) || (status[STATUS_5] != RTEMS_SUCCESSFUL) ||
977 (status[STATUS_6] != RTEMS_SUCCESSFUL) || (status[STATUS_7] != RTEMS_SUCCESSFUL) ||
978 (status[STATUS_8] != RTEMS_SUCCESSFUL) || (status[STATUS_9] != RTEMS_SUCCESSFUL) )
979 979 {
980 980 ret = RTEMS_UNSATISFIED;
981 981 }
982 982
983 983 return ret;
984 984 }
985 985
986 986 int restart_asm_tasks( unsigned char lfrRequestedMode )
987 987 {
988 988 /** This function is used to restart average spectral matrices tasks.
989 989 *
990 990 * @return RTEMS directive status codes:
991 991 * - RTEMS_SUCCESSFUL - task restarted successfully
992 992 * - RTEMS_INVALID_ID - task id invalid
993 993 * - RTEMS_INCORRECT_STATE - task never started
994 994 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
995 995 *
996 996 * ASM tasks are AVF0, PRC0, AVF1, PRC1, AVF2 and PRC2
997 997 *
998 998 */
999 999
1000 rtems_status_code status[6];
1000 rtems_status_code status[NB_ASM_TASKS];
1001 1001 rtems_status_code ret;
1002 1002
1003 1003 ret = RTEMS_SUCCESSFUL;
1004 1004
1005 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
1006 if (status[0] != RTEMS_SUCCESSFUL)
1005 status[STATUS_0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
1006 if (status[STATUS_0] != RTEMS_SUCCESSFUL)
1007 1007 {
1008 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
1008 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[STATUS_0])
1009 1009 }
1010 1010
1011 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
1012 if (status[1] != RTEMS_SUCCESSFUL)
1011 status[STATUS_1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
1012 if (status[STATUS_1] != RTEMS_SUCCESSFUL)
1013 1013 {
1014 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
1014 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[STATUS_1])
1015 1015 }
1016 1016
1017 status[2] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
1018 if (status[2] != RTEMS_SUCCESSFUL)
1017 status[STATUS_2] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
1018 if (status[STATUS_2] != RTEMS_SUCCESSFUL)
1019 1019 {
1020 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[2])
1020 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[STATUS_2])
1021 1021 }
1022 1022
1023 status[3] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
1024 if (status[3] != RTEMS_SUCCESSFUL)
1023 status[STATUS_3] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
1024 if (status[STATUS_3] != RTEMS_SUCCESSFUL)
1025 1025 {
1026 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[3])
1026 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[STATUS_3])
1027 1027 }
1028 1028
1029 status[4] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
1030 if (status[4] != RTEMS_SUCCESSFUL)
1029 status[STATUS_4] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
1030 if (status[STATUS_4] != RTEMS_SUCCESSFUL)
1031 1031 {
1032 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[4])
1032 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[STATUS_4])
1033 1033 }
1034 1034
1035 status[5] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
1036 if (status[5] != RTEMS_SUCCESSFUL)
1035 status[STATUS_5] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
1036 if (status[STATUS_5] != RTEMS_SUCCESSFUL)
1037 1037 {
1038 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[5])
1038 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[STATUS_5])
1039 1039 }
1040 1040
1041 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
1042 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
1043 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) )
1041 if ( (status[STATUS_0] != RTEMS_SUCCESSFUL) || (status[STATUS_1] != RTEMS_SUCCESSFUL) ||
1042 (status[STATUS_2] != RTEMS_SUCCESSFUL) || (status[STATUS_3] != RTEMS_SUCCESSFUL) ||
1043 (status[STATUS_4] != RTEMS_SUCCESSFUL) || (status[STATUS_5] != RTEMS_SUCCESSFUL) )
1044 1044 {
1045 1045 ret = RTEMS_UNSATISFIED;
1046 1046 }
1047 1047
1048 1048 return ret;
1049 1049 }
1050 1050
1051 1051 int suspend_science_tasks( void )
1052 1052 {
1053 1053 /** This function suspends the science tasks.
1054 1054 *
1055 1055 * @return RTEMS directive status codes:
1056 1056 * - RTEMS_SUCCESSFUL - task restarted successfully
1057 1057 * - RTEMS_INVALID_ID - task id invalid
1058 1058 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1059 1059 *
1060 1060 */
1061 1061
1062 1062 rtems_status_code status;
1063 1063
1064 1064 PRINTF("in suspend_science_tasks\n")
1065 1065
1066 1066 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1067 1067 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1068 1068 {
1069 1069 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1070 1070 }
1071 1071 else
1072 1072 {
1073 1073 status = RTEMS_SUCCESSFUL;
1074 1074 }
1075 1075 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1076 1076 {
1077 1077 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1078 1078 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1079 1079 {
1080 1080 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1081 1081 }
1082 1082 else
1083 1083 {
1084 1084 status = RTEMS_SUCCESSFUL;
1085 1085 }
1086 1086 }
1087 1087 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1088 1088 {
1089 1089 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1090 1090 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1091 1091 {
1092 1092 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1093 1093 }
1094 1094 else
1095 1095 {
1096 1096 status = RTEMS_SUCCESSFUL;
1097 1097 }
1098 1098 }
1099 1099 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1100 1100 {
1101 1101 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1102 1102 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1103 1103 {
1104 1104 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1105 1105 }
1106 1106 else
1107 1107 {
1108 1108 status = RTEMS_SUCCESSFUL;
1109 1109 }
1110 1110 }
1111 1111 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1112 1112 {
1113 1113 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1114 1114 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1115 1115 {
1116 1116 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1117 1117 }
1118 1118 else
1119 1119 {
1120 1120 status = RTEMS_SUCCESSFUL;
1121 1121 }
1122 1122 }
1123 1123 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1124 1124 {
1125 1125 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1126 1126 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1127 1127 {
1128 1128 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1129 1129 }
1130 1130 else
1131 1131 {
1132 1132 status = RTEMS_SUCCESSFUL;
1133 1133 }
1134 1134 }
1135 1135 if (status == RTEMS_SUCCESSFUL) // suspend WFRM
1136 1136 {
1137 1137 status = rtems_task_suspend( Task_id[TASKID_WFRM] );
1138 1138 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1139 1139 {
1140 1140 PRINTF1("in suspend_science_task *** WFRM ERR %d\n", status)
1141 1141 }
1142 1142 else
1143 1143 {
1144 1144 status = RTEMS_SUCCESSFUL;
1145 1145 }
1146 1146 }
1147 1147 if (status == RTEMS_SUCCESSFUL) // suspend CWF3
1148 1148 {
1149 1149 status = rtems_task_suspend( Task_id[TASKID_CWF3] );
1150 1150 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1151 1151 {
1152 1152 PRINTF1("in suspend_science_task *** CWF3 ERR %d\n", status)
1153 1153 }
1154 1154 else
1155 1155 {
1156 1156 status = RTEMS_SUCCESSFUL;
1157 1157 }
1158 1158 }
1159 1159 if (status == RTEMS_SUCCESSFUL) // suspend CWF2
1160 1160 {
1161 1161 status = rtems_task_suspend( Task_id[TASKID_CWF2] );
1162 1162 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1163 1163 {
1164 1164 PRINTF1("in suspend_science_task *** CWF2 ERR %d\n", status)
1165 1165 }
1166 1166 else
1167 1167 {
1168 1168 status = RTEMS_SUCCESSFUL;
1169 1169 }
1170 1170 }
1171 1171 if (status == RTEMS_SUCCESSFUL) // suspend CWF1
1172 1172 {
1173 1173 status = rtems_task_suspend( Task_id[TASKID_CWF1] );
1174 1174 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1175 1175 {
1176 1176 PRINTF1("in suspend_science_task *** CWF1 ERR %d\n", status)
1177 1177 }
1178 1178 else
1179 1179 {
1180 1180 status = RTEMS_SUCCESSFUL;
1181 1181 }
1182 1182 }
1183 1183
1184 1184 return status;
1185 1185 }
1186 1186
1187 1187 int suspend_asm_tasks( void )
1188 1188 {
1189 1189 /** This function suspends the science tasks.
1190 1190 *
1191 1191 * @return RTEMS directive status codes:
1192 1192 * - RTEMS_SUCCESSFUL - task restarted successfully
1193 1193 * - RTEMS_INVALID_ID - task id invalid
1194 1194 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1195 1195 *
1196 1196 */
1197 1197
1198 1198 rtems_status_code status;
1199 1199
1200 1200 PRINTF("in suspend_science_tasks\n")
1201 1201
1202 1202 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1203 1203 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1204 1204 {
1205 1205 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1206 1206 }
1207 1207 else
1208 1208 {
1209 1209 status = RTEMS_SUCCESSFUL;
1210 1210 }
1211 1211
1212 1212 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1213 1213 {
1214 1214 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1215 1215 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1216 1216 {
1217 1217 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1218 1218 }
1219 1219 else
1220 1220 {
1221 1221 status = RTEMS_SUCCESSFUL;
1222 1222 }
1223 1223 }
1224 1224
1225 1225 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1226 1226 {
1227 1227 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1228 1228 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1229 1229 {
1230 1230 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1231 1231 }
1232 1232 else
1233 1233 {
1234 1234 status = RTEMS_SUCCESSFUL;
1235 1235 }
1236 1236 }
1237 1237
1238 1238 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1239 1239 {
1240 1240 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1241 1241 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1242 1242 {
1243 1243 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1244 1244 }
1245 1245 else
1246 1246 {
1247 1247 status = RTEMS_SUCCESSFUL;
1248 1248 }
1249 1249 }
1250 1250
1251 1251 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1252 1252 {
1253 1253 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1254 1254 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1255 1255 {
1256 1256 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1257 1257 }
1258 1258 else
1259 1259 {
1260 1260 status = RTEMS_SUCCESSFUL;
1261 1261 }
1262 1262 }
1263 1263
1264 1264 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1265 1265 {
1266 1266 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1267 1267 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1268 1268 {
1269 1269 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1270 1270 }
1271 1271 else
1272 1272 {
1273 1273 status = RTEMS_SUCCESSFUL;
1274 1274 }
1275 1275 }
1276 1276
1277 1277 return status;
1278 1278 }
1279 1279
1280 1280 void launch_waveform_picker( unsigned char mode, unsigned int transitionCoarseTime )
1281 1281 {
1282 1282
1283 1283 WFP_reset_current_ring_nodes();
1284 1284
1285 1285 reset_waveform_picker_regs();
1286 1286
1287 1287 set_wfp_burst_enable_register( mode );
1288 1288
1289 1289 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER );
1290 1290 LEON_Unmask_interrupt( IRQ_WAVEFORM_PICKER );
1291 1291
1292 1292 if (transitionCoarseTime == 0)
1293 1293 {
1294 1294 // instant transition means transition on the next valid date
1295 1295 // this is mandatory to have a good snapshot period and a good correction of the snapshot period
1296 1296 waveform_picker_regs->start_date = time_management_regs->coarse_time + 1;
1297 1297 }
1298 1298 else
1299 1299 {
1300 1300 waveform_picker_regs->start_date = transitionCoarseTime;
1301 1301 }
1302 1302
1303 1303 update_last_valid_transition_date(waveform_picker_regs->start_date);
1304 1304
1305 1305 }
1306 1306
1307 1307 void launch_spectral_matrix( void )
1308 1308 {
1309 1309 SM_reset_current_ring_nodes();
1310 1310
1311 1311 reset_spectral_matrix_regs();
1312 1312
1313 1313 reset_nb_sm();
1314 1314
1315 1315 set_sm_irq_onNewMatrix( 1 );
1316 1316
1317 1317 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX );
1318 1318 LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRIX );
1319 1319
1320 1320 }
1321 1321
1322 1322 void set_sm_irq_onNewMatrix( unsigned char value )
1323 1323 {
1324 1324 if (value == 1)
1325 1325 {
1326 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x01;
1326 spectral_matrix_regs->config = spectral_matrix_regs->config | BIT_IRQ_ON_NEW_MATRIX;
1327 1327 }
1328 1328 else
1329 1329 {
1330 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffe; // 1110
1330 spectral_matrix_regs->config = spectral_matrix_regs->config & MASK_IRQ_ON_NEW_MATRIX; // 1110
1331 1331 }
1332 1332 }
1333 1333
1334 1334 void set_sm_irq_onError( unsigned char value )
1335 1335 {
1336 1336 if (value == 1)
1337 1337 {
1338 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x02;
1338 spectral_matrix_regs->config = spectral_matrix_regs->config | BIT_IRQ_ON_ERROR;
1339 1339 }
1340 1340 else
1341 1341 {
1342 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffd; // 1101
1342 spectral_matrix_regs->config = spectral_matrix_regs->config & MASK_IRQ_ON_ERROR; // 1101
1343 1343 }
1344 1344 }
1345 1345
1346 1346 //*****************************
1347 1347 // CONFIGURE CALIBRATION SIGNAL
1348 1348 void setCalibrationPrescaler( unsigned int prescaler )
1349 1349 {
1350 1350 // prescaling of the master clock (25 MHz)
1351 1351 // master clock is divided by 2^prescaler
1352 1352 time_management_regs->calPrescaler = prescaler;
1353 1353 }
1354 1354
1355 1355 void setCalibrationDivisor( unsigned int divisionFactor )
1356 1356 {
1357 1357 // division of the prescaled clock by the division factor
1358 1358 time_management_regs->calDivisor = divisionFactor;
1359 1359 }
1360 1360
1361 void setCalibrationData( void ){
1361 void setCalibrationData( void )
1362 {
1363 /** This function is used to store the values used to drive the DAC in order to generate the SCM calibration signal
1364 *
1365 * @param void
1366 *
1367 * @return void
1368 *
1369 */
1370
1362 1371 unsigned int k;
1363 1372 unsigned short data;
1364 1373 float val;
1365 float f0;
1366 float f1;
1367 float fs;
1368 1374 float Ts;
1369 float scaleFactor;
1370 1375
1371 f0 = 625;
1372 f1 = 10000;
1373 fs = 160256.410;
1374 Ts = 1. / fs;
1375 scaleFactor = 0.250 / 0.000654; // 191, 500 mVpp, 2 sinus waves => 500 mVpp each, amplitude = 250 mV
1376
1377 time_management_regs->calDataPtr = 0x00;
1376 time_management_regs->calDataPtr = INIT_CHAR;
1378 1377
1379 1378 // build the signal for the SCM calibration
1380 for (k=0; k<256; k++)
1379 for (k = 0; k < CAL_NB_PTS; k++)
1381 1380 {
1382 val = sin( 2 * pi * f0 * k * Ts )
1383 + sin( 2 * pi * f1 * k * Ts );
1384 data = (unsigned short) ((val * scaleFactor) + 2048);
1385 time_management_regs->calData = data & 0xfff;
1381 val = sin( 2 * pi * CAL_F0 * k * Ts )
1382 + sin( 2 * pi * CAL_F1 * k * Ts );
1383 data = (unsigned short) ((val * CAL_SCALE_FACTOR) + CONST_2048);
1384 time_management_regs->calData = data & CAL_DATA_MASK;
1386 1385 }
1387 1386 }
1388 1387
1389 void setCalibrationDataInterleaved( void ){
1388 void setCalibrationDataInterleaved( void )
1389 {
1390 /** This function is used to store the values used to drive the DAC in order to generate the SCM calibration signal
1391 *
1392 * @param void
1393 *
1394 * @return void
1395 *
1396 * In interleaved mode, one can store more values than in normal mode.
1397 * The data are stored in bunch of 18 bits, 12 bits from one sample and 6 bits from another sample.
1398 * T store 3 values, one need two write operations.
1399 * s1 [ b11 b10 b9 b8 b7 b6 ] s0 [ b11 b10 b9 b8 b7 b6 b5 b3 b2 b1 b0 ]
1400 * s1 [ b5 b4 b3 b2 b1 b0 ] s2 [ b11 b10 b9 b8 b7 b6 b5 b3 b2 b1 b0 ]
1401 *
1402 */
1403
1390 1404 unsigned int k;
1391 1405 float val;
1392 float f0;
1393 float f1;
1394 float fs;
1395 1406 float Ts;
1396 unsigned short data[384];
1407 unsigned short data[CAL_NB_PTS_INTER];
1397 1408 unsigned char *dataPtr;
1398 1409
1399 f0 = 625;
1400 f1 = 10000;
1401 fs = 240384.615;
1402 Ts = 1. / fs;
1410 Ts = 1. / CAL_FS_INTER;
1403 1411
1404 time_management_regs->calDataPtr = 0x00;
1412 time_management_regs->calDataPtr = INIT_CHAR;
1405 1413
1406 1414 // build the signal for the SCM calibration
1407 for (k=0; k<384; k++)
1415 for (k=0; k<CAL_NB_PTS_INTER; k++)
1408 1416 {
1409 val = sin( 2 * pi * f0 * k * Ts )
1410 + sin( 2 * pi * f1 * k * Ts );
1411 data[k] = (unsigned short) (val * 512 + 2048);
1417 val = sin( 2 * pi * CAL_F0 * k * Ts )
1418 + sin( 2 * pi * CAL_F1 * k * Ts );
1419 data[k] = (unsigned short) ((val * CONST_512) + CONST_2048);
1412 1420 }
1413 1421
1414 1422 // write the signal in interleaved mode
1415 for (k=0; k<128; k++)
1423 for (k=0; k < STEPS_FOR_STORAGE_INTER; k++)
1416 1424 {
1417 dataPtr = (unsigned char*) &data[k*3 + 2];
1418 time_management_regs->calData = (data[k*3] & 0xfff)
1419 + ( (dataPtr[0] & 0x3f) << 12);
1420 time_management_regs->calData = (data[k*3 + 1] & 0xfff)
1421 + ( (dataPtr[1] & 0x3f) << 12);
1425 dataPtr = (unsigned char*) &data[ (k * BYTES_FOR_2_SAMPLES) + 2 ];
1426 time_management_regs->calData = ( data[ k * BYTES_FOR_2_SAMPLES ] & CAL_DATA_MASK )
1427 + ( (dataPtr[0] & CAL_DATA_MASK_INTER) << CAL_DATA_SHIFT_INTER);
1428 time_management_regs->calData = ( data[(k * BYTES_FOR_2_SAMPLES) + 1] & CAL_DATA_MASK )
1429 + ( (dataPtr[1] & CAL_DATA_MASK_INTER) << CAL_DATA_SHIFT_INTER);
1422 1430 }
1423 1431 }
1424 1432
1425 1433 void setCalibrationReload( bool state)
1426 1434 {
1427 1435 if (state == true)
1428 1436 {
1429 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000010; // [0001 0000]
1437 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | BIT_CAL_RELOAD; // [0001 0000]
1430 1438 }
1431 1439 else
1432 1440 {
1433 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffef; // [1110 1111]
1441 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & MASK_CAL_RELOAD; // [1110 1111]
1434 1442 }
1435 1443 }
1436 1444
1437 1445 void setCalibrationEnable( bool state )
1438 1446 {
1439 1447 // this bit drives the multiplexer
1440 1448 if (state == true)
1441 1449 {
1442 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000040; // [0100 0000]
1450 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | BIT_CAL_ENABLE; // [0100 0000]
1443 1451 }
1444 1452 else
1445 1453 {
1446 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffbf; // [1011 1111]
1454 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & MASK_CAL_ENABLE; // [1011 1111]
1447 1455 }
1448 1456 }
1449 1457
1450 1458 void setCalibrationInterleaved( bool state )
1451 1459 {
1452 1460 // this bit drives the multiplexer
1453 1461 if (state == true)
1454 1462 {
1455 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000020; // [0010 0000]
1463 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | BIT_SET_INTERLEAVED; // [0010 0000]
1456 1464 }
1457 1465 else
1458 1466 {
1459 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffdf; // [1101 1111]
1467 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & MASK_SET_INTERLEAVED; // [1101 1111]
1460 1468 }
1461 1469 }
1462 1470
1463 1471 void setCalibration( bool state )
1464 1472 {
1465 1473 if (state == true)
1466 1474 {
1467 1475 setCalibrationEnable( true );
1468 1476 setCalibrationReload( false );
1469 1477 set_hk_lfr_calib_enable( true );
1470 1478 }
1471 1479 else
1472 1480 {
1473 1481 setCalibrationEnable( false );
1474 1482 setCalibrationReload( true );
1475 1483 set_hk_lfr_calib_enable( false );
1476 1484 }
1477 1485 }
1478 1486
1479 1487 void configureCalibration( bool interleaved )
1480 1488 {
1481 1489 setCalibration( false );
1482 1490 if ( interleaved == true )
1483 1491 {
1484 1492 setCalibrationInterleaved( true );
1485 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1486 setCalibrationDivisor( 26 ); // => 240 384
1493 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1494 setCalibrationDivisor( CAL_F_DIVISOR_INTER ); // => 240 384
1487 1495 setCalibrationDataInterleaved();
1488 1496 }
1489 1497 else
1490 1498 {
1491 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1492 setCalibrationDivisor( 38 ); // => 160 256 (39 - 1)
1499 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1500 setCalibrationDivisor( CAL_F_DIVISOR ); // => 160 256 (39 - 1)
1493 1501 setCalibrationData();
1494 1502 }
1495 1503 }
1496 1504
1497 1505 //****************
1498 1506 // CLOSING ACTIONS
1499 1507 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC, unsigned char * time )
1500 1508 {
1501 1509 /** This function is used to update the HK packets statistics after a successful TC execution.
1502 1510 *
1503 1511 * @param TC points to the TC being processed
1504 1512 * @param time is the time used to date the TC execution
1505 1513 *
1506 1514 */
1507 1515
1508 1516 unsigned int val;
1509 1517
1510 1518 housekeeping_packet.hk_lfr_last_exe_tc_id[0] = TC->packetID[0];
1511 1519 housekeeping_packet.hk_lfr_last_exe_tc_id[1] = TC->packetID[1];
1512 housekeeping_packet.hk_lfr_last_exe_tc_type[0] = 0x00;
1520 housekeeping_packet.hk_lfr_last_exe_tc_type[0] = INIT_CHAR;
1513 1521 housekeeping_packet.hk_lfr_last_exe_tc_type[1] = TC->serviceType;
1514 housekeeping_packet.hk_lfr_last_exe_tc_subtype[0] = 0x00;
1522 housekeeping_packet.hk_lfr_last_exe_tc_subtype[0] = INIT_CHAR;
1515 1523 housekeeping_packet.hk_lfr_last_exe_tc_subtype[1] = TC->serviceSubType;
1516 housekeeping_packet.hk_lfr_last_exe_tc_time[0] = time[0];
1517 housekeeping_packet.hk_lfr_last_exe_tc_time[1] = time[1];
1518 housekeeping_packet.hk_lfr_last_exe_tc_time[2] = time[2];
1519 housekeeping_packet.hk_lfr_last_exe_tc_time[3] = time[3];
1520 housekeeping_packet.hk_lfr_last_exe_tc_time[4] = time[4];
1521 housekeeping_packet.hk_lfr_last_exe_tc_time[5] = time[5];
1524 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_0] = time[BYTE_0];
1525 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_1] = time[BYTE_1];
1526 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_2] = time[BYTE_2];
1527 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_3] = time[BYTE_3];
1528 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_4] = time[BYTE_4];
1529 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_5] = time[BYTE_5];
1522 1530
1523 val = housekeeping_packet.hk_lfr_exe_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_exe_tc_cnt[1];
1531 val = (housekeeping_packet.hk_lfr_exe_tc_cnt[0] * CONST_256) + housekeeping_packet.hk_lfr_exe_tc_cnt[1];
1524 1532 val++;
1525 housekeeping_packet.hk_lfr_exe_tc_cnt[0] = (unsigned char) (val >> 8);
1533 housekeeping_packet.hk_lfr_exe_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
1526 1534 housekeeping_packet.hk_lfr_exe_tc_cnt[1] = (unsigned char) (val);
1527 1535 }
1528 1536
1529 1537 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC, unsigned char * time )
1530 1538 {
1531 1539 /** This function is used to update the HK packets statistics after a TC rejection.
1532 1540 *
1533 1541 * @param TC points to the TC being processed
1534 1542 * @param time is the time used to date the TC rejection
1535 1543 *
1536 1544 */
1537 1545
1538 1546 unsigned int val;
1539 1547
1540 1548 housekeeping_packet.hk_lfr_last_rej_tc_id[0] = TC->packetID[0];
1541 1549 housekeeping_packet.hk_lfr_last_rej_tc_id[1] = TC->packetID[1];
1542 housekeeping_packet.hk_lfr_last_rej_tc_type[0] = 0x00;
1550 housekeeping_packet.hk_lfr_last_rej_tc_type[0] = INIT_CHAR;
1543 1551 housekeeping_packet.hk_lfr_last_rej_tc_type[1] = TC->serviceType;
1544 housekeeping_packet.hk_lfr_last_rej_tc_subtype[0] = 0x00;
1552 housekeeping_packet.hk_lfr_last_rej_tc_subtype[0] = INIT_CHAR;
1545 1553 housekeeping_packet.hk_lfr_last_rej_tc_subtype[1] = TC->serviceSubType;
1546 housekeeping_packet.hk_lfr_last_rej_tc_time[0] = time[0];
1547 housekeeping_packet.hk_lfr_last_rej_tc_time[1] = time[1];
1548 housekeeping_packet.hk_lfr_last_rej_tc_time[2] = time[2];
1549 housekeeping_packet.hk_lfr_last_rej_tc_time[3] = time[3];
1550 housekeeping_packet.hk_lfr_last_rej_tc_time[4] = time[4];
1551 housekeeping_packet.hk_lfr_last_rej_tc_time[5] = time[5];
1554 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_0] = time[BYTE_0];
1555 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_1] = time[BYTE_1];
1556 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_2] = time[BYTE_2];
1557 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_3] = time[BYTE_3];
1558 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_4] = time[BYTE_4];
1559 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_5] = time[BYTE_5];
1552 1560
1553 val = housekeeping_packet.hk_lfr_rej_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_rej_tc_cnt[1];
1561 val = (housekeeping_packet.hk_lfr_rej_tc_cnt[0] * CONST_256) + housekeeping_packet.hk_lfr_rej_tc_cnt[1];
1554 1562 val++;
1555 housekeeping_packet.hk_lfr_rej_tc_cnt[0] = (unsigned char) (val >> 8);
1563 housekeeping_packet.hk_lfr_rej_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
1556 1564 housekeeping_packet.hk_lfr_rej_tc_cnt[1] = (unsigned char) (val);
1557 1565 }
1558 1566
1559 1567 void close_action(ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id )
1560 1568 {
1561 1569 /** This function is the last step of the TC execution workflow.
1562 1570 *
1563 1571 * @param TC points to the TC being processed
1564 1572 * @param result is the result of the TC execution (LFR_SUCCESSFUL / LFR_DEFAULT)
1565 1573 * @param queue_id is the id of the RTEMS message queue used to send TM packets
1566 1574 * @param time is the time used to date the TC execution
1567 1575 *
1568 1576 */
1569 1577
1570 1578 unsigned char requestedMode;
1571 1579
1572 1580 if (result == LFR_SUCCESSFUL)
1573 1581 {
1574 1582 if ( !( (TC->serviceType==TC_TYPE_TIME) & (TC->serviceSubType==TC_SUBTYPE_UPDT_TIME) )
1575 1583 &
1576 1584 !( (TC->serviceType==TC_TYPE_GEN) & (TC->serviceSubType==TC_SUBTYPE_UPDT_INFO))
1577 1585 )
1578 1586 {
1579 1587 send_tm_lfr_tc_exe_success( TC, queue_id );
1580 1588 }
1581 1589 if ( (TC->serviceType == TC_TYPE_GEN) & (TC->serviceSubType == TC_SUBTYPE_ENTER) )
1582 1590 {
1583 1591 //**********************************
1584 1592 // UPDATE THE LFRMODE LOCAL VARIABLE
1585 1593 requestedMode = TC->dataAndCRC[1];
1586 1594 updateLFRCurrentMode( requestedMode );
1587 1595 }
1588 1596 }
1589 1597 else if (result == LFR_EXE_ERROR)
1590 1598 {
1591 1599 send_tm_lfr_tc_exe_error( TC, queue_id );
1592 1600 }
1593 1601 }
1594 1602
1595 1603 //***************************
1596 1604 // Interrupt Service Routines
1597 1605 rtems_isr commutation_isr1( rtems_vector_number vector )
1598 1606 {
1599 1607 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1600 1608 PRINTF("In commutation_isr1 *** Error sending event to DUMB\n")
1601 1609 }
1602 1610 }
1603 1611
1604 1612 rtems_isr commutation_isr2( rtems_vector_number vector )
1605 1613 {
1606 1614 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1607 1615 PRINTF("In commutation_isr2 *** Error sending event to DUMB\n")
1608 1616 }
1609 1617 }
1610 1618
1611 1619 //****************
1612 1620 // OTHER FUNCTIONS
1613 1621 void updateLFRCurrentMode( unsigned char requestedMode )
1614 1622 {
1615 1623 /** This function updates the value of the global variable lfrCurrentMode.
1616 1624 *
1617 1625 * lfrCurrentMode is a parameter used by several functions to know in which mode LFR is running.
1618 1626 *
1619 1627 */
1620 1628
1621 1629 // update the local value of lfrCurrentMode with the value contained in the housekeeping_packet structure
1622 housekeeping_packet.lfr_status_word[0] = (unsigned char) ((requestedMode << 4) + 0x0d);
1630 housekeeping_packet.lfr_status_word[0] = (housekeeping_packet.lfr_status_word[0] & STATUS_WORD_LFR_MODE_MASK)
1631 + (unsigned char) ( requestedMode << STATUS_WORD_LFR_MODE_SHIFT );
1623 1632 lfrCurrentMode = requestedMode;
1624 1633 }
1625 1634
1626 1635 void set_lfr_soft_reset( unsigned char value )
1627 1636 {
1628 1637 if (value == 1)
1629 1638 {
1630 time_management_regs->ctrl = time_management_regs->ctrl | 0x00000004; // [0100]
1639 time_management_regs->ctrl = time_management_regs->ctrl | BIT_SOFT_RESET; // [0100]
1631 1640 }
1632 1641 else
1633 1642 {
1634 time_management_regs->ctrl = time_management_regs->ctrl & 0xfffffffb; // [1011]
1643 time_management_regs->ctrl = time_management_regs->ctrl & MASK_SOFT_RESET; // [1011]
1635 1644 }
1636 1645 }
1637 1646
1638 1647 void reset_lfr( void )
1639 1648 {
1640 1649 set_lfr_soft_reset( 1 );
1641 1650
1642 1651 set_lfr_soft_reset( 0 );
1643 1652
1644 1653 set_hk_lfr_sc_potential_flag( true );
1645 1654 }
Show file before | Show file after | Hide comments
@@ -1,1623 +1,1650
1 1 /** Functions to load and dump parameters in the LFR registers.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TC related to parameter loading and dumping.\n
7 7 * TC_LFR_LOAD_COMMON_PAR\n
8 8 * TC_LFR_LOAD_NORMAL_PAR\n
9 9 * TC_LFR_LOAD_BURST_PAR\n
10 10 * TC_LFR_LOAD_SBM1_PAR\n
11 11 * TC_LFR_LOAD_SBM2_PAR\n
12 12 *
13 13 */
14 14
15 15 #include "tc_load_dump_parameters.h"
16 16
17 17 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_1;
18 18 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2;
19 19 ring_node kcoefficient_node_1;
20 20 ring_node kcoefficient_node_2;
21 21
22 22 int action_load_common_par(ccsdsTelecommandPacket_t *TC)
23 23 {
24 24 /** This function updates the LFR registers with the incoming common parameters.
25 25 *
26 26 * @param TC points to the TeleCommand packet that is being processed
27 27 *
28 28 *
29 29 */
30 30
31 31 parameter_dump_packet.sy_lfr_common_parameters_spare = TC->dataAndCRC[0];
32 32 parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
33 33 set_wfp_data_shaping( );
34 34 return LFR_SUCCESSFUL;
35 35 }
36 36
37 37 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
38 38 {
39 39 /** This function updates the LFR registers with the incoming normal parameters.
40 40 *
41 41 * @param TC points to the TeleCommand packet that is being processed
42 42 * @param queue_id is the id of the queue which handles TM related to this execution step
43 43 *
44 44 */
45 45
46 46 int result;
47 47 int flag;
48 48 rtems_status_code status;
49 49
50 50 flag = LFR_SUCCESSFUL;
51 51
52 52 if ( (lfrCurrentMode == LFR_MODE_NORMAL) ||
53 53 (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) {
54 54 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
55 55 flag = LFR_DEFAULT;
56 56 }
57 57
58 58 // CHECK THE PARAMETERS SET CONSISTENCY
59 59 if (flag == LFR_SUCCESSFUL)
60 60 {
61 61 flag = check_normal_par_consistency( TC, queue_id );
62 62 }
63 63
64 64 // SET THE PARAMETERS IF THEY ARE CONSISTENT
65 65 if (flag == LFR_SUCCESSFUL)
66 66 {
67 67 result = set_sy_lfr_n_swf_l( TC );
68 68 result = set_sy_lfr_n_swf_p( TC );
69 69 result = set_sy_lfr_n_bp_p0( TC );
70 70 result = set_sy_lfr_n_bp_p1( TC );
71 71 result = set_sy_lfr_n_asm_p( TC );
72 72 result = set_sy_lfr_n_cwf_long_f3( TC );
73 73 }
74 74
75 75 return flag;
76 76 }
77 77
78 78 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
79 79 {
80 80 /** This function updates the LFR registers with the incoming burst parameters.
81 81 *
82 82 * @param TC points to the TeleCommand packet that is being processed
83 83 * @param queue_id is the id of the queue which handles TM related to this execution step
84 84 *
85 85 */
86 86
87 87 int flag;
88 88 rtems_status_code status;
89 89 unsigned char sy_lfr_b_bp_p0;
90 90 unsigned char sy_lfr_b_bp_p1;
91 91 float aux;
92 92
93 93 flag = LFR_SUCCESSFUL;
94 94
95 95 if ( lfrCurrentMode == LFR_MODE_BURST ) {
96 96 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
97 97 flag = LFR_DEFAULT;
98 98 }
99 99
100 100 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
101 101 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
102 102
103 103 // sy_lfr_b_bp_p0 shall not be lower than its default value
104 104 if (flag == LFR_SUCCESSFUL)
105 105 {
106 106 if (sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0 )
107 107 {
108 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0+10, sy_lfr_b_bp_p0 );
108 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0 + DATAFIELD_OFFSET, sy_lfr_b_bp_p0 );
109 109 flag = WRONG_APP_DATA;
110 110 }
111 111 }
112 112 // sy_lfr_b_bp_p1 shall not be lower than its default value
113 113 if (flag == LFR_SUCCESSFUL)
114 114 {
115 115 if (sy_lfr_b_bp_p1 < DEFAULT_SY_LFR_B_BP_P1 )
116 116 {
117 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P1+10, sy_lfr_b_bp_p1 );
117 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P1 + DATAFIELD_OFFSET, sy_lfr_b_bp_p1 );
118 118 flag = WRONG_APP_DATA;
119 119 }
120 120 }
121 121 //****************************************************************
122 122 // check the consistency between sy_lfr_b_bp_p0 and sy_lfr_b_bp_p1
123 123 if (flag == LFR_SUCCESSFUL)
124 124 {
125 125 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
126 126 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
127 127 aux = ( (float ) sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0 ) - floor(sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0);
128 128 if (aux > FLOAT_EQUAL_ZERO)
129 129 {
130 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0+10, sy_lfr_b_bp_p0 );
130 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0 + DATAFIELD_OFFSET, sy_lfr_b_bp_p0 );
131 131 flag = LFR_DEFAULT;
132 132 }
133 133 }
134 134
135 135 // SET THE PARAMETERS
136 136 if (flag == LFR_SUCCESSFUL)
137 137 {
138 138 flag = set_sy_lfr_b_bp_p0( TC );
139 139 flag = set_sy_lfr_b_bp_p1( TC );
140 140 }
141 141
142 142 return flag;
143 143 }
144 144
145 145 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
146 146 {
147 147 /** This function updates the LFR registers with the incoming sbm1 parameters.
148 148 *
149 149 * @param TC points to the TeleCommand packet that is being processed
150 150 * @param queue_id is the id of the queue which handles TM related to this execution step
151 151 *
152 152 */
153 153
154 154 int flag;
155 155 rtems_status_code status;
156 156 unsigned char sy_lfr_s1_bp_p0;
157 157 unsigned char sy_lfr_s1_bp_p1;
158 158 float aux;
159 159
160 160 flag = LFR_SUCCESSFUL;
161 161
162 162 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
163 163 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
164 164 flag = LFR_DEFAULT;
165 165 }
166 166
167 167 sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
168 168 sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
169 169
170 170 // sy_lfr_s1_bp_p0
171 171 if (flag == LFR_SUCCESSFUL)
172 172 {
173 173 if (sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0 )
174 174 {
175 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0+10, sy_lfr_s1_bp_p0 );
175 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p0 );
176 176 flag = WRONG_APP_DATA;
177 177 }
178 178 }
179 179 // sy_lfr_s1_bp_p1
180 180 if (flag == LFR_SUCCESSFUL)
181 181 {
182 182 if (sy_lfr_s1_bp_p1 < DEFAULT_SY_LFR_S1_BP_P1 )
183 183 {
184 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P1+10, sy_lfr_s1_bp_p1 );
184 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P1 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p1 );
185 185 flag = WRONG_APP_DATA;
186 186 }
187 187 }
188 188 //******************************************************************
189 189 // check the consistency between sy_lfr_s1_bp_p0 and sy_lfr_s1_bp_p1
190 190 if (flag == LFR_SUCCESSFUL)
191 191 {
192 aux = ( (float ) sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0*0.25) ) - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0*0.25));
192 aux = ( (float ) sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE) )
193 - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE));
193 194 if (aux > FLOAT_EQUAL_ZERO)
194 195 {
195 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0+10, sy_lfr_s1_bp_p0 );
196 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p0 );
196 197 flag = LFR_DEFAULT;
197 198 }
198 199 }
199 200
200 201 // SET THE PARAMETERS
201 202 if (flag == LFR_SUCCESSFUL)
202 203 {
203 204 flag = set_sy_lfr_s1_bp_p0( TC );
204 205 flag = set_sy_lfr_s1_bp_p1( TC );
205 206 }
206 207
207 208 return flag;
208 209 }
209 210
210 211 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
211 212 {
212 213 /** This function updates the LFR registers with the incoming sbm2 parameters.
213 214 *
214 215 * @param TC points to the TeleCommand packet that is being processed
215 216 * @param queue_id is the id of the queue which handles TM related to this execution step
216 217 *
217 218 */
218 219
219 220 int flag;
220 221 rtems_status_code status;
221 222 unsigned char sy_lfr_s2_bp_p0;
222 223 unsigned char sy_lfr_s2_bp_p1;
223 224 float aux;
224 225
225 226 flag = LFR_SUCCESSFUL;
226 227
227 228 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
228 229 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
229 230 flag = LFR_DEFAULT;
230 231 }
231 232
232 233 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
233 234 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
234 235
235 236 // sy_lfr_s2_bp_p0
236 237 if (flag == LFR_SUCCESSFUL)
237 238 {
238 239 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
239 240 {
240 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
241 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p0 );
241 242 flag = WRONG_APP_DATA;
242 243 }
243 244 }
244 245 // sy_lfr_s2_bp_p1
245 246 if (flag == LFR_SUCCESSFUL)
246 247 {
247 248 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
248 249 {
249 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P1+10, sy_lfr_s2_bp_p1 );
250 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P1 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p1 );
250 251 flag = WRONG_APP_DATA;
251 252 }
252 253 }
253 254 //******************************************************************
254 255 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
255 256 if (flag == LFR_SUCCESSFUL)
256 257 {
257 258 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
258 259 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
259 260 aux = ( (float ) sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0 ) - floor(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
260 261 if (aux > FLOAT_EQUAL_ZERO)
261 262 {
262 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
263 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p0 );
263 264 flag = LFR_DEFAULT;
264 265 }
265 266 }
266 267
267 268 // SET THE PARAMETERS
268 269 if (flag == LFR_SUCCESSFUL)
269 270 {
270 271 flag = set_sy_lfr_s2_bp_p0( TC );
271 272 flag = set_sy_lfr_s2_bp_p1( TC );
272 273 }
273 274
274 275 return flag;
275 276 }
276 277
277 278 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
278 279 {
279 280 /** This function updates the LFR registers with the incoming sbm2 parameters.
280 281 *
281 282 * @param TC points to the TeleCommand packet that is being processed
282 283 * @param queue_id is the id of the queue which handles TM related to this execution step
283 284 *
284 285 */
285 286
286 287 int flag;
287 288
288 289 flag = LFR_DEFAULT;
289 290
290 291 flag = set_sy_lfr_kcoeff( TC, queue_id );
291 292
292 293 return flag;
293 294 }
294 295
295 296 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
296 297 {
297 298 /** This function updates the LFR registers with the incoming sbm2 parameters.
298 299 *
299 300 * @param TC points to the TeleCommand packet that is being processed
300 301 * @param queue_id is the id of the queue which handles TM related to this execution step
301 302 *
302 303 */
303 304
304 305 int flag;
305 306
306 307 flag = LFR_DEFAULT;
307 308
308 309 flag = set_sy_lfr_fbins( TC );
309 310
310 311 // once the fbins masks have been stored, they have to be merged with the masks which handle the reaction wheels frequencies filtering
311 312 merge_fbins_masks();
312 313
313 314 return flag;
314 315 }
315 316
316 317 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
317 318 {
318 319 /** This function updates the LFR registers with the incoming sbm2 parameters.
319 320 *
320 321 * @param TC points to the TeleCommand packet that is being processed
321 322 * @param queue_id is the id of the queue which handles TM related to this execution step
322 323 *
323 324 */
324 325
325 326 int flag;
326 327
327 328 flag = LFR_DEFAULT;
328 329
329 330 flag = check_sy_lfr_filter_parameters( TC, queue_id );
330 331
331 332 if (flag == LFR_SUCCESSFUL)
332 333 {
333 334 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ];
334 335 parameter_dump_packet.sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
335 parameter_dump_packet.sy_lfr_pas_filter_tbad[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 0 ];
336 parameter_dump_packet.sy_lfr_pas_filter_tbad[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 1 ];
337 parameter_dump_packet.sy_lfr_pas_filter_tbad[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 2 ];
338 parameter_dump_packet.sy_lfr_pas_filter_tbad[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 3 ];
336 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_0 ];
337 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_1 ];
338 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_2 ];
339 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_3 ];
339 340 parameter_dump_packet.sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
340 parameter_dump_packet.sy_lfr_pas_filter_shift[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 0 ];
341 parameter_dump_packet.sy_lfr_pas_filter_shift[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 1 ];
342 parameter_dump_packet.sy_lfr_pas_filter_shift[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 2 ];
343 parameter_dump_packet.sy_lfr_pas_filter_shift[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 3 ];
344 parameter_dump_packet.sy_lfr_sc_rw_delta_f[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 0 ];
345 parameter_dump_packet.sy_lfr_sc_rw_delta_f[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 1 ];
346 parameter_dump_packet.sy_lfr_sc_rw_delta_f[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 2 ];
347 parameter_dump_packet.sy_lfr_sc_rw_delta_f[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 3 ];
341 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_0 ];
342 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_1 ];
343 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_2 ];
344 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_3 ];
345 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_0 ];
346 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_1 ];
347 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_2 ];
348 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_3 ];
348 349
349 350 //****************************
350 351 // store PAS filter parameters
351 352 // sy_lfr_pas_filter_enabled
352 353 filterPar.spare_sy_lfr_pas_filter_enabled = parameter_dump_packet.spare_sy_lfr_pas_filter_enabled;
353 set_sy_lfr_pas_filter_enabled( parameter_dump_packet.spare_sy_lfr_pas_filter_enabled & 0x01 );
354 set_sy_lfr_pas_filter_enabled( parameter_dump_packet.spare_sy_lfr_pas_filter_enabled & BIT_PAS_FILTER_ENABLED );
354 355 // sy_lfr_pas_filter_modulus
355 356 filterPar.sy_lfr_pas_filter_modulus = parameter_dump_packet.sy_lfr_pas_filter_modulus;
356 357 // sy_lfr_pas_filter_tbad
357 358 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_tbad,
358 359 parameter_dump_packet.sy_lfr_pas_filter_tbad );
359 360 // sy_lfr_pas_filter_offset
360 361 filterPar.sy_lfr_pas_filter_offset = parameter_dump_packet.sy_lfr_pas_filter_offset;
361 362 // sy_lfr_pas_filter_shift
362 363 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_shift,
363 364 parameter_dump_packet.sy_lfr_pas_filter_shift );
364 365
365 366 //****************************************************
366 367 // store the parameter sy_lfr_sc_rw_delta_f as a float
367 368 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f,
368 369 parameter_dump_packet.sy_lfr_sc_rw_delta_f );
369 370 }
370 371
371 372 return flag;
372 373 }
373 374
374 375 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
375 376 {
376 377 /** This function updates the LFR registers with the incoming sbm2 parameters.
377 378 *
378 379 * @param TC points to the TeleCommand packet that is being processed
379 380 * @param queue_id is the id of the queue which handles TM related to this execution step
380 381 *
381 382 */
382 383
383 384 unsigned int address;
384 385 rtems_status_code status;
385 386 unsigned int freq;
386 387 unsigned int bin;
387 388 unsigned int coeff;
388 389 unsigned char *kCoeffPtr;
389 390 unsigned char *kCoeffDumpPtr;
390 391
391 392 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
392 393 // F0 => 11 bins
393 394 // F1 => 13 bins
394 395 // F2 => 12 bins
395 396 // 36 bins to dump in two packets (30 bins max per packet)
396 397
397 398 //*********
398 399 // PACKET 1
399 400 // 11 F0 bins, 13 F1 bins and 6 F2 bins
400 401 kcoefficients_dump_1.destinationID = TC->sourceID;
401 402 increment_seq_counter_destination_id_dump( kcoefficients_dump_1.packetSequenceControl, TC->sourceID );
402 for( freq=0;
403 freq<NB_BINS_COMPRESSED_SM_F0;
403 for( freq = 0;
404 freq < NB_BINS_COMPRESSED_SM_F0;
404 405 freq++ )
405 406 {
406 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1] = freq;
407 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1] = freq;
407 408 bin = freq;
408 409 // printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
409 410 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
410 411 {
411 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
412 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
413 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
414 ]; // 2 for the kcoeff_frequency
412 415 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
413 416 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
414 417 }
415 418 }
416 for( freq=NB_BINS_COMPRESSED_SM_F0;
417 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
419 for( freq = NB_BINS_COMPRESSED_SM_F0;
420 freq < ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
418 421 freq++ )
419 422 {
420 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
423 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = freq;
421 424 bin = freq - NB_BINS_COMPRESSED_SM_F0;
422 425 // printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
423 426 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
424 427 {
425 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
428 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
429 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
430 ]; // 2 for the kcoeff_frequency
426 431 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
427 432 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
428 433 }
429 434 }
430 for( freq=(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
431 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1+6);
435 for( freq = ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
436 freq < KCOEFF_BLK_NR_PKT1 ;
432 437 freq++ )
433 438 {
434 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
435 bin = freq - (NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
439 kcoefficients_dump_1.kcoeff_blks[ (freq * KCOEFF_BLK_SIZE) + 1 ] = freq;
440 bin = freq - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
436 441 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
437 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
442 for ( coeff = 0; coeff <NB_K_COEFF_PER_BIN; coeff++ )
438 443 {
439 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
444 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
445 (freq * KCOEFF_BLK_SIZE) + (coeff * NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
446 ]; // 2 for the kcoeff_frequency
440 447 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
441 448 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
442 449 }
443 450 }
444 kcoefficients_dump_1.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
445 kcoefficients_dump_1.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
446 kcoefficients_dump_1.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
447 kcoefficients_dump_1.time[3] = (unsigned char) (time_management_regs->coarse_time);
448 kcoefficients_dump_1.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
449 kcoefficients_dump_1.time[5] = (unsigned char) (time_management_regs->fine_time);
451 kcoefficients_dump_1.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
452 kcoefficients_dump_1.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
453 kcoefficients_dump_1.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
454 kcoefficients_dump_1.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
455 kcoefficients_dump_1.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
456 kcoefficients_dump_1.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
450 457 // SEND DATA
451 458 kcoefficient_node_1.status = 1;
452 459 address = (unsigned int) &kcoefficient_node_1;
453 460 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
454 461 if (status != RTEMS_SUCCESSFUL) {
455 462 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
456 463 }
457 464
458 465 //********
459 466 // PACKET 2
460 467 // 6 F2 bins
461 468 kcoefficients_dump_2.destinationID = TC->sourceID;
462 469 increment_seq_counter_destination_id_dump( kcoefficients_dump_2.packetSequenceControl, TC->sourceID );
463 for( freq=0; freq<6; freq++ )
470 for( freq = 0;
471 freq < KCOEFF_BLK_NR_PKT2;
472 freq++ )
464 473 {
465 kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + 6 + freq;
466 bin = freq + 6;
474 kcoefficients_dump_2.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = KCOEFF_BLK_NR_PKT1 + freq;
475 bin = freq + KCOEFF_BLK_NR_PKT2;
467 476 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
468 477 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
469 478 {
470 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
479 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[
480 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ ]; // 2 for the kcoeff_frequency
471 481 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
472 482 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
473 483 }
474 484 }
475 kcoefficients_dump_2.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
476 kcoefficients_dump_2.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
477 kcoefficients_dump_2.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
478 kcoefficients_dump_2.time[3] = (unsigned char) (time_management_regs->coarse_time);
479 kcoefficients_dump_2.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
480 kcoefficients_dump_2.time[5] = (unsigned char) (time_management_regs->fine_time);
485 kcoefficients_dump_2.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
486 kcoefficients_dump_2.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
487 kcoefficients_dump_2.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
488 kcoefficients_dump_2.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
489 kcoefficients_dump_2.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
490 kcoefficients_dump_2.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
481 491 // SEND DATA
482 492 kcoefficient_node_2.status = 1;
483 493 address = (unsigned int) &kcoefficient_node_2;
484 494 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
485 495 if (status != RTEMS_SUCCESSFUL) {
486 496 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
487 497 }
488 498
489 499 return status;
490 500 }
491 501
492 502 int action_dump_par( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
493 503 {
494 504 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
495 505 *
496 506 * @param queue_id is the id of the queue which handles TM related to this execution step.
497 507 *
498 508 * @return RTEMS directive status codes:
499 509 * - RTEMS_SUCCESSFUL - message sent successfully
500 510 * - RTEMS_INVALID_ID - invalid queue id
501 511 * - RTEMS_INVALID_SIZE - invalid message size
502 512 * - RTEMS_INVALID_ADDRESS - buffer is NULL
503 513 * - RTEMS_UNSATISFIED - out of message buffers
504 514 * - RTEMS_TOO_MANY - queue s limit has been reached
505 515 *
506 516 */
507 517
508 518 int status;
509 519
510 520 increment_seq_counter_destination_id_dump( parameter_dump_packet.packetSequenceControl, TC->sourceID );
511 521 parameter_dump_packet.destinationID = TC->sourceID;
512 522
513 523 // UPDATE TIME
514 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
515 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
516 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
517 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
518 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
519 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
524 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
525 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
526 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
527 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
528 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
529 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
520 530 // SEND DATA
521 531 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
522 532 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
523 533 if (status != RTEMS_SUCCESSFUL) {
524 534 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
525 535 }
526 536
527 537 return status;
528 538 }
529 539
530 540 //***********************
531 541 // NORMAL MODE PARAMETERS
532 542
533 543 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
534 544 {
535 545 unsigned char msb;
536 546 unsigned char lsb;
537 547 int flag;
538 548 float aux;
539 549 rtems_status_code status;
540 550
541 551 unsigned int sy_lfr_n_swf_l;
542 552 unsigned int sy_lfr_n_swf_p;
543 553 unsigned int sy_lfr_n_asm_p;
544 554 unsigned char sy_lfr_n_bp_p0;
545 555 unsigned char sy_lfr_n_bp_p1;
546 556 unsigned char sy_lfr_n_cwf_long_f3;
547 557
548 558 flag = LFR_SUCCESSFUL;
549 559
550 560 //***************
551 561 // get parameters
552 562 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
553 563 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
554 sy_lfr_n_swf_l = msb * 256 + lsb;
564 sy_lfr_n_swf_l = (msb * CONST_256) + lsb;
555 565
556 566 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
557 567 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
558 sy_lfr_n_swf_p = msb * 256 + lsb;
568 sy_lfr_n_swf_p = (msb * CONST_256) + lsb;
559 569
560 570 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
561 571 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
562 sy_lfr_n_asm_p = msb * 256 + lsb;
572 sy_lfr_n_asm_p = (msb * CONST_256) + lsb;
563 573
564 574 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
565 575
566 576 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
567 577
568 578 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
569 579
570 580 //******************
571 581 // check consistency
572 582 // sy_lfr_n_swf_l
573 if (sy_lfr_n_swf_l != 2048)
583 if (sy_lfr_n_swf_l != DFLT_SY_LFR_N_SWF_L)
574 584 {
575 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L+10, sy_lfr_n_swf_l );
585 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L + DATAFIELD_OFFSET, sy_lfr_n_swf_l );
576 586 flag = WRONG_APP_DATA;
577 587 }
578 588 // sy_lfr_n_swf_p
579 589 if (flag == LFR_SUCCESSFUL)
580 590 {
581 if ( sy_lfr_n_swf_p < 22 )
591 if ( sy_lfr_n_swf_p < MIN_SY_LFR_N_SWF_P )
582 592 {
583 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P+10, sy_lfr_n_swf_p );
593 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P + DATAFIELD_OFFSET, sy_lfr_n_swf_p );
584 594 flag = WRONG_APP_DATA;
585 595 }
586 596 }
587 597 // sy_lfr_n_bp_p0
588 598 if (flag == LFR_SUCCESSFUL)
589 599 {
590 600 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
591 601 {
592 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0+10, sy_lfr_n_bp_p0 );
602 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0 + DATAFIELD_OFFSET, sy_lfr_n_bp_p0 );
593 603 flag = WRONG_APP_DATA;
594 604 }
595 605 }
596 606 // sy_lfr_n_asm_p
597 607 if (flag == LFR_SUCCESSFUL)
598 608 {
599 609 if (sy_lfr_n_asm_p == 0)
600 610 {
601 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
611 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
602 612 flag = WRONG_APP_DATA;
603 613 }
604 614 }
605 615 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
606 616 if (flag == LFR_SUCCESSFUL)
607 617 {
608 618 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
609 619 if (aux > FLOAT_EQUAL_ZERO)
610 620 {
611 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
621 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
612 622 flag = WRONG_APP_DATA;
613 623 }
614 624 }
615 625 // sy_lfr_n_bp_p1
616 626 if (flag == LFR_SUCCESSFUL)
617 627 {
618 628 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
619 629 {
620 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
630 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
621 631 flag = WRONG_APP_DATA;
622 632 }
623 633 }
624 634 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
625 635 if (flag == LFR_SUCCESSFUL)
626 636 {
627 637 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
628 638 if (aux > FLOAT_EQUAL_ZERO)
629 639 {
630 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
640 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
631 641 flag = LFR_DEFAULT;
632 642 }
633 643 }
634 644 // sy_lfr_n_cwf_long_f3
635 645
636 646 return flag;
637 647 }
638 648
639 649 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
640 650 {
641 651 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
642 652 *
643 653 * @param TC points to the TeleCommand packet that is being processed
644 654 * @param queue_id is the id of the queue which handles TM related to this execution step
645 655 *
646 656 */
647 657
648 658 int result;
649 659
650 660 result = LFR_SUCCESSFUL;
651 661
652 662 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
653 663 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
654 664
655 665 return result;
656 666 }
657 667
658 668 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
659 669 {
660 670 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
661 671 *
662 672 * @param TC points to the TeleCommand packet that is being processed
663 673 * @param queue_id is the id of the queue which handles TM related to this execution step
664 674 *
665 675 */
666 676
667 677 int result;
668 678
669 679 result = LFR_SUCCESSFUL;
670 680
671 681 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
672 682 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
673 683
674 684 return result;
675 685 }
676 686
677 687 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
678 688 {
679 689 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
680 690 *
681 691 * @param TC points to the TeleCommand packet that is being processed
682 692 * @param queue_id is the id of the queue which handles TM related to this execution step
683 693 *
684 694 */
685 695
686 696 int result;
687 697
688 698 result = LFR_SUCCESSFUL;
689 699
690 700 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
691 701 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
692 702
693 703 return result;
694 704 }
695 705
696 706 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
697 707 {
698 708 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
699 709 *
700 710 * @param TC points to the TeleCommand packet that is being processed
701 711 * @param queue_id is the id of the queue which handles TM related to this execution step
702 712 *
703 713 */
704 714
705 715 int status;
706 716
707 717 status = LFR_SUCCESSFUL;
708 718
709 719 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
710 720
711 721 return status;
712 722 }
713 723
714 724 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
715 725 {
716 726 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
717 727 *
718 728 * @param TC points to the TeleCommand packet that is being processed
719 729 * @param queue_id is the id of the queue which handles TM related to this execution step
720 730 *
721 731 */
722 732
723 733 int status;
724 734
725 735 status = LFR_SUCCESSFUL;
726 736
727 737 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
728 738
729 739 return status;
730 740 }
731 741
732 742 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
733 743 {
734 744 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
735 745 *
736 746 * @param TC points to the TeleCommand packet that is being processed
737 747 * @param queue_id is the id of the queue which handles TM related to this execution step
738 748 *
739 749 */
740 750
741 751 int status;
742 752
743 753 status = LFR_SUCCESSFUL;
744 754
745 755 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
746 756
747 757 return status;
748 758 }
749 759
750 760 //**********************
751 761 // BURST MODE PARAMETERS
752 762 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
753 763 {
754 764 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
755 765 *
756 766 * @param TC points to the TeleCommand packet that is being processed
757 767 * @param queue_id is the id of the queue which handles TM related to this execution step
758 768 *
759 769 */
760 770
761 771 int status;
762 772
763 773 status = LFR_SUCCESSFUL;
764 774
765 775 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
766 776
767 777 return status;
768 778 }
769 779
770 780 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
771 781 {
772 782 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
773 783 *
774 784 * @param TC points to the TeleCommand packet that is being processed
775 785 * @param queue_id is the id of the queue which handles TM related to this execution step
776 786 *
777 787 */
778 788
779 789 int status;
780 790
781 791 status = LFR_SUCCESSFUL;
782 792
783 793 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
784 794
785 795 return status;
786 796 }
787 797
788 798 //*********************
789 799 // SBM1 MODE PARAMETERS
790 800 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
791 801 {
792 802 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
793 803 *
794 804 * @param TC points to the TeleCommand packet that is being processed
795 805 * @param queue_id is the id of the queue which handles TM related to this execution step
796 806 *
797 807 */
798 808
799 809 int status;
800 810
801 811 status = LFR_SUCCESSFUL;
802 812
803 813 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
804 814
805 815 return status;
806 816 }
807 817
808 818 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
809 819 {
810 820 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
811 821 *
812 822 * @param TC points to the TeleCommand packet that is being processed
813 823 * @param queue_id is the id of the queue which handles TM related to this execution step
814 824 *
815 825 */
816 826
817 827 int status;
818 828
819 829 status = LFR_SUCCESSFUL;
820 830
821 831 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
822 832
823 833 return status;
824 834 }
825 835
826 836 //*********************
827 837 // SBM2 MODE PARAMETERS
828 838 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC )
829 839 {
830 840 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
831 841 *
832 842 * @param TC points to the TeleCommand packet that is being processed
833 843 * @param queue_id is the id of the queue which handles TM related to this execution step
834 844 *
835 845 */
836 846
837 847 int status;
838 848
839 849 status = LFR_SUCCESSFUL;
840 850
841 851 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
842 852
843 853 return status;
844 854 }
845 855
846 856 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
847 857 {
848 858 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
849 859 *
850 860 * @param TC points to the TeleCommand packet that is being processed
851 861 * @param queue_id is the id of the queue which handles TM related to this execution step
852 862 *
853 863 */
854 864
855 865 int status;
856 866
857 867 status = LFR_SUCCESSFUL;
858 868
859 869 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
860 870
861 871 return status;
862 872 }
863 873
864 874 //*******************
865 875 // TC_LFR_UPDATE_INFO
866 876 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
867 877 {
868 878 unsigned int status;
869 879
870 880 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
871 881 || (mode == LFR_MODE_BURST)
872 882 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
873 883 {
874 884 status = LFR_SUCCESSFUL;
875 885 }
876 886 else
877 887 {
878 888 status = LFR_DEFAULT;
879 889 }
880 890
881 891 return status;
882 892 }
883 893
884 894 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
885 895 {
886 896 unsigned int status;
887 897
888 898 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
889 899 || (mode == TDS_MODE_BURST)
890 900 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
891 901 || (mode == TDS_MODE_LFM))
892 902 {
893 903 status = LFR_SUCCESSFUL;
894 904 }
895 905 else
896 906 {
897 907 status = LFR_DEFAULT;
898 908 }
899 909
900 910 return status;
901 911 }
902 912
903 913 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
904 914 {
905 915 unsigned int status;
906 916
907 917 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
908 918 || (mode == THR_MODE_BURST))
909 919 {
910 920 status = LFR_SUCCESSFUL;
911 921 }
912 922 else
913 923 {
914 924 status = LFR_DEFAULT;
915 925 }
916 926
917 927 return status;
918 928 }
919 929
920 930 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC )
921 931 {
922 932 /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally.
923 933 *
924 934 * @param TC points to the TeleCommand packet that is being processed
925 935 *
926 936 */
927 937
928 938 unsigned char * bytePosPtr; // pointer to the beginning of the incoming TC packet
929 939
930 940 bytePosPtr = (unsigned char *) &TC->packetID;
931 941
932 942 // cp_rpw_sc_rw1_f1
933 943 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f1,
934 944 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] );
935 945
936 946 // cp_rpw_sc_rw1_f2
937 947 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f2,
938 948 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] );
939 949
940 950 // cp_rpw_sc_rw2_f1
941 951 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f1,
942 952 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] );
943 953
944 954 // cp_rpw_sc_rw2_f2
945 955 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f2,
946 956 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] );
947 957
948 958 // cp_rpw_sc_rw3_f1
949 959 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f1,
950 960 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] );
951 961
952 962 // cp_rpw_sc_rw3_f2
953 963 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f2,
954 964 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] );
955 965
956 966 // cp_rpw_sc_rw4_f1
957 967 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f1,
958 968 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] );
959 969
960 970 // cp_rpw_sc_rw4_f2
961 971 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f2,
962 972 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] );
963 973 }
964 974
965 975 void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag )
966 976 {
967 977 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
968 978 *
969 979 * @param fbins_mask
970 980 * @param rw_f is the reaction wheel frequency to filter
971 981 * @param delta_f is the frequency step between the frequency bins, it depends on the frequency channel
972 982 * @param flag [true] filtering enabled [false] filtering disabled
973 983 *
974 984 * @return void
975 985 *
976 986 */
977 987
978 988 float f_RW_min;
979 989 float f_RW_MAX;
980 990 float fi_min;
981 991 float fi_MAX;
982 992 float fi;
983 993 float deltaBelow;
984 994 float deltaAbove;
985 995 int binBelow;
986 996 int binAbove;
987 997 int closestBin;
988 998 unsigned int whichByte;
989 999 int selectedByte;
990 1000 int bin;
991 int binToRemove[3];
1001 int binToRemove[NB_BINS_TO_REMOVE];
992 1002 int k;
993 1003
994 1004 whichByte = 0;
995 1005 bin = 0;
996 1006
997 binToRemove[0] = -1;
998 binToRemove[1] = -1;
999 binToRemove[2] = -1;
1007 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1008 {
1009 binToRemove[k] = -1;
1010 }
1000 1011
1001 1012 // compute the frequency range to filter [ rw_f - delta_f/2; rw_f + delta_f/2 ]
1002 f_RW_min = rw_f - filterPar.sy_lfr_sc_rw_delta_f / 2.;
1003 f_RW_MAX = rw_f + filterPar.sy_lfr_sc_rw_delta_f / 2.;
1013 f_RW_min = rw_f - (filterPar.sy_lfr_sc_rw_delta_f / 2.);
1014 f_RW_MAX = rw_f + (filterPar.sy_lfr_sc_rw_delta_f / 2.);
1004 1015
1005 1016 // compute the index of the frequency bin immediately below rw_f
1006 1017 binBelow = (int) ( floor( ((double) rw_f) / ((double) deltaFreq)) );
1007 1018 deltaBelow = rw_f - binBelow * deltaFreq;
1008 1019
1009 1020 // compute the index of the frequency bin immediately above rw_f
1010 1021 binAbove = (int) ( ceil( ((double) rw_f) / ((double) deltaFreq)) );
1011 1022 deltaAbove = binAbove * deltaFreq - rw_f;
1012 1023
1013 1024 // search the closest bin
1014 1025 if (deltaAbove > deltaBelow)
1015 1026 {
1016 1027 closestBin = binBelow;
1017 1028 }
1018 1029 else
1019 1030 {
1020 1031 closestBin = binAbove;
1021 1032 }
1022 1033
1023 1034 // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285]
1024 1035 fi = closestBin * deltaFreq;
1025 fi_min = fi - (deltaFreq * 0.285);
1026 fi_MAX = fi + (deltaFreq * 0.285);
1036 fi_min = fi - (deltaFreq * FI_INTERVAL_COEFF);
1037 fi_MAX = fi + (deltaFreq * FI_INTERVAL_COEFF);
1027 1038
1028 1039 //**************************************************************************************
1029 1040 // be careful here, one shall take into account that the bin 0 IS DROPPED in the spectra
1030 1041 // thus, the index 0 in a mask corresponds to the bin 1 of the spectrum
1031 1042 //**************************************************************************************
1032 1043
1033 1044 // 1. IF [ f_RW_min, f_RW_MAX] is included in [ fi_min; fi_MAX ]
1034 1045 // => remove f_(i), f_(i-1) and f_(i+1)
1035 1046 if ( ( f_RW_min > fi_min ) && ( f_RW_MAX < fi_MAX ) )
1036 1047 {
1037 1048 binToRemove[0] = (closestBin - 1) - 1;
1038 1049 binToRemove[1] = (closestBin) - 1;
1039 1050 binToRemove[2] = (closestBin + 1) - 1;
1040 1051 }
1041 1052 // 2. ELSE
1042 1053 // => remove the two f_(i) which are around f_RW
1043 1054 else
1044 1055 {
1045 1056 binToRemove[0] = (binBelow) - 1;
1046 1057 binToRemove[1] = (binAbove) - 1;
1047 1058 binToRemove[2] = (-1);
1048 1059 }
1049 1060
1050 for (k = 0; k < 3; k++)
1061 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1051 1062 {
1052 1063 bin = binToRemove[k];
1053 if ( (bin >= 0) && (bin <= 127) )
1064 if ( (bin >= BIN_MIN) && (bin <= BIN_MAX) )
1054 1065 {
1055 1066 if (flag == 1)
1056 1067 {
1057 whichByte = (bin >> 3); // division by 8
1058 selectedByte = ( 1 << (bin - (whichByte * 8)) );
1059 fbins_mask[15 - whichByte] = fbins_mask[15 - whichByte] & ((unsigned char) (~selectedByte)); // bytes are ordered MSB first in the packets
1068 whichByte = (bin >> SHIFT_3_BITS); // division by 8
1069 selectedByte = ( 1 << (bin - (whichByte * BITS_PER_BYTE)) );
1070 fbins_mask[BYTES_PER_MASK - 1 - whichByte] =
1071 fbins_mask[BYTES_PER_MASK - 1 - whichByte] & ((unsigned char) (~selectedByte)); // bytes are ordered MSB first in the packets
1060 1072 }
1061 1073 }
1062 1074 }
1063 1075 }
1064 1076
1065 1077 void build_sy_lfr_rw_mask( unsigned int channel )
1066 1078 {
1067 unsigned char local_rw_fbins_mask[16];
1079 unsigned char local_rw_fbins_mask[BYTES_PER_MASK];
1068 1080 unsigned char *maskPtr;
1069 1081 double deltaF;
1070 1082 unsigned k;
1071 1083
1072 1084 k = 0;
1073 1085
1074 1086 maskPtr = NULL;
1075 deltaF = 1.;
1087 deltaF = DELTAF_F2;
1076 1088
1077 1089 switch (channel)
1078 1090 {
1079 case 0:
1091 case CHANNELF0:
1080 1092 maskPtr = parameter_dump_packet.sy_lfr_rw_mask.fx.f0_word1;
1081 deltaF = 96.;
1093 deltaF = DELTAF_F0;
1082 1094 break;
1083 case 1:
1095 case CHANNELF1:
1084 1096 maskPtr = parameter_dump_packet.sy_lfr_rw_mask.fx.f1_word1;
1085 deltaF = 16.;
1097 deltaF = DELTAF_F1;
1086 1098 break;
1087 case 2:
1099 case CHANNELF2:
1088 1100 maskPtr = parameter_dump_packet.sy_lfr_rw_mask.fx.f2_word1;
1089 deltaF = 1.;
1101 deltaF = DELTAF_F2;
1090 1102 break;
1091 1103 default:
1092 1104 break;
1093 1105 }
1094 1106
1095 for (k = 0; k < 16; k++)
1107 for (k = 0; k < BYTES_PER_MASK; k++)
1096 1108 {
1097 local_rw_fbins_mask[k] = 0xff;
1109 local_rw_fbins_mask[k] = INT8_ALL_F;
1098 1110 }
1099 1111
1100 1112 // RW1 F1
1101 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x80) >> 7 ); // [1000 0000]
1113 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW1_F1) >> SHIFT_7_BITS ); // [1000 0000]
1102 1114
1103 1115 // RW1 F2
1104 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x40) >> 6 ); // [0100 0000]
1116 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW1_F2) >> SHIFT_6_BITS ); // [0100 0000]
1105 1117
1106 1118 // RW2 F1
1107 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x20) >> 5 ); // [0010 0000]
1119 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW2_F1) >> SHIFT_5_BITS ); // [0010 0000]
1108 1120
1109 1121 // RW2 F2
1110 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x10) >> 4 ); // [0001 0000]
1122 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW2_F2) >> SHIFT_4_BITS ); // [0001 0000]
1111 1123
1112 1124 // RW3 F1
1113 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x08) >> 3 ); // [0000 1000]
1125 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW3_F1) >> SHIFT_3_BITS ); // [0000 1000]
1114 1126
1115 1127 // RW3 F2
1116 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x04) >> 2 ); // [0000 0100]
1128 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW3_F2) >> SHIFT_2_BITS ); // [0000 0100]
1117 1129
1118 1130 // RW4 F1
1119 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x02) >> 1 ); // [0000 0010]
1131 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW4_F1) >> 1 ); // [0000 0010]
1120 1132
1121 1133 // RW4 F2
1122 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x01) ); // [0000 0001]
1134 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW4_F2) ); // [0000 0001]
1123 1135
1124 1136 // update the value of the fbins related to reaction wheels frequency filtering
1125 1137 if (maskPtr != NULL)
1126 1138 {
1127 for (k = 0; k < 16; k++)
1139 for (k = 0; k < BYTES_PER_MASK; k++)
1128 1140 {
1129 1141 maskPtr[k] = local_rw_fbins_mask[k];
1130 1142 }
1131 1143 }
1132 1144 }
1133 1145
1134 1146 void build_sy_lfr_rw_masks( void )
1135 1147 {
1136 build_sy_lfr_rw_mask( 0 );
1137 build_sy_lfr_rw_mask( 1 );
1138 build_sy_lfr_rw_mask( 2 );
1148 build_sy_lfr_rw_mask( CHANNELF0 );
1149 build_sy_lfr_rw_mask( CHANNELF1 );
1150 build_sy_lfr_rw_mask( CHANNELF2 );
1139 1151 }
1140 1152
1141 1153 void merge_fbins_masks( void )
1142 1154 {
1143 1155 unsigned char k;
1144 1156
1145 1157 unsigned char *fbins_f0;
1146 1158 unsigned char *fbins_f1;
1147 1159 unsigned char *fbins_f2;
1148 1160 unsigned char *rw_mask_f0;
1149 1161 unsigned char *rw_mask_f1;
1150 1162 unsigned char *rw_mask_f2;
1151 1163
1152 1164 fbins_f0 = parameter_dump_packet.sy_lfr_fbins.fx.f0_word1;
1153 1165 fbins_f1 = parameter_dump_packet.sy_lfr_fbins.fx.f1_word1;
1154 1166 fbins_f2 = parameter_dump_packet.sy_lfr_fbins.fx.f2_word1;
1155 1167 rw_mask_f0 = parameter_dump_packet.sy_lfr_rw_mask.fx.f0_word1;
1156 1168 rw_mask_f1 = parameter_dump_packet.sy_lfr_rw_mask.fx.f1_word1;
1157 1169 rw_mask_f2 = parameter_dump_packet.sy_lfr_rw_mask.fx.f2_word1;
1158 1170
1159 for( k=0; k < 16; k++ )
1171 for( k=0; k < BYTES_PER_MASK; k++ )
1160 1172 {
1161 1173 fbins_masks.merged_fbins_mask_f0[k] = fbins_f0[k] & rw_mask_f0[k];
1162 1174 fbins_masks.merged_fbins_mask_f1[k] = fbins_f1[k] & rw_mask_f1[k];
1163 1175 fbins_masks.merged_fbins_mask_f2[k] = fbins_f2[k] & rw_mask_f2[k];
1164 1176 }
1165 1177 }
1166 1178
1167 1179 //***********
1168 1180 // FBINS MASK
1169 1181
1170 1182 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
1171 1183 {
1172 1184 int status;
1173 1185 unsigned int k;
1174 1186 unsigned char *fbins_mask_dump;
1175 1187 unsigned char *fbins_mask_TC;
1176 1188
1177 1189 status = LFR_SUCCESSFUL;
1178 1190
1179 1191 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins.raw;
1180 1192 fbins_mask_TC = TC->dataAndCRC;
1181 1193
1182 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
1194 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1183 1195 {
1184 1196 fbins_mask_dump[k] = fbins_mask_TC[k];
1185 1197 }
1186 1198
1187 1199 return status;
1188 1200 }
1189 1201
1190 1202 //***************************
1191 1203 // TC_LFR_LOAD_PAS_FILTER_PAR
1192 1204
1193 1205 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
1194 1206 {
1195 1207 int flag;
1196 1208 rtems_status_code status;
1197 1209
1198 1210 unsigned char sy_lfr_pas_filter_enabled;
1199 1211 unsigned char sy_lfr_pas_filter_modulus;
1200 1212 float sy_lfr_pas_filter_tbad;
1201 1213 unsigned char sy_lfr_pas_filter_offset;
1202 1214 float sy_lfr_pas_filter_shift;
1203 1215 float sy_lfr_sc_rw_delta_f;
1204 1216 char *parPtr;
1205 1217
1206 1218 flag = LFR_SUCCESSFUL;
1207 sy_lfr_pas_filter_tbad = 0.0;
1208 sy_lfr_pas_filter_shift = 0.0;
1209 sy_lfr_sc_rw_delta_f = 0.0;
1219 sy_lfr_pas_filter_tbad = INIT_FLOAT;
1220 sy_lfr_pas_filter_shift = INIT_FLOAT;
1221 sy_lfr_sc_rw_delta_f = INIT_FLOAT;
1210 1222 parPtr = NULL;
1211 1223
1212 1224 //***************
1213 1225 // get parameters
1214 sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & 0x01; // [0000 0001]
1226 sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & BIT_PAS_FILTER_ENABLED; // [0000 0001]
1215 1227 sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
1216 1228 copyFloatByChar(
1217 1229 (unsigned char*) &sy_lfr_pas_filter_tbad,
1218 1230 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD ]
1219 1231 );
1220 1232 sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
1221 1233 copyFloatByChar(
1222 1234 (unsigned char*) &sy_lfr_pas_filter_shift,
1223 1235 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT ]
1224 1236 );
1225 1237 copyFloatByChar(
1226 1238 (unsigned char*) &sy_lfr_sc_rw_delta_f,
1227 1239 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F ]
1228 1240 );
1229 1241
1230 1242 //******************
1231 1243 // CHECK CONSISTENCY
1232 1244
1233 1245 //**************************
1234 1246 // sy_lfr_pas_filter_enabled
1235 1247 // nothing to check, value is 0 or 1
1236 1248
1237 1249 //**************************
1238 1250 // sy_lfr_pas_filter_modulus
1239 if ( (sy_lfr_pas_filter_modulus < 4) || (sy_lfr_pas_filter_modulus > 8) )
1251 if ( (sy_lfr_pas_filter_modulus < MIN_PAS_FILTER_MODULUS) || (sy_lfr_pas_filter_modulus > MAX_PAS_FILTER_MODULUS) )
1240 1252 {
1241 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS+10, sy_lfr_pas_filter_modulus );
1253 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1242 1254 flag = WRONG_APP_DATA;
1243 1255 }
1244 1256
1245 1257 //***********************
1246 1258 // sy_lfr_pas_filter_tbad
1247 if ( (sy_lfr_pas_filter_tbad < 0.0) || (sy_lfr_pas_filter_tbad > 4.0) )
1259 if ( (sy_lfr_pas_filter_tbad < MIN_PAS_FILTER_TBAD) || (sy_lfr_pas_filter_tbad > MAX_PAS_FILTER_TBAD) )
1248 1260 {
1249 1261 parPtr = (char*) &sy_lfr_pas_filter_tbad;
1250 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD+10, parPtr[3] );
1262 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1251 1263 flag = WRONG_APP_DATA;
1252 1264 }
1253 1265
1254 1266 //*************************
1255 1267 // sy_lfr_pas_filter_offset
1256 1268 if (flag == LFR_SUCCESSFUL)
1257 1269 {
1258 if ( (sy_lfr_pas_filter_offset < 0) || (sy_lfr_pas_filter_offset > 7) )
1270 if ( (sy_lfr_pas_filter_offset < MIN_PAS_FILTER_OFFSET) || (sy_lfr_pas_filter_offset > MAX_PAS_FILTER_OFFSET) )
1259 1271 {
1260 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET+10, sy_lfr_pas_filter_offset );
1272 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET + DATAFIELD_OFFSET, sy_lfr_pas_filter_offset );
1261 1273 flag = WRONG_APP_DATA;
1262 1274 }
1263 1275 }
1264 1276
1265 1277 //************************
1266 1278 // sy_lfr_pas_filter_shift
1267 if ( (sy_lfr_pas_filter_shift < 0.0) || (sy_lfr_pas_filter_shift > 1.0) )
1279 if (flag == LFR_SUCCESSFUL)
1268 1280 {
1269 parPtr = (char*) &sy_lfr_pas_filter_shift;
1270 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT+10, parPtr[3] );
1271 flag = WRONG_APP_DATA;
1281 if ( (sy_lfr_pas_filter_shift < MIN_PAS_FILTER_SHIFT) || (sy_lfr_pas_filter_shift > MAX_PAS_FILTER_SHIFT) )
1282 {
1283 parPtr = (char*) &sy_lfr_pas_filter_shift;
1284 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1285 flag = WRONG_APP_DATA;
1286 }
1287 }
1288
1289 //*************************************
1290 // check global coherency of the values
1291 if (flag == LFR_SUCCESSFUL)
1292 {
1293 if ( (sy_lfr_pas_filter_tbad + sy_lfr_pas_filter_offset + sy_lfr_pas_filter_shift) > sy_lfr_pas_filter_modulus )
1294 {
1295 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1296 flag = WRONG_APP_DATA;
1297 }
1272 1298 }
1273 1299
1274 1300 //*********************
1275 1301 // sy_lfr_sc_rw_delta_f
1276 1302 // nothing to check, no default value in the ICD
1277 1303
1278 1304 return flag;
1279 1305 }
1280 1306
1281 1307 //**************
1282 1308 // KCOEFFICIENTS
1283 1309 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC,rtems_id queue_id )
1284 1310 {
1285 1311 unsigned int kcoeff;
1286 1312 unsigned short sy_lfr_kcoeff_frequency;
1287 1313 unsigned short bin;
1288 1314 unsigned short *freqPtr;
1289 1315 float *kcoeffPtr_norm;
1290 1316 float *kcoeffPtr_sbm;
1291 1317 int status;
1292 1318 unsigned char *kcoeffLoadPtr;
1293 1319 unsigned char *kcoeffNormPtr;
1294 1320 unsigned char *kcoeffSbmPtr_a;
1295 1321 unsigned char *kcoeffSbmPtr_b;
1296 1322
1297 1323 status = LFR_SUCCESSFUL;
1298 1324
1299 1325 kcoeffPtr_norm = NULL;
1300 1326 kcoeffPtr_sbm = NULL;
1301 1327 bin = 0;
1302 1328
1303 1329 freqPtr = (unsigned short *) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY];
1304 1330 sy_lfr_kcoeff_frequency = *freqPtr;
1305 1331
1306 1332 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
1307 1333 {
1308 1334 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
1309 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 10 + 1,
1335 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + DATAFIELD_OFFSET + 1,
1310 1336 TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 1] ); // +1 to get the LSB instead of the MSB
1311 1337 status = LFR_DEFAULT;
1312 1338 }
1313 1339 else
1314 1340 {
1315 1341 if ( ( sy_lfr_kcoeff_frequency >= 0 )
1316 1342 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
1317 1343 {
1318 1344 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
1319 1345 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
1320 1346 bin = sy_lfr_kcoeff_frequency;
1321 1347 }
1322 1348 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
1323 1349 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
1324 1350 {
1325 1351 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
1326 1352 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
1327 1353 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
1328 1354 }
1329 1355 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
1330 1356 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
1331 1357 {
1332 1358 kcoeffPtr_norm = k_coeff_intercalib_f2;
1333 1359 kcoeffPtr_sbm = NULL;
1334 1360 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
1335 1361 }
1336 1362 }
1337 1363
1338 1364 if (kcoeffPtr_norm != NULL ) // update K coefficient for NORMAL data products
1339 1365 {
1340 1366 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1341 1367 {
1342 1368 // destination
1343 1369 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + kcoeff ];
1344 1370 // source
1345 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * kcoeff];
1371 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1346 1372 // copy source to destination
1347 1373 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
1348 1374 }
1349 1375 }
1350 1376
1351 1377 if (kcoeffPtr_sbm != NULL ) // update K coefficient for SBM data products
1352 1378 {
1353 1379 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1354 1380 {
1355 1381 // destination
1356 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * 2 ];
1357 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * 2 + 1 ];
1382 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ];
1383 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ (((bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_KCOEFF_PER_NORM_KCOEFF) + 1 ];
1358 1384 // source
1359 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * kcoeff];
1385 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1360 1386 // copy source to destination
1361 1387 copyFloatByChar( kcoeffSbmPtr_a, kcoeffLoadPtr );
1362 1388 copyFloatByChar( kcoeffSbmPtr_b, kcoeffLoadPtr );
1363 1389 }
1364 1390 }
1365 1391
1366 1392 // print_k_coeff();
1367 1393
1368 1394 return status;
1369 1395 }
1370 1396
1371 1397 void copyFloatByChar( unsigned char *destination, unsigned char *source )
1372 1398 {
1373 destination[0] = source[0];
1374 destination[1] = source[1];
1375 destination[2] = source[2];
1376 destination[3] = source[3];
1399 destination[BYTE_0] = source[BYTE_0];
1400 destination[BYTE_1] = source[BYTE_1];
1401 destination[BYTE_2] = source[BYTE_2];
1402 destination[BYTE_3] = source[BYTE_3];
1377 1403 }
1378 1404
1379 1405 void floatToChar( float value, unsigned char* ptr)
1380 1406 {
1381 1407 unsigned char* valuePtr;
1382 1408
1383 1409 valuePtr = (unsigned char*) &value;
1384 ptr[0] = valuePtr[0];
1385 ptr[1] = valuePtr[1];
1386 ptr[2] = valuePtr[2];
1387 ptr[3] = valuePtr[3];
1410 ptr[BYTE_0] = valuePtr[BYTE_0];
1411 ptr[BYTE_1] = valuePtr[BYTE_1];
1412 ptr[BYTE_2] = valuePtr[BYTE_2];
1413 ptr[BYTE_3] = valuePtr[BYTE_3];
1388 1414 }
1389 1415
1390 1416 //**********
1391 1417 // init dump
1392 1418
1393 1419 void init_parameter_dump( void )
1394 1420 {
1395 1421 /** This function initialize the parameter_dump_packet global variable with default values.
1396 1422 *
1397 1423 */
1398 1424
1399 1425 unsigned int k;
1400 1426
1401 1427 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
1402 1428 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
1403 1429 parameter_dump_packet.reserved = CCSDS_RESERVED;
1404 1430 parameter_dump_packet.userApplication = CCSDS_USER_APP;
1405 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);
1431 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1406 1432 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1407 1433 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1408 1434 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1409 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> 8);
1435 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> SHIFT_1_BYTE);
1410 1436 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
1411 1437 // DATA FIELD HEADER
1412 1438 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1413 1439 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
1414 1440 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
1415 1441 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
1416 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
1417 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
1418 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
1419 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
1420 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
1421 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
1442 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
1443 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
1444 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
1445 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
1446 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
1447 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
1422 1448 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
1423 1449
1424 1450 //******************
1425 1451 // COMMON PARAMETERS
1426 1452 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
1427 1453 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
1428 1454
1429 1455 //******************
1430 1456 // NORMAL PARAMETERS
1431 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> 8);
1457 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> SHIFT_1_BYTE);
1432 1458 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1433 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> 8);
1459 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> SHIFT_1_BYTE);
1434 1460 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1435 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> 8);
1461 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> SHIFT_1_BYTE);
1436 1462 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1437 1463 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1438 1464 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1439 1465 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1440 1466
1441 1467 //*****************
1442 1468 // BURST PARAMETERS
1443 1469 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1444 1470 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1445 1471
1446 1472 //****************
1447 1473 // SBM1 PARAMETERS
1448 1474 parameter_dump_packet.sy_lfr_s1_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P0; // min value is 0.25 s for the period
1449 1475 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1450 1476
1451 1477 //****************
1452 1478 // SBM2 PARAMETERS
1453 1479 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1454 1480 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1455 1481
1456 1482 //************
1457 1483 // FBINS MASKS
1458 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
1484 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1459 1485 {
1460 parameter_dump_packet.sy_lfr_fbins.raw[k] = 0xff;
1486 parameter_dump_packet.sy_lfr_fbins.raw[k] = INT8_ALL_F;
1461 1487 }
1462 1488
1463 1489 // PAS FILTER PARAMETERS
1464 parameter_dump_packet.pa_rpw_spare8_2 = 0x00;
1465 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = 0x00;
1490 parameter_dump_packet.pa_rpw_spare8_2 = INIT_CHAR;
1491 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = INIT_CHAR;
1466 1492 parameter_dump_packet.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
1467 1493 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_TBAD, parameter_dump_packet.sy_lfr_pas_filter_tbad );
1468 1494 parameter_dump_packet.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
1469 1495 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift );
1470 1496 floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f );
1471 1497
1472 1498 // LFR_RW_MASK
1473 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
1499 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1474 1500 {
1475 parameter_dump_packet.sy_lfr_rw_mask.raw[k] = 0xff;
1501 parameter_dump_packet.sy_lfr_rw_mask.raw[k] = INT8_ALL_F;
1476 1502 }
1477 1503
1478 1504 // once the reaction wheels masks have been initialized, they have to be merged with the fbins masks
1479 1505 merge_fbins_masks();
1480 1506 }
1481 1507
1482 1508 void init_kcoefficients_dump( void )
1483 1509 {
1484 init_kcoefficients_dump_packet( &kcoefficients_dump_1, 1, 30 );
1485 init_kcoefficients_dump_packet( &kcoefficients_dump_2, 2, 6 );
1510 init_kcoefficients_dump_packet( &kcoefficients_dump_1, PKTNR_1, KCOEFF_BLK_NR_PKT1 );
1511 init_kcoefficients_dump_packet( &kcoefficients_dump_2, PKTNR_2, KCOEFF_BLK_NR_PKT2 );
1486 1512
1487 1513 kcoefficient_node_1.previous = NULL;
1488 1514 kcoefficient_node_1.next = NULL;
1489 1515 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1490 kcoefficient_node_1.coarseTime = 0x00;
1491 kcoefficient_node_1.fineTime = 0x00;
1516 kcoefficient_node_1.coarseTime = INIT_CHAR;
1517 kcoefficient_node_1.fineTime = INIT_CHAR;
1492 1518 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1493 kcoefficient_node_1.status = 0x00;
1519 kcoefficient_node_1.status = INIT_CHAR;
1494 1520
1495 1521 kcoefficient_node_2.previous = NULL;
1496 1522 kcoefficient_node_2.next = NULL;
1497 1523 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1498 kcoefficient_node_2.coarseTime = 0x00;
1499 kcoefficient_node_2.fineTime = 0x00;
1524 kcoefficient_node_2.coarseTime = INIT_CHAR;
1525 kcoefficient_node_2.fineTime = INIT_CHAR;
1500 1526 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1501 kcoefficient_node_2.status = 0x00;
1527 kcoefficient_node_2.status = INIT_CHAR;
1502 1528 }
1503 1529
1504 1530 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1505 1531 {
1506 1532 unsigned int k;
1507 1533 unsigned int packetLength;
1508 1534
1509 packetLength = blk_nr * 130 + 20 - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1535 packetLength =
1536 ((blk_nr * KCOEFF_BLK_SIZE) + BYTE_POS_KCOEFFICIENTS_PARAMETES) - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1510 1537
1511 1538 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1512 1539 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1513 1540 kcoefficients_dump->reserved = CCSDS_RESERVED;
1514 1541 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1515 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);;
1516 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;;
1542 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1543 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1517 1544 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1518 1545 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1519 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> 8);
1546 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
1520 1547 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1521 1548 // DATA FIELD HEADER
1522 1549 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1523 1550 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1524 1551 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1525 1552 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1526 kcoefficients_dump->time[0] = 0x00;
1527 kcoefficients_dump->time[1] = 0x00;
1528 kcoefficients_dump->time[2] = 0x00;
1529 kcoefficients_dump->time[3] = 0x00;
1530 kcoefficients_dump->time[4] = 0x00;
1531 kcoefficients_dump->time[5] = 0x00;
1553 kcoefficients_dump->time[BYTE_0] = INIT_CHAR;
1554 kcoefficients_dump->time[BYTE_1] = INIT_CHAR;
1555 kcoefficients_dump->time[BYTE_2] = INIT_CHAR;
1556 kcoefficients_dump->time[BYTE_3] = INIT_CHAR;
1557 kcoefficients_dump->time[BYTE_4] = INIT_CHAR;
1558 kcoefficients_dump->time[BYTE_5] = INIT_CHAR;
1532 1559 kcoefficients_dump->sid = SID_K_DUMP;
1533 1560
1534 kcoefficients_dump->pkt_cnt = 2;
1535 kcoefficients_dump->pkt_nr = pkt_nr;
1561 kcoefficients_dump->pkt_cnt = KCOEFF_PKTCNT;
1562 kcoefficients_dump->pkt_nr = PKTNR_1;
1536 1563 kcoefficients_dump->blk_nr = blk_nr;
1537 1564
1538 1565 //******************
1539 1566 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1540 1567 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1541 for (k=0; k<3900; k++)
1568 for (k=0; k<(KCOEFF_BLK_NR_PKT1 * KCOEFF_BLK_SIZE); k++)
1542 1569 {
1543 kcoefficients_dump->kcoeff_blks[k] = 0x00;
1570 kcoefficients_dump->kcoeff_blks[k] = INIT_CHAR;
1544 1571 }
1545 1572 }
1546 1573
1547 1574 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id )
1548 1575 {
1549 1576 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
1550 1577 *
1551 1578 * @param packet_sequence_control points to the packet sequence control which will be incremented
1552 1579 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
1553 1580 *
1554 1581 * If the destination ID is not known, a dedicated counter is incremented.
1555 1582 *
1556 1583 */
1557 1584
1558 1585 unsigned short sequence_cnt;
1559 1586 unsigned short segmentation_grouping_flag;
1560 1587 unsigned short new_packet_sequence_control;
1561 1588 unsigned char i;
1562 1589
1563 1590 switch (destination_id)
1564 1591 {
1565 1592 case SID_TC_GROUND:
1566 1593 i = GROUND;
1567 1594 break;
1568 1595 case SID_TC_MISSION_TIMELINE:
1569 1596 i = MISSION_TIMELINE;
1570 1597 break;
1571 1598 case SID_TC_TC_SEQUENCES:
1572 1599 i = TC_SEQUENCES;
1573 1600 break;
1574 1601 case SID_TC_RECOVERY_ACTION_CMD:
1575 1602 i = RECOVERY_ACTION_CMD;
1576 1603 break;
1577 1604 case SID_TC_BACKUP_MISSION_TIMELINE:
1578 1605 i = BACKUP_MISSION_TIMELINE;
1579 1606 break;
1580 1607 case SID_TC_DIRECT_CMD:
1581 1608 i = DIRECT_CMD;
1582 1609 break;
1583 1610 case SID_TC_SPARE_GRD_SRC1:
1584 1611 i = SPARE_GRD_SRC1;
1585 1612 break;
1586 1613 case SID_TC_SPARE_GRD_SRC2:
1587 1614 i = SPARE_GRD_SRC2;
1588 1615 break;
1589 1616 case SID_TC_OBCP:
1590 1617 i = OBCP;
1591 1618 break;
1592 1619 case SID_TC_SYSTEM_CONTROL:
1593 1620 i = SYSTEM_CONTROL;
1594 1621 break;
1595 1622 case SID_TC_AOCS:
1596 1623 i = AOCS;
1597 1624 break;
1598 1625 case SID_TC_RPW_INTERNAL:
1599 1626 i = RPW_INTERNAL;
1600 1627 break;
1601 1628 default:
1602 1629 i = GROUND;
1603 1630 break;
1604 1631 }
1605 1632
1606 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
1607 sequence_cnt = sequenceCounters_TM_DUMP[ i ] & 0x3fff;
1633 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
1634 sequence_cnt = sequenceCounters_TM_DUMP[ i ] & SEQ_CNT_MASK;
1608 1635
1609 1636 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
1610 1637
1611 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
1638 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> SHIFT_1_BYTE);
1612 1639 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1613 1640
1614 1641 // increment the sequence counter
1615 1642 if ( sequenceCounters_TM_DUMP[ i ] < SEQ_CNT_MAX )
1616 1643 {
1617 1644 sequenceCounters_TM_DUMP[ i ] = sequenceCounters_TM_DUMP[ i ] + 1;
1618 1645 }
1619 1646 else
1620 1647 {
1621 1648 sequenceCounters_TM_DUMP[ i ] = 0;
1622 1649 }
1623 1650 }
Show file before | Show file after | Hide comments
@@ -1,514 +1,514
1 1 /** Functions to send TM packets related to TC parsing and execution.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to send appropriate TM packets after parsing and execution:
7 7 * - TM_LFR_TC_EXE_SUCCESS
8 8 * - TM_LFR_TC_EXE_INCONSISTENT
9 9 * - TM_LFR_TC_EXE_NOT_EXECUTABLE
10 10 * - TM_LFR_TC_EXE_NOT_IMPLEMENTED
11 11 * - TM_LFR_TC_EXE_ERROR
12 12 * - TM_LFR_TC_EXE_CORRUPTED
13 13 *
14 14 */
15 15
16 16 #include "tm_lfr_tc_exe.h"
17 17
18 18 int send_tm_lfr_tc_exe_success( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
19 19 {
20 20 /** This function sends a TM_LFR_TC_EXE_SUCCESS packet in the dedicated RTEMS message queue.
21 21 *
22 22 * @param TC points to the TeleCommand packet that is being processed
23 23 * @param queue_id is the id of the queue which handles TM
24 24 *
25 25 * @return RTEMS directive status code:
26 26 * - RTEMS_SUCCESSFUL - message sent successfully
27 27 * - RTEMS_INVALID_ID - invalid queue id
28 28 * - RTEMS_INVALID_SIZE - invalid message size
29 29 * - RTEMS_INVALID_ADDRESS - buffer is NULL
30 30 * - RTEMS_UNSATISFIED - out of message buffers
31 31 * - RTEMS_TOO_MANY - queue s limit has been reached
32 32 *
33 33 */
34 34
35 35 rtems_status_code status;
36 36 Packet_TM_LFR_TC_EXE_SUCCESS_t TM;
37 37 unsigned char messageSize;
38 38
39 39 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
40 40 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
41 41 TM.reserved = DEFAULT_RESERVED;
42 42 TM.userApplication = CCSDS_USER_APP;
43 43 // PACKET HEADER
44 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
44 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
45 45 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
46 46 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
47 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_SUCCESS >> 8);
47 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_SUCCESS >> SHIFT_1_BYTE);
48 48 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_SUCCESS );
49 49 // DATA FIELD HEADER
50 50 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
51 51 TM.serviceType = TM_TYPE_TC_EXE;
52 52 TM.serviceSubType = TM_SUBTYPE_EXE_OK;
53 53 TM.destinationID = TC->sourceID;
54 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
55 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
56 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
57 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
58 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
59 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
54 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
55 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
56 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
57 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
58 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
59 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
60 60 //
61 61 TM.telecommand_pkt_id[0] = TC->packetID[0];
62 62 TM.telecommand_pkt_id[1] = TC->packetID[1];
63 63 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
64 64 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
65 65
66 66 messageSize = PACKET_LENGTH_TC_EXE_SUCCESS + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
67 67
68 68 // SEND DATA
69 69 status = rtems_message_queue_send( queue_id, &TM, messageSize);
70 70 if (status != RTEMS_SUCCESSFUL) {
71 71 PRINTF("in send_tm_lfr_tc_exe_success *** ERR\n")
72 72 }
73 73
74 74 // UPDATE HK FIELDS
75 75 update_last_TC_exe( TC, TM.time );
76 76
77 77 return status;
78 78 }
79 79
80 80 int send_tm_lfr_tc_exe_inconsistent( ccsdsTelecommandPacket_t *TC, rtems_id queue_id,
81 81 unsigned char byte_position, unsigned char rcv_value )
82 82 {
83 83 /** This function sends a TM_LFR_TC_EXE_INCONSISTENT packet in the dedicated RTEMS message queue.
84 84 *
85 85 * @param TC points to the TeleCommand packet that is being processed
86 86 * @param queue_id is the id of the queue which handles TM
87 87 * @param byte_position is the byte position of the MSB of the parameter that has been seen as inconsistent
88 88 * @param rcv_value is the value of the LSB of the parameter that has been detected as inconsistent
89 89 *
90 90 * @return RTEMS directive status code:
91 91 * - RTEMS_SUCCESSFUL - message sent successfully
92 92 * - RTEMS_INVALID_ID - invalid queue id
93 93 * - RTEMS_INVALID_SIZE - invalid message size
94 94 * - RTEMS_INVALID_ADDRESS - buffer is NULL
95 95 * - RTEMS_UNSATISFIED - out of message buffers
96 96 * - RTEMS_TOO_MANY - queue s limit has been reached
97 97 *
98 98 */
99 99
100 100 rtems_status_code status;
101 101 Packet_TM_LFR_TC_EXE_INCONSISTENT_t TM;
102 102 unsigned char messageSize;
103 103
104 104 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
105 105 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
106 106 TM.reserved = DEFAULT_RESERVED;
107 107 TM.userApplication = CCSDS_USER_APP;
108 108 // PACKET HEADER
109 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
109 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
110 110 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
111 111 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
112 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_INCONSISTENT >> 8);
112 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_INCONSISTENT >> SHIFT_1_BYTE);
113 113 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_INCONSISTENT );
114 114 // DATA FIELD HEADER
115 115 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
116 116 TM.serviceType = TM_TYPE_TC_EXE;
117 117 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
118 118 TM.destinationID = TC->sourceID;
119 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
120 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
121 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
122 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
123 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
124 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
119 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
120 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
121 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
122 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
123 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
124 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
125 125 //
126 TM.tc_failure_code[0] = (char) (WRONG_APP_DATA >> 8);
126 TM.tc_failure_code[0] = (char) (WRONG_APP_DATA >> SHIFT_1_BYTE);
127 127 TM.tc_failure_code[1] = (char) (WRONG_APP_DATA );
128 128 TM.telecommand_pkt_id[0] = TC->packetID[0];
129 129 TM.telecommand_pkt_id[1] = TC->packetID[1];
130 130 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
131 131 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
132 132 TM.tc_service = TC->serviceType; // type of the rejected TC
133 133 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
134 134 TM.byte_position = byte_position;
135 135 TM.rcv_value = (unsigned char) rcv_value;
136 136
137 137 messageSize = PACKET_LENGTH_TC_EXE_INCONSISTENT + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
138 138
139 139 // SEND DATA
140 140 status = rtems_message_queue_send( queue_id, &TM, messageSize);
141 141 if (status != RTEMS_SUCCESSFUL) {
142 142 PRINTF("in send_tm_lfr_tc_exe_inconsistent *** ERR\n")
143 143 }
144 144
145 145 // UPDATE HK FIELDS
146 146 update_last_TC_rej( TC, TM.time );
147 147
148 148 return status;
149 149 }
150 150
151 151 int send_tm_lfr_tc_exe_not_executable( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
152 152 {
153 153 /** This function sends a TM_LFR_TC_EXE_NOT_EXECUTABLE packet in the dedicated RTEMS message queue.
154 154 *
155 155 * @param TC points to the TeleCommand packet that is being processed
156 156 * @param queue_id is the id of the queue which handles TM
157 157 *
158 158 * @return RTEMS directive status code:
159 159 * - RTEMS_SUCCESSFUL - message sent successfully
160 160 * - RTEMS_INVALID_ID - invalid queue id
161 161 * - RTEMS_INVALID_SIZE - invalid message size
162 162 * - RTEMS_INVALID_ADDRESS - buffer is NULL
163 163 * - RTEMS_UNSATISFIED - out of message buffers
164 164 * - RTEMS_TOO_MANY - queue s limit has been reached
165 165 *
166 166 */
167 167
168 168 rtems_status_code status;
169 169 Packet_TM_LFR_TC_EXE_NOT_EXECUTABLE_t TM;
170 170 unsigned char messageSize;
171 171
172 172 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
173 173 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
174 174 TM.reserved = DEFAULT_RESERVED;
175 175 TM.userApplication = CCSDS_USER_APP;
176 176 // PACKET HEADER
177 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
177 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
178 178 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
179 179 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
180 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE >> 8);
180 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE >> SHIFT_1_BYTE);
181 181 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE );
182 182 // DATA FIELD HEADER
183 183 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
184 184 TM.serviceType = TM_TYPE_TC_EXE;
185 185 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
186 186 TM.destinationID = TC->sourceID; // default destination id
187 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
188 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
189 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
190 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
191 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
192 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
187 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
188 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
189 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
190 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
191 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
192 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
193 193 //
194 TM.tc_failure_code[0] = (char) (TC_NOT_EXE >> 8);
194 TM.tc_failure_code[0] = (char) (TC_NOT_EXE >> SHIFT_1_BYTE);
195 195 TM.tc_failure_code[1] = (char) (TC_NOT_EXE );
196 196 TM.telecommand_pkt_id[0] = TC->packetID[0];
197 197 TM.telecommand_pkt_id[1] = TC->packetID[1];
198 198 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
199 199 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
200 200 TM.tc_service = TC->serviceType; // type of the rejected TC
201 201 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
202 202 TM.lfr_status_word[0] = housekeeping_packet.lfr_status_word[0];
203 203 TM.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1];
204 204
205 205 messageSize = PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
206 206
207 207 // SEND DATA
208 208 status = rtems_message_queue_send( queue_id, &TM, messageSize);
209 209 if (status != RTEMS_SUCCESSFUL) {
210 210 PRINTF("in send_tm_lfr_tc_exe_not_executable *** ERR\n")
211 211 }
212 212
213 213 // UPDATE HK FIELDS
214 214 update_last_TC_rej( TC, TM.time );
215 215
216 216 return status;
217 217 }
218 218
219 219 int send_tm_lfr_tc_exe_not_implemented( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time )
220 220 {
221 221 /** This function sends a TM_LFR_TC_EXE_NOT_IMPLEMENTED packet in the dedicated RTEMS message queue.
222 222 *
223 223 * @param TC points to the TeleCommand packet that is being processed
224 224 * @param queue_id is the id of the queue which handles TM
225 225 *
226 226 * @return RTEMS directive status code:
227 227 * - RTEMS_SUCCESSFUL - message sent successfully
228 228 * - RTEMS_INVALID_ID - invalid queue id
229 229 * - RTEMS_INVALID_SIZE - invalid message size
230 230 * - RTEMS_INVALID_ADDRESS - buffer is NULL
231 231 * - RTEMS_UNSATISFIED - out of message buffers
232 232 * - RTEMS_TOO_MANY - queue s limit has been reached
233 233 *
234 234 */
235 235
236 236 rtems_status_code status;
237 237 Packet_TM_LFR_TC_EXE_NOT_IMPLEMENTED_t TM;
238 238 unsigned char messageSize;
239 239
240 240 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
241 241 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
242 242 TM.reserved = DEFAULT_RESERVED;
243 243 TM.userApplication = CCSDS_USER_APP;
244 244 // PACKET HEADER
245 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
245 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
246 246 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
247 247 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
248 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED >> 8);
248 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED >> SHIFT_1_BYTE);
249 249 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED );
250 250 // DATA FIELD HEADER
251 251 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
252 252 TM.serviceType = TM_TYPE_TC_EXE;
253 253 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
254 254 TM.destinationID = TC->sourceID; // default destination id
255 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
256 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
257 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
258 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
259 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
260 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
255 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
256 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
257 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
258 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
259 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
260 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
261 261 //
262 TM.tc_failure_code[0] = (char) (FUNCT_NOT_IMPL >> 8);
262 TM.tc_failure_code[0] = (char) (FUNCT_NOT_IMPL >> SHIFT_1_BYTE);
263 263 TM.tc_failure_code[1] = (char) (FUNCT_NOT_IMPL );
264 264 TM.telecommand_pkt_id[0] = TC->packetID[0];
265 265 TM.telecommand_pkt_id[1] = TC->packetID[1];
266 266 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
267 267 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
268 268 TM.tc_service = TC->serviceType; // type of the rejected TC
269 269 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
270 270
271 271 messageSize = PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
272 272
273 273 // SEND DATA
274 274 status = rtems_message_queue_send( queue_id, &TM, messageSize);
275 275 if (status != RTEMS_SUCCESSFUL) {
276 276 PRINTF("in send_tm_lfr_tc_exe_not_implemented *** ERR\n")
277 277 }
278 278
279 279 // UPDATE HK FIELDS
280 280 update_last_TC_rej( TC, TM.time );
281 281
282 282 return status;
283 283 }
284 284
285 285 int send_tm_lfr_tc_exe_error( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
286 286 {
287 287 /** This function sends a TM_LFR_TC_EXE_ERROR packet in the dedicated RTEMS message queue.
288 288 *
289 289 * @param TC points to the TeleCommand packet that is being processed
290 290 * @param queue_id is the id of the queue which handles TM
291 291 *
292 292 * @return RTEMS directive status code:
293 293 * - RTEMS_SUCCESSFUL - message sent successfully
294 294 * - RTEMS_INVALID_ID - invalid queue id
295 295 * - RTEMS_INVALID_SIZE - invalid message size
296 296 * - RTEMS_INVALID_ADDRESS - buffer is NULL
297 297 * - RTEMS_UNSATISFIED - out of message buffers
298 298 * - RTEMS_TOO_MANY - queue s limit has been reached
299 299 *
300 300 */
301 301
302 302 rtems_status_code status;
303 303 Packet_TM_LFR_TC_EXE_ERROR_t TM;
304 304 unsigned char messageSize;
305 305
306 306 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
307 307 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
308 308 TM.reserved = DEFAULT_RESERVED;
309 309 TM.userApplication = CCSDS_USER_APP;
310 310 // PACKET HEADER
311 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
311 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
312 312 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
313 313 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
314 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_ERROR >> 8);
314 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_ERROR >> SHIFT_1_BYTE);
315 315 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_ERROR );
316 316 // DATA FIELD HEADER
317 317 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
318 318 TM.serviceType = TM_TYPE_TC_EXE;
319 319 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
320 320 TM.destinationID = TC->sourceID; // default destination id
321 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
322 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
323 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
324 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
325 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
326 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
321 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
322 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
323 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
324 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
325 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
326 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
327 327 //
328 TM.tc_failure_code[0] = (char) (FAIL_DETECTED >> 8);
328 TM.tc_failure_code[0] = (char) (FAIL_DETECTED >> SHIFT_1_BYTE);
329 329 TM.tc_failure_code[1] = (char) (FAIL_DETECTED );
330 330 TM.telecommand_pkt_id[0] = TC->packetID[0];
331 331 TM.telecommand_pkt_id[1] = TC->packetID[1];
332 332 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
333 333 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
334 334 TM.tc_service = TC->serviceType; // type of the rejected TC
335 335 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
336 336
337 337 messageSize = PACKET_LENGTH_TC_EXE_ERROR + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
338 338
339 339 // SEND DATA
340 340 status = rtems_message_queue_send( queue_id, &TM, messageSize);
341 341 if (status != RTEMS_SUCCESSFUL) {
342 342 PRINTF("in send_tm_lfr_tc_exe_error *** ERR\n")
343 343 }
344 344
345 345 // UPDATE HK FIELDS
346 346 update_last_TC_rej( TC, TM.time );
347 347
348 348 return status;
349 349 }
350 350
351 351 int send_tm_lfr_tc_exe_corrupted(ccsdsTelecommandPacket_t *TC, rtems_id queue_id,
352 352 unsigned char *computed_CRC, unsigned char *currentTC_LEN_RCV,
353 353 unsigned char destinationID )
354 354 {
355 355 /** This function sends a TM_LFR_TC_EXE_CORRUPTED packet in the dedicated RTEMS message queue.
356 356 *
357 357 * @param TC points to the TeleCommand packet that is being processed
358 358 * @param queue_id is the id of the queue which handles TM
359 359 * @param computed_CRC points to a buffer of two bytes containing the CRC computed during the parsing of the TeleCommand
360 360 * @param currentTC_LEN_RCV points to a buffer of two bytes containing a packet size field computed on the received data
361 361 *
362 362 * @return RTEMS directive status code:
363 363 * - RTEMS_SUCCESSFUL - message sent successfully
364 364 * - RTEMS_INVALID_ID - invalid queue id
365 365 * - RTEMS_INVALID_SIZE - invalid message size
366 366 * - RTEMS_INVALID_ADDRESS - buffer is NULL
367 367 * - RTEMS_UNSATISFIED - out of message buffers
368 368 * - RTEMS_TOO_MANY - queue s limit has been reached
369 369 *
370 370 */
371 371
372 372 rtems_status_code status;
373 373 Packet_TM_LFR_TC_EXE_CORRUPTED_t TM;
374 374 unsigned char messageSize;
375 375 unsigned int packetLength;
376 376 unsigned int estimatedPacketLength;
377 377 unsigned char *packetDataField;
378 378
379 packetLength = (TC->packetLength[0] * 256) + TC->packetLength[1]; // compute the packet length parameter written in the TC
380 estimatedPacketLength = (unsigned int) (currentTC_LEN_RCV[0] * 256 + currentTC_LEN_RCV[1]);
379 packetLength = (TC->packetLength[0] * CONST_256) + TC->packetLength[1]; // compute the packet length parameter written in the TC
380 estimatedPacketLength = (unsigned int) ((currentTC_LEN_RCV[0] * CONST_256) + currentTC_LEN_RCV[1]);
381 381 packetDataField = (unsigned char *) &TC->headerFlag_pusVersion_Ack; // get the beginning of the data field
382 382
383 383 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
384 384 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
385 385 TM.reserved = DEFAULT_RESERVED;
386 386 TM.userApplication = CCSDS_USER_APP;
387 387 // PACKET HEADER
388 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
388 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
389 389 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
390 390 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
391 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_CORRUPTED >> 8);
391 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_CORRUPTED >> SHIFT_1_BYTE);
392 392 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_CORRUPTED );
393 393 // DATA FIELD HEADER
394 394 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
395 395 TM.serviceType = TM_TYPE_TC_EXE;
396 396 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
397 397 TM.destinationID = destinationID;
398 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
399 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
400 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
401 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
402 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
403 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
398 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
399 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
400 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
401 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
402 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
403 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
404 404 //
405 TM.tc_failure_code[0] = (unsigned char) (CORRUPTED >> 8);
405 TM.tc_failure_code[0] = (unsigned char) (CORRUPTED >> SHIFT_1_BYTE);
406 406 TM.tc_failure_code[1] = (unsigned char) (CORRUPTED );
407 407 TM.telecommand_pkt_id[0] = TC->packetID[0];
408 408 TM.telecommand_pkt_id[1] = TC->packetID[1];
409 409 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
410 410 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
411 411 TM.tc_service = TC->serviceType; // type of the rejected TC
412 412 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
413 413 TM.pkt_len_rcv_value[0] = TC->packetLength[0];
414 414 TM.pkt_len_rcv_value[1] = TC->packetLength[1];
415 415 TM.pkt_datafieldsize_cnt[0] = currentTC_LEN_RCV[0];
416 416 TM.pkt_datafieldsize_cnt[1] = currentTC_LEN_RCV[1];
417 417 // TM.rcv_crc[0] = packetDataField[ packetLength - 1 ];
418 418 // TM.rcv_crc[1] = packetDataField[ packetLength ];
419 419 TM.rcv_crc[0] = packetDataField[ estimatedPacketLength - 1 ];
420 420 TM.rcv_crc[1] = packetDataField[ estimatedPacketLength ];
421 421 TM.computed_crc[0] = computed_CRC[0];
422 422 TM.computed_crc[1] = computed_CRC[1];
423 423
424 424 messageSize = PACKET_LENGTH_TC_EXE_CORRUPTED + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
425 425
426 426 // SEND DATA
427 427 status = rtems_message_queue_send( queue_id, &TM, messageSize);
428 428 if (status != RTEMS_SUCCESSFUL) {
429 429 PRINTF("in send_tm_lfr_tc_exe_error *** ERR\n")
430 430 }
431 431
432 432 // UPDATE HK FIELDS
433 433 update_last_TC_rej( TC, TM.time );
434 434
435 435 return status;
436 436 }
437 437
438 438 void increment_seq_counter_destination_id( unsigned char *packet_sequence_control, unsigned char destination_id )
439 439 {
440 440 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
441 441 *
442 442 * @param packet_sequence_control points to the packet sequence control which will be incremented
443 443 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
444 444 *
445 445 * If the destination ID is not known, a dedicated counter is incremented.
446 446 *
447 447 */
448 448
449 449 unsigned short sequence_cnt;
450 450 unsigned short segmentation_grouping_flag;
451 451 unsigned short new_packet_sequence_control;
452 452 unsigned char i;
453 453
454 454 switch (destination_id)
455 455 {
456 456 case SID_TC_GROUND:
457 457 i = GROUND;
458 458 break;
459 459 case SID_TC_MISSION_TIMELINE:
460 460 i = MISSION_TIMELINE;
461 461 break;
462 462 case SID_TC_TC_SEQUENCES:
463 463 i = TC_SEQUENCES;
464 464 break;
465 465 case SID_TC_RECOVERY_ACTION_CMD:
466 466 i = RECOVERY_ACTION_CMD;
467 467 break;
468 468 case SID_TC_BACKUP_MISSION_TIMELINE:
469 469 i = BACKUP_MISSION_TIMELINE;
470 470 break;
471 471 case SID_TC_DIRECT_CMD:
472 472 i = DIRECT_CMD;
473 473 break;
474 474 case SID_TC_SPARE_GRD_SRC1:
475 475 i = SPARE_GRD_SRC1;
476 476 break;
477 477 case SID_TC_SPARE_GRD_SRC2:
478 478 i = SPARE_GRD_SRC2;
479 479 break;
480 480 case SID_TC_OBCP:
481 481 i = OBCP;
482 482 break;
483 483 case SID_TC_SYSTEM_CONTROL:
484 484 i = SYSTEM_CONTROL;
485 485 break;
486 486 case SID_TC_AOCS:
487 487 i = AOCS;
488 488 break;
489 489 case SID_TC_RPW_INTERNAL:
490 490 i = RPW_INTERNAL;
491 491 break;
492 492 default:
493 493 i = GROUND;
494 494 break;
495 495 }
496 496
497 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
498 sequence_cnt = sequenceCounters_TC_EXE[ i ] & 0x3fff;
497 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
498 sequence_cnt = sequenceCounters_TC_EXE[ i ] & SEQ_CNT_MASK;
499 499
500 500 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
501 501
502 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
502 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> SHIFT_1_BYTE);
503 503 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
504 504
505 505 // increment the sequence counter
506 506 if ( sequenceCounters_TC_EXE[ i ] < SEQ_CNT_MAX )
507 507 {
508 508 sequenceCounters_TC_EXE[ i ] = sequenceCounters_TC_EXE[ i ] + 1;
509 509 }
510 510 else
511 511 {
512 512 sequenceCounters_TC_EXE[ i ] = 0;
513 513 }
514 514 }
Show file before | Show file after | Hide comments
@@ -1,1314 +1,1312
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 extractSWF1 = false;
35 35 bool extractSWF2 = false;
36 36 bool swf0_ready_flag_f1 = false;
37 37 bool swf0_ready_flag_f2 = false;
38 38 bool swf1_ready = false;
39 39 bool swf2_ready = false;
40 40
41 41 int swf1_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ];
42 42 int swf2_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ];
43 43 ring_node ring_node_swf1_extracted;
44 44 ring_node ring_node_swf2_extracted;
45 45
46 46 typedef enum resynchro_state_t
47 47 {
48 48 MEASURE,
49 49 CORRECTION
50 50 } resynchro_state;
51 51
52 52 //*********************
53 53 // Interrupt SubRoutine
54 54
55 55 ring_node * getRingNodeToSendCWF( unsigned char frequencyChannel)
56 56 {
57 57 ring_node *node;
58 58
59 59 node = NULL;
60 60 switch ( frequencyChannel ) {
61 case 1:
61 case CHANNELF1:
62 62 node = ring_node_to_send_cwf_f1;
63 63 break;
64 case 2:
64 case CHANNELF2:
65 65 node = ring_node_to_send_cwf_f2;
66 66 break;
67 case 3:
67 case CHANNELF3:
68 68 node = ring_node_to_send_cwf_f3;
69 69 break;
70 70 default:
71 71 break;
72 72 }
73 73
74 74 return node;
75 75 }
76 76
77 77 ring_node * getRingNodeToSendSWF( unsigned char frequencyChannel)
78 78 {
79 79 ring_node *node;
80 80
81 81 node = NULL;
82 82 switch ( frequencyChannel ) {
83 case 0:
83 case CHANNELF0:
84 84 node = ring_node_to_send_swf_f0;
85 85 break;
86 case 1:
86 case CHANNELF1:
87 87 node = ring_node_to_send_swf_f1;
88 88 break;
89 case 2:
89 case CHANNELF2:
90 90 node = ring_node_to_send_swf_f2;
91 91 break;
92 92 default:
93 93 break;
94 94 }
95 95
96 96 return node;
97 97 }
98 98
99 99 void reset_extractSWF( void )
100 100 {
101 101 extractSWF1 = false;
102 102 extractSWF2 = false;
103 103 swf0_ready_flag_f1 = false;
104 104 swf0_ready_flag_f2 = false;
105 105 swf1_ready = false;
106 106 swf2_ready = false;
107 107 }
108 108
109 109 inline void waveforms_isr_f3( void )
110 110 {
111 111 rtems_status_code spare_status;
112 112
113 113 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
114 114 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
115 115 { // in modes other than STANDBY and BURST, send the CWF_F3 data
116 116 //***
117 117 // F3
118 if ( (waveform_picker_regs->status & 0xc0) != 0x00 ) { // [1100 0000] check the f3 full bits
118 if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F3) != INIT_CHAR ) { // [1100 0000] check the f3 full bits
119 119 ring_node_to_send_cwf_f3 = current_ring_node_f3->previous;
120 120 current_ring_node_f3 = current_ring_node_f3->next;
121 if ((waveform_picker_regs->status & 0x40) == 0x40){ // [0100 0000] f3 buffer 0 is full
121 if ((waveform_picker_regs->status & BIT_WFP_BUF_F3_0) == BIT_WFP_BUF_F3_0){ // [0100 0000] f3 buffer 0 is full
122 122 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_0_coarse_time;
123 123 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_0_fine_time;
124 124 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->buffer_address;
125 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008840; // [1000 1000 0100 0000]
125 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F3_0; // [1000 1000 0100 0000]
126 126 }
127 else if ((waveform_picker_regs->status & 0x80) == 0x80){ // [1000 0000] f3 buffer 1 is full
127 else if ((waveform_picker_regs->status & BIT_WFP_BUF_F3_1) == BIT_WFP_BUF_F3_1){ // [1000 0000] f3 buffer 1 is full
128 128 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_1_coarse_time;
129 129 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_1_fine_time;
130 130 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address;
131 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008880; // [1000 1000 1000 0000]
131 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F3_1; // [1000 1000 1000 0000]
132 132 }
133 133 if (rtems_event_send( Task_id[TASKID_CWF3], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
134 134 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
135 135 }
136 136 }
137 137 }
138 138 }
139 139
140 140 inline void waveforms_isr_burst( void )
141 141 {
142 142 unsigned char status;
143 143 rtems_status_code spare_status;
144 144
145 status = (waveform_picker_regs->status & 0x30) >> 4; // [0011 0000] get the status bits for f2
146
145 status = (waveform_picker_regs->status & BITS_WFP_STATUS_F2) >> SHIFT_WFP_STATUS_F2; // [0011 0000] get the status bits for f2
147 146
148 147 switch(status)
149 148 {
150 case 1:
149 case BIT_WFP_BUFFER_0:
151 150 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
152 151 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
153 152 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
154 153 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
155 154 current_ring_node_f2 = current_ring_node_f2->next;
156 155 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
157 156 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
158 157 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
159 158 }
160 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
159 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_0; // [0100 0100 0001 0000]
161 160 break;
162 case 2:
161 case BIT_WFP_BUFFER_1:
163 162 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
164 163 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
165 164 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
166 165 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
167 166 current_ring_node_f2 = current_ring_node_f2->next;
168 167 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
169 168 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
170 169 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
171 170 }
172 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
171 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_1; // [0100 0100 0010 0000]
173 172 break;
174 173 default:
175 174 break;
176 175 }
177 176 }
178 177
179 178 inline void waveform_isr_normal_sbm1_sbm2( void )
180 179 {
181 180 rtems_status_code status;
182 181
183 182 //***
184 183 // F0
185 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) // [0000 0011] check the f0 full bits
184 if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F0) != INIT_CHAR ) // [0000 0011] check the f0 full bits
186 185 {
187 186 swf0_ready_flag_f1 = true;
188 187 swf0_ready_flag_f2 = true;
189 188 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
190 189 current_ring_node_f0 = current_ring_node_f0->next;
191 if ( (waveform_picker_regs->status & 0x01) == 0x01)
190 if ( (waveform_picker_regs->status & BIT_WFP_BUFFER_0) == BIT_WFP_BUFFER_0)
192 191 {
193 192
194 193 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
195 194 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
196 195 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
197 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
196 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F0_0; // [0001 0001 0000 0001]
198 197 }
199 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
198 else if ( (waveform_picker_regs->status & BIT_WFP_BUFFER_1) == BIT_WFP_BUFFER_1)
200 199 {
201 200 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
202 201 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
203 202 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
204 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
203 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F0_1; // [0001 0001 0000 0010]
205 204 }
206 205 // send an event to the WFRM task for resynchro activities
207 206 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_SWF_RESYNCH );
208 207 }
209 208
210 209 //***
211 210 // F1
212 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bits
211 if ( (waveform_picker_regs->status & 0x0c) != INIT_CHAR ) { // [0000 1100] check the f1 full bits
213 212 // (1) change the receiving buffer for the waveform picker
214 213 ring_node_to_send_cwf_f1 = current_ring_node_f1->previous;
215 214 current_ring_node_f1 = current_ring_node_f1->next;
216 if ( (waveform_picker_regs->status & 0x04) == 0x04)
215 if ( (waveform_picker_regs->status & BIT_WFP_BUF_F1_0) == BIT_WFP_BUF_F1_0)
217 216 {
218 217 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
219 218 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
220 219 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
221 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
220 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F1_0; // [0010 0010 0000 0100] f1 bits = 0
222 221 }
223 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
222 else if ( (waveform_picker_regs->status & BIT_WFP_BUF_F1_1) == BIT_WFP_BUF_F1_1)
224 223 {
225 224 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
226 225 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
227 226 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
228 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
227 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F1_1; // [0010 0010 0000 1000] f1 bits = 0
229 228 }
230 229 // (2) send an event for the the CWF1 task for transmission (and snapshot extraction if needed)
231 230 status = rtems_event_send( Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_NORM_S1_S2 );
232 231 }
233 232
234 233 //***
235 234 // F2
236 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bit
235 if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F2) != INIT_CHAR ) { // [0011 0000] check the f2 full bit
237 236 // (1) change the receiving buffer for the waveform picker
238 237 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
239 238 ring_node_to_send_cwf_f2->sid = SID_SBM2_CWF_F2;
240 239 current_ring_node_f2 = current_ring_node_f2->next;
241 if ( (waveform_picker_regs->status & 0x10) == 0x10)
240 if ( (waveform_picker_regs->status & BIT_WFP_BUF_F2_0) == BIT_WFP_BUF_F2_0)
242 241 {
243 242 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
244 243 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
245 244 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
246 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
245 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_0; // [0100 0100 0001 0000]
247 246 }
248 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
247 else if ( (waveform_picker_regs->status & BIT_WFP_BUF_F2_1) == BIT_WFP_BUF_F2_1)
249 248 {
250 249 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
251 250 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
252 251 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
253 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
252 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_1; // [0100 0100 0010 0000]
254 253 }
255 254 // (2) send an event for the waveforms transmission
256 255 status = rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_NORM_S1_S2 );
257 256 }
258 257 }
259 258
260 259 rtems_isr waveforms_isr( rtems_vector_number vector )
261 260 {
262 261 /** This is the interrupt sub routine called by the waveform picker core.
263 262 *
264 263 * This ISR launch different actions depending mainly on two pieces of information:
265 264 * 1. the values read in the registers of the waveform picker.
266 265 * 2. the current LFR mode.
267 266 *
268 267 */
269 268
270 269 // STATUS
271 270 // new error error buffer full
272 271 // 15 14 13 12 11 10 9 8
273 272 // f3 f2 f1 f0 f3 f2 f1 f0
274 273 //
275 274 // ready buffer
276 275 // 7 6 5 4 3 2 1 0
277 276 // f3_1 f3_0 f2_1 f2_0 f1_1 f1_0 f0_1 f0_0
278 277
279 278 rtems_status_code spare_status;
280 279
281 280 waveforms_isr_f3();
282 281
283 282 //*************************************************
284 283 // copy the status bits in the housekeeping packets
285 284 housekeeping_packet.hk_lfr_vhdl_iir_cal =
286 (unsigned char) ((waveform_picker_regs->status & 0xff00) >> 8);
285 (unsigned char) ((waveform_picker_regs->status & BYTE0_MASK) >> SHIFT_1_BYTE);
287 286
288 if ( (waveform_picker_regs->status & 0xff00) != 0x00) // [1111 1111 0000 0000] check the error bits
287 if ( (waveform_picker_regs->status & BYTE0_MASK) != INIT_CHAR) // [1111 1111 0000 0000] check the error bits
289 288 {
290 289 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_10 );
291 290 }
292 291
293 292 switch(lfrCurrentMode)
294 293 {
295 294 //********
296 295 // STANDBY
297 296 case LFR_MODE_STANDBY:
298 297 break;
299 298 //**************************
300 299 // LFR NORMAL, SBM1 and SBM2
301 300 case LFR_MODE_NORMAL:
302 301 case LFR_MODE_SBM1:
303 302 case LFR_MODE_SBM2:
304 303 waveform_isr_normal_sbm1_sbm2();
305 304 break;
306 305 //******
307 306 // BURST
308 307 case LFR_MODE_BURST:
309 308 waveforms_isr_burst();
310 309 break;
311 310 //********
312 311 // DEFAULT
313 312 default:
314 313 break;
315 314 }
316 315 }
317 316
318 317 //************
319 318 // RTEMS TASKS
320 319
321 320 rtems_task wfrm_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
322 321 {
323 322 /** This RTEMS task is dedicated to the transmission of snapshots of the NORMAL mode.
324 323 *
325 324 * @param unused is the starting argument of the RTEMS task
326 325 *
327 326 * The following data packets are sent by this task:
328 327 * - TM_LFR_SCIENCE_NORMAL_SWF_F0
329 328 * - TM_LFR_SCIENCE_NORMAL_SWF_F1
330 329 * - TM_LFR_SCIENCE_NORMAL_SWF_F2
331 330 *
332 331 */
333 332
334 333 rtems_event_set event_out;
335 334 rtems_id queue_id;
336 335 rtems_status_code status;
337 336 ring_node *ring_node_swf1_extracted_ptr;
338 337 ring_node *ring_node_swf2_extracted_ptr;
339 338
340 339 ring_node_swf1_extracted_ptr = (ring_node *) &ring_node_swf1_extracted;
341 340 ring_node_swf2_extracted_ptr = (ring_node *) &ring_node_swf2_extracted;
342 341
343 342 status = get_message_queue_id_send( &queue_id );
344 343 if (status != RTEMS_SUCCESSFUL)
345 344 {
346 345 PRINTF1("in WFRM *** ERR get_message_queue_id_send %d\n", status);
347 346 }
348 347
349 348 BOOT_PRINTF("in WFRM ***\n");
350 349
351 350 while(1){
352 351 // wait for an RTEMS_EVENT
353 352 rtems_event_receive(RTEMS_EVENT_MODE_NORMAL | RTEMS_EVENT_SWF_RESYNCH,
354 353 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
355 354
356 355 if (event_out == RTEMS_EVENT_MODE_NORMAL)
357 356 {
358 357 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM2\n");
359 358 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
360 359 ring_node_swf1_extracted_ptr->sid = SID_NORM_SWF_F1;
361 360 ring_node_swf2_extracted_ptr->sid = SID_NORM_SWF_F2;
362 361 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
363 362 status = rtems_message_queue_send( queue_id, &ring_node_swf1_extracted_ptr, sizeof( ring_node* ) );
364 363 status = rtems_message_queue_send( queue_id, &ring_node_swf2_extracted_ptr, sizeof( ring_node* ) );
365 364 }
366 365 if (event_out == RTEMS_EVENT_SWF_RESYNCH)
367 366 {
368 367 snapshot_resynchronization( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
369 368 }
370 369 }
371 370 }
372 371
373 372 rtems_task cwf3_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
374 373 {
375 374 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f3.
376 375 *
377 376 * @param unused is the starting argument of the RTEMS task
378 377 *
379 378 * The following data packet is sent by this task:
380 379 * - TM_LFR_SCIENCE_NORMAL_CWF_F3
381 380 *
382 381 */
383 382
384 383 rtems_event_set event_out;
385 384 rtems_id queue_id;
386 385 rtems_status_code status;
387 386 ring_node ring_node_cwf3_light;
388 387 ring_node *ring_node_to_send_cwf;
389 388
390 389 status = get_message_queue_id_send( &queue_id );
391 390 if (status != RTEMS_SUCCESSFUL)
392 391 {
393 392 PRINTF1("in CWF3 *** ERR get_message_queue_id_send %d\n", status)
394 393 }
395 394
396 395 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
397 396
398 397 // init the ring_node_cwf3_light structure
399 398 ring_node_cwf3_light.buffer_address = (int) wf_cont_f3_light;
400 ring_node_cwf3_light.coarseTime = 0x00;
401 ring_node_cwf3_light.fineTime = 0x00;
399 ring_node_cwf3_light.coarseTime = INIT_CHAR;
400 ring_node_cwf3_light.fineTime = INIT_CHAR;
402 401 ring_node_cwf3_light.next = NULL;
403 402 ring_node_cwf3_light.previous = NULL;
404 403 ring_node_cwf3_light.sid = SID_NORM_CWF_F3;
405 ring_node_cwf3_light.status = 0x00;
404 ring_node_cwf3_light.status = INIT_CHAR;
406 405
407 406 BOOT_PRINTF("in CWF3 ***\n");
408 407
409 408 while(1){
410 409 // wait for an RTEMS_EVENT
411 410 rtems_event_receive( RTEMS_EVENT_0,
412 411 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
413 412 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
414 413 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode==LFR_MODE_SBM2) )
415 414 {
416 ring_node_to_send_cwf = getRingNodeToSendCWF( 3 );
417 if ( (parameter_dump_packet.sy_lfr_n_cwf_long_f3 & 0x01) == 0x01)
415 ring_node_to_send_cwf = getRingNodeToSendCWF( CHANNELF3 );
416 if ( (parameter_dump_packet.sy_lfr_n_cwf_long_f3 & BIT_CWF_LONG_F3) == BIT_CWF_LONG_F3)
418 417 {
419 418 PRINTF("send CWF_LONG_F3\n");
420 419 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
421 420 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
422 421 }
423 422 else
424 423 {
425 424 PRINTF("send CWF_F3 (light)\n");
426 425 send_waveform_CWF3_light( ring_node_to_send_cwf, &ring_node_cwf3_light, queue_id );
427 426 }
428 427
429 428 }
430 429 else
431 430 {
432 431 PRINTF1("in CWF3 *** lfrCurrentMode is %d, no data will be sent\n", lfrCurrentMode)
433 432 }
434 433 }
435 434 }
436 435
437 436 rtems_task cwf2_task(rtems_task_argument argument) // ONLY USED IN BURST AND SBM2
438 437 {
439 438 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f2.
440 439 *
441 440 * @param unused is the starting argument of the RTEMS task
442 441 *
443 442 * The following data packet is sent by this function:
444 443 * - TM_LFR_SCIENCE_BURST_CWF_F2
445 444 * - TM_LFR_SCIENCE_SBM2_CWF_F2
446 445 *
447 446 */
448 447
449 448 rtems_event_set event_out;
450 449 rtems_id queue_id;
451 450 rtems_status_code status;
452 451 ring_node *ring_node_to_send;
453 452 unsigned long long int acquisitionTimeF0_asLong;
454 453
455 acquisitionTimeF0_asLong = 0x00;
454 acquisitionTimeF0_asLong = INIT_CHAR;
456 455
457 456 status = get_message_queue_id_send( &queue_id );
458 457 if (status != RTEMS_SUCCESSFUL)
459 458 {
460 459 PRINTF1("in CWF2 *** ERR get_message_queue_id_send %d\n", status)
461 460 }
462 461
463 462 BOOT_PRINTF("in CWF2 ***\n");
464 463
465 464 while(1){
466 465 // wait for an RTEMS_EVENT// send the snapshot when built
467 466 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM2 );
468 467 rtems_event_receive( RTEMS_EVENT_MODE_NORM_S1_S2 | RTEMS_EVENT_MODE_BURST,
469 468 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
470 ring_node_to_send = getRingNodeToSendCWF( 2 );
469 ring_node_to_send = getRingNodeToSendCWF( CHANNELF2 );
471 470 if (event_out == RTEMS_EVENT_MODE_BURST)
472 471 { // data are sent whatever the transition time
473 472 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
474 473 }
475 474 else if (event_out == RTEMS_EVENT_MODE_NORM_S1_S2)
476 475 {
477 476 if ( lfrCurrentMode == LFR_MODE_SBM2 )
478 477 {
479 478 // data are sent depending on the transition time
480 479 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
481 480 {
482 481 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
483 482 }
484 483 }
485 484 // launch snapshot extraction if needed
486 485 if (extractSWF2 == true)
487 486 {
488 487 ring_node_to_send_swf_f2 = ring_node_to_send_cwf_f2;
489 488 // extract the snapshot
490 build_snapshot_from_ring( ring_node_to_send_swf_f2, 2, acquisitionTimeF0_asLong,
489 build_snapshot_from_ring( ring_node_to_send_swf_f2, CHANNELF2, acquisitionTimeF0_asLong,
491 490 &ring_node_swf2_extracted, swf2_extracted );
492 491 extractSWF2 = false;
493 492 swf2_ready = true; // once the snapshot at f2 is ready the CWF1 task will send an event to WFRM
494 493 }
495 494 if (swf0_ready_flag_f2 == true)
496 495 {
497 496 extractSWF2 = true;
498 497 // record the acquition time of the f0 snapshot to use to build the snapshot at f2
499 498 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
500 499 swf0_ready_flag_f2 = false;
501 500 }
502 501 }
503 502 }
504 503 }
505 504
506 505 rtems_task cwf1_task(rtems_task_argument argument) // ONLY USED IN SBM1
507 506 {
508 507 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f1.
509 508 *
510 509 * @param unused is the starting argument of the RTEMS task
511 510 *
512 511 * The following data packet is sent by this function:
513 512 * - TM_LFR_SCIENCE_SBM1_CWF_F1
514 513 *
515 514 */
516 515
517 516 rtems_event_set event_out;
518 517 rtems_id queue_id;
519 518 rtems_status_code status;
520 519
521 520 ring_node *ring_node_to_send_cwf;
522 521
523 522 status = get_message_queue_id_send( &queue_id );
524 523 if (status != RTEMS_SUCCESSFUL)
525 524 {
526 525 PRINTF1("in CWF1 *** ERR get_message_queue_id_send %d\n", status)
527 526 }
528 527
529 528 BOOT_PRINTF("in CWF1 ***\n");
530 529
531 530 while(1){
532 531 // wait for an RTEMS_EVENT
533 532 rtems_event_receive( RTEMS_EVENT_MODE_NORM_S1_S2,
534 533 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
535 534 ring_node_to_send_cwf = getRingNodeToSendCWF( 1 );
536 535 ring_node_to_send_cwf_f1->sid = SID_SBM1_CWF_F1;
537 536 if (lfrCurrentMode == LFR_MODE_SBM1)
538 537 {
539 538 // data are sent depending on the transition time
540 539 if ( time_management_regs->coarse_time >= lastValidEnterModeTime )
541 540 {
542 541 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
543 542 }
544 543 }
545 544 // launch snapshot extraction if needed
546 545 if (extractSWF1 == true)
547 546 {
548 547 ring_node_to_send_swf_f1 = ring_node_to_send_cwf;
549 548 // launch the snapshot extraction
550 549 status = rtems_event_send( Task_id[TASKID_SWBD], RTEMS_EVENT_MODE_NORM_S1_S2 );
551 550 extractSWF1 = false;
552 551 }
553 552 if (swf0_ready_flag_f1 == true)
554 553 {
555 554 extractSWF1 = true;
556 555 swf0_ready_flag_f1 = false; // this step shall be executed only one time
557 556 }
558 557 if ((swf1_ready == true) && (swf2_ready == true)) // swf_f1 is ready after the extraction
559 558 {
560 559 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_NORMAL );
561 560 swf1_ready = false;
562 561 swf2_ready = false;
563 562 }
564 563 }
565 564 }
566 565
567 566 rtems_task swbd_task(rtems_task_argument argument)
568 567 {
569 568 /** This RTEMS task is dedicated to the building of snapshots from different continuous waveforms buffers.
570 569 *
571 570 * @param unused is the starting argument of the RTEMS task
572 571 *
573 572 */
574 573
575 574 rtems_event_set event_out;
576 575 unsigned long long int acquisitionTimeF0_asLong;
577 576
578 acquisitionTimeF0_asLong = 0x00;
577 acquisitionTimeF0_asLong = INIT_CHAR;
579 578
580 579 BOOT_PRINTF("in SWBD ***\n")
581 580
582 581 while(1){
583 582 // wait for an RTEMS_EVENT
584 583 rtems_event_receive( RTEMS_EVENT_MODE_NORM_S1_S2,
585 584 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
586 585 if (event_out == RTEMS_EVENT_MODE_NORM_S1_S2)
587 586 {
588 587 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
589 588 build_snapshot_from_ring( ring_node_to_send_swf_f1, 1, acquisitionTimeF0_asLong,
590 589 &ring_node_swf1_extracted, swf1_extracted );
591 590 swf1_ready = true; // the snapshot has been extracted and is ready to be sent
592 591 }
593 592 else
594 593 {
595 594 PRINTF1("in SWBD *** unexpected rtems event received %x\n", (int) event_out)
596 595 }
597 596 }
598 597 }
599 598
600 599 //******************
601 600 // general functions
602 601
603 602 void WFP_init_rings( void )
604 603 {
605 604 // F0 RING
606 605 init_ring( waveform_ring_f0, NB_RING_NODES_F0, wf_buffer_f0, WFRM_BUFFER );
607 606 // F1 RING
608 607 init_ring( waveform_ring_f1, NB_RING_NODES_F1, wf_buffer_f1, WFRM_BUFFER );
609 608 // F2 RING
610 609 init_ring( waveform_ring_f2, NB_RING_NODES_F2, wf_buffer_f2, WFRM_BUFFER );
611 610 // F3 RING
612 611 init_ring( waveform_ring_f3, NB_RING_NODES_F3, wf_buffer_f3, WFRM_BUFFER );
613 612
614 613 ring_node_swf1_extracted.buffer_address = (int) swf1_extracted;
615 614 ring_node_swf2_extracted.buffer_address = (int) swf2_extracted;
616 615
617 616 DEBUG_PRINTF1("waveform_ring_f0 @%x\n", (unsigned int) waveform_ring_f0)
618 617 DEBUG_PRINTF1("waveform_ring_f1 @%x\n", (unsigned int) waveform_ring_f1)
619 618 DEBUG_PRINTF1("waveform_ring_f2 @%x\n", (unsigned int) waveform_ring_f2)
620 619 DEBUG_PRINTF1("waveform_ring_f3 @%x\n", (unsigned int) waveform_ring_f3)
621 620 DEBUG_PRINTF1("wf_buffer_f0 @%x\n", (unsigned int) wf_buffer_f0)
622 621 DEBUG_PRINTF1("wf_buffer_f1 @%x\n", (unsigned int) wf_buffer_f1)
623 622 DEBUG_PRINTF1("wf_buffer_f2 @%x\n", (unsigned int) wf_buffer_f2)
624 623 DEBUG_PRINTF1("wf_buffer_f3 @%x\n", (unsigned int) wf_buffer_f3)
625 624
626 625 }
627 626
628 627 void WFP_reset_current_ring_nodes( void )
629 628 {
630 629 current_ring_node_f0 = waveform_ring_f0[0].next;
631 630 current_ring_node_f1 = waveform_ring_f1[0].next;
632 631 current_ring_node_f2 = waveform_ring_f2[0].next;
633 632 current_ring_node_f3 = waveform_ring_f3[0].next;
634 633
635 634 ring_node_to_send_swf_f0 = waveform_ring_f0;
636 635 ring_node_to_send_swf_f1 = waveform_ring_f1;
637 636 ring_node_to_send_swf_f2 = waveform_ring_f2;
638 637
639 638 ring_node_to_send_cwf_f1 = waveform_ring_f1;
640 639 ring_node_to_send_cwf_f2 = waveform_ring_f2;
641 640 ring_node_to_send_cwf_f3 = waveform_ring_f3;
642 641 }
643 642
644 643 int send_waveform_CWF3_light( ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id )
645 644 {
646 645 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
647 646 *
648 647 * @param waveform points to the buffer containing the data that will be send.
649 648 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
650 649 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
651 650 * contain information to setup the transmission of the data packets.
652 651 *
653 652 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
654 653 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
655 654 *
656 655 */
657 656
658 657 unsigned int i;
658 unsigned int j;
659 659 int ret;
660 660 rtems_status_code status;
661 661
662 662 char *sample;
663 663 int *dataPtr;
664 664
665 665 ret = LFR_DEFAULT;
666 666
667 667 dataPtr = (int*) ring_node_to_send->buffer_address;
668 668
669 669 ring_node_cwf3_light->coarseTime = ring_node_to_send->coarseTime;
670 670 ring_node_cwf3_light->fineTime = ring_node_to_send->fineTime;
671 671
672 672 //**********************
673 673 // BUILD CWF3_light DATA
674 674 for ( i=0; i< NB_SAMPLES_PER_SNAPSHOT; i++)
675 675 {
676 676 sample = (char*) &dataPtr[ (i * NB_WORDS_SWF_BLK) ];
677 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) ] = sample[ 0 ];
678 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 1 ] = sample[ 1 ];
679 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 2 ] = sample[ 2 ];
680 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 3 ] = sample[ 3 ];
681 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 4 ] = sample[ 4 ];
682 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 5 ] = sample[ 5 ];
677 for (j=0; j < CWF_BLK_SIZE; j++)
678 {
679 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + j] = sample[ j ];
680 }
683 681 }
684 682
685 683 // SEND PACKET
686 684 status = rtems_message_queue_send( queue_id, &ring_node_cwf3_light, sizeof( ring_node* ) );
687 685 if (status != RTEMS_SUCCESSFUL) {
688 686 ret = LFR_DEFAULT;
689 687 }
690 688
691 689 return ret;
692 690 }
693 691
694 692 void compute_acquisition_time( unsigned int coarseTime, unsigned int fineTime,
695 693 unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char * acquisitionTime )
696 694 {
697 695 unsigned long long int acquisitionTimeAsLong;
698 unsigned char localAcquisitionTime[6];
696 unsigned char localAcquisitionTime[BYTES_PER_TIME];
699 697 double deltaT;
700 698
701 deltaT = 0.;
699 deltaT = INIT_FLOAT;
702 700
703 localAcquisitionTime[0] = (unsigned char) ( coarseTime >> 24 );
704 localAcquisitionTime[1] = (unsigned char) ( coarseTime >> 16 );
705 localAcquisitionTime[2] = (unsigned char) ( coarseTime >> 8 );
706 localAcquisitionTime[3] = (unsigned char) ( coarseTime );
707 localAcquisitionTime[4] = (unsigned char) ( fineTime >> 8 );
708 localAcquisitionTime[5] = (unsigned char) ( fineTime );
701 localAcquisitionTime[BYTE_0] = (unsigned char) ( coarseTime >> SHIFT_3_BYTES );
702 localAcquisitionTime[BYTE_1] = (unsigned char) ( coarseTime >> SHIFT_2_BYTES );
703 localAcquisitionTime[BYTE_2] = (unsigned char) ( coarseTime >> SHIFT_1_BYTE );
704 localAcquisitionTime[BYTE_3] = (unsigned char) ( coarseTime );
705 localAcquisitionTime[BYTE_4] = (unsigned char) ( fineTime >> SHIFT_1_BYTE );
706 localAcquisitionTime[BYTE_5] = (unsigned char) ( fineTime );
709 707
710 acquisitionTimeAsLong = ( (unsigned long long int) localAcquisitionTime[0] << 40 )
711 + ( (unsigned long long int) localAcquisitionTime[1] << 32 )
712 + ( (unsigned long long int) localAcquisitionTime[2] << 24 )
713 + ( (unsigned long long int) localAcquisitionTime[3] << 16 )
714 + ( (unsigned long long int) localAcquisitionTime[4] << 8 )
715 + ( (unsigned long long int) localAcquisitionTime[5] );
708 acquisitionTimeAsLong = ( (unsigned long long int) localAcquisitionTime[BYTE_0] << SHIFT_5_BYTES )
709 + ( (unsigned long long int) localAcquisitionTime[BYTE_1] << SHIFT_4_BYTES )
710 + ( (unsigned long long int) localAcquisitionTime[BYTE_2] << SHIFT_3_BYTES )
711 + ( (unsigned long long int) localAcquisitionTime[BYTE_3] << SHIFT_2_BYTES )
712 + ( (unsigned long long int) localAcquisitionTime[BYTE_4] << SHIFT_1_BYTE )
713 + ( (unsigned long long int) localAcquisitionTime[BYTE_5] );
716 714
717 715 switch( sid )
718 716 {
719 717 case SID_NORM_SWF_F0:
720 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 24576. ;
718 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * T0_IN_FINETIME ;
721 719 break;
722 720
723 721 case SID_NORM_SWF_F1:
724 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 4096. ;
722 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * T1_IN_FINETIME ;
725 723 break;
726 724
727 725 case SID_NORM_SWF_F2:
728 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 256. ;
726 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * T2_IN_FINETIME ;
729 727 break;
730 728
731 729 case SID_SBM1_CWF_F1:
732 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 4096. ;
730 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * T1_IN_FINETIME ;
733 731 break;
734 732
735 733 case SID_SBM2_CWF_F2:
736 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
734 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * T2_IN_FINETIME ;
737 735 break;
738 736
739 737 case SID_BURST_CWF_F2:
740 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
738 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * T2_IN_FINETIME ;
741 739 break;
742 740
743 741 case SID_NORM_CWF_F3:
744 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF_SHORT_F3 * 65536. / 16. ;
742 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF_SHORT_F3 * T3_IN_FINETIME ;
745 743 break;
746 744
747 745 case SID_NORM_CWF_LONG_F3:
748 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 16. ;
746 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * T3_IN_FINETIME ;
749 747 break;
750 748
751 749 default:
752 750 PRINTF1("in compute_acquisition_time *** ERR unexpected sid %d\n", sid)
753 751 deltaT = 0.;
754 752 break;
755 753 }
756 754
757 755 acquisitionTimeAsLong = acquisitionTimeAsLong + (unsigned long long int) deltaT;
758 756 //
759 acquisitionTime[0] = (unsigned char) (acquisitionTimeAsLong >> 40);
760 acquisitionTime[1] = (unsigned char) (acquisitionTimeAsLong >> 32);
761 acquisitionTime[2] = (unsigned char) (acquisitionTimeAsLong >> 24);
762 acquisitionTime[3] = (unsigned char) (acquisitionTimeAsLong >> 16);
763 acquisitionTime[4] = (unsigned char) (acquisitionTimeAsLong >> 8 );
764 acquisitionTime[5] = (unsigned char) (acquisitionTimeAsLong );
757 acquisitionTime[BYTE_0] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_5_BYTES);
758 acquisitionTime[BYTE_1] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_4_BYTES);
759 acquisitionTime[BYTE_2] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_3_BYTES);
760 acquisitionTime[BYTE_3] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_2_BYTES);
761 acquisitionTime[BYTE_4] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_1_BYTE );
762 acquisitionTime[BYTE_5] = (unsigned char) (acquisitionTimeAsLong );
765 763
766 764 }
767 765
768 766 void build_snapshot_from_ring( ring_node *ring_node_to_send,
769 767 unsigned char frequencyChannel,
770 768 unsigned long long int acquisitionTimeF0_asLong,
771 769 ring_node *ring_node_swf_extracted,
772 770 int *swf_extracted)
773 771 {
774 772 unsigned int i;
775 773 unsigned long long int centerTime_asLong;
776 774 unsigned long long int acquisitionTime_asLong;
777 775 unsigned long long int bufferAcquisitionTime_asLong;
778 776 unsigned char *ptr1;
779 777 unsigned char *ptr2;
780 778 unsigned char *timeCharPtr;
781 779 unsigned char nb_ring_nodes;
782 780 unsigned long long int frequency_asLong;
783 781 unsigned long long int nbTicksPerSample_asLong;
784 782 unsigned long long int nbSamplesPart1_asLong;
785 783 unsigned long long int sampleOffset_asLong;
786 784
787 785 unsigned int deltaT_F0;
788 786 unsigned int deltaT_F1;
789 787 unsigned long long int deltaT_F2;
790 788
791 deltaT_F0 = 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
792 deltaT_F1 = 16384; // (2048. / 4096. / 2.) * 65536. = 16384;
793 deltaT_F2 = 262144; // (2048. / 256. / 2.) * 65536. = 262144;
794 sampleOffset_asLong = 0x00;
789 deltaT_F0 = DELTAT_F0;
790 deltaT_F1 = DELTAF_F1;
791 deltaT_F2 = DELTAF_F2;
792 sampleOffset_asLong = INIT_CHAR;
795 793
796 794 // (1) get the f0 acquisition time => the value is passed in argument
797 795
798 796 // (2) compute the central reference time
799 797 centerTime_asLong = acquisitionTimeF0_asLong + deltaT_F0;
800 798
801 799 // (3) compute the acquisition time of the current snapshot
802 800 switch(frequencyChannel)
803 801 {
804 case 1: // 1 is for F1 = 4096 Hz
802 case CHANNELF1: // 1 is for F1 = 4096 Hz
805 803 acquisitionTime_asLong = centerTime_asLong - deltaT_F1;
806 804 nb_ring_nodes = NB_RING_NODES_F1;
807 frequency_asLong = 4096;
808 nbTicksPerSample_asLong = 16; // 65536 / 4096;
805 frequency_asLong = FREQ_F1;
806 nbTicksPerSample_asLong = TICKS_PER_T1; // 65536 / 4096;
809 807 break;
810 case 2: // 2 is for F2 = 256 Hz
808 case CHANNELF2: // 2 is for F2 = 256 Hz
811 809 acquisitionTime_asLong = centerTime_asLong - deltaT_F2;
812 810 nb_ring_nodes = NB_RING_NODES_F2;
813 frequency_asLong = 256;
814 nbTicksPerSample_asLong = 256; // 65536 / 256;
811 frequency_asLong = FREQ_F2;
812 nbTicksPerSample_asLong = TICKS_PER_T2; // 65536 / 256;
815 813 break;
816 814 default:
817 815 acquisitionTime_asLong = centerTime_asLong;
818 frequency_asLong = 256;
819 nbTicksPerSample_asLong = 256;
816 frequency_asLong = FREQ_F2;
817 nbTicksPerSample_asLong = TICKS_PER_T2;
820 818 break;
821 819 }
822 820
823 821 //****************************************************************************
824 822 // (4) search the ring_node with the acquisition time <= acquisitionTime_asLong
825 823 for (i=0; i<nb_ring_nodes; i++)
826 824 {
827 825 //PRINTF1("%d ... ", i);
828 826 bufferAcquisitionTime_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send->coarseTime );
829 827 if (bufferAcquisitionTime_asLong <= acquisitionTime_asLong)
830 828 {
831 829 //PRINTF1("buffer found with acquisition time = %llx\n", bufferAcquisitionTime_asLong);
832 830 break;
833 831 }
834 832 ring_node_to_send = ring_node_to_send->previous;
835 833 }
836 834
837 835 // (5) compute the number of samples to take in the current buffer
838 sampleOffset_asLong = ((acquisitionTime_asLong - bufferAcquisitionTime_asLong) * frequency_asLong ) >> 16;
836 sampleOffset_asLong = ((acquisitionTime_asLong - bufferAcquisitionTime_asLong) * frequency_asLong ) >> SHIFT_2_BYTES;
839 837 nbSamplesPart1_asLong = NB_SAMPLES_PER_SNAPSHOT - sampleOffset_asLong;
840 838 //PRINTF2("sampleOffset_asLong = %lld, nbSamplesPart1_asLong = %lld\n", sampleOffset_asLong, nbSamplesPart1_asLong);
841 839
842 // (6) compute the final acquisition time
843 acquisitionTime_asLong = bufferAcquisitionTime_asLong +
844 sampleOffset_asLong * nbTicksPerSample_asLong;
840 // (6) compute the final acquisition time
841 acquisitionTime_asLong = bufferAcquisitionTime_asLong +
842 (sampleOffset_asLong * nbTicksPerSample_asLong);
845 843
846 844 // (7) copy the acquisition time at the beginning of the extrated snapshot
847 845 ptr1 = (unsigned char*) &acquisitionTime_asLong;
848 846 // fine time
849 847 ptr2 = (unsigned char*) &ring_node_swf_extracted->fineTime;
850 ptr2[2] = ptr1[ 4 + 2 ];
851 ptr2[3] = ptr1[ 5 + 2 ];
848 ptr2[BYTE_2] = ptr1[ BYTE_4 + OFFSET_2_BYTES ];
849 ptr2[BYTE_3] = ptr1[ BYTE_5 + OFFSET_2_BYTES ];
852 850 // coarse time
853 851 ptr2 = (unsigned char*) &ring_node_swf_extracted->coarseTime;
854 ptr2[0] = ptr1[ 0 + 2 ];
855 ptr2[1] = ptr1[ 1 + 2 ];
856 ptr2[2] = ptr1[ 2 + 2 ];
857 ptr2[3] = ptr1[ 3 + 2 ];
852 ptr2[BYTE_0] = ptr1[ BYTE_0 + OFFSET_2_BYTES ];
853 ptr2[BYTE_1] = ptr1[ BYTE_1 + OFFSET_2_BYTES ];
854 ptr2[BYTE_2] = ptr1[ BYTE_2 + OFFSET_2_BYTES ];
855 ptr2[BYTE_3] = ptr1[ BYTE_3 + OFFSET_2_BYTES ];
858 856
859 857 // re set the synchronization bit
860 858 timeCharPtr = (unsigned char*) &ring_node_to_send->coarseTime;
861 ptr2[0] = ptr2[0] | (timeCharPtr[0] & 0x80); // [1000 0000]
859 ptr2[0] = ptr2[0] | (timeCharPtr[0] & SYNC_BIT); // [1000 0000]
862 860
863 861 if ( (nbSamplesPart1_asLong >= NB_SAMPLES_PER_SNAPSHOT) | (nbSamplesPart1_asLong < 0) )
864 862 {
865 863 nbSamplesPart1_asLong = 0;
866 864 }
867 865 // copy the part 1 of the snapshot in the extracted buffer
868 866 for ( i = 0; i < (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i++ )
869 867 {
870 868 swf_extracted[i] =
871 869 ((int*) ring_node_to_send->buffer_address)[ i + (sampleOffset_asLong * NB_WORDS_SWF_BLK) ];
872 870 }
873 871 // copy the part 2 of the snapshot in the extracted buffer
874 872 ring_node_to_send = ring_node_to_send->next;
875 873 for ( i = (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i < (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK); i++ )
876 874 {
877 875 swf_extracted[i] =
878 876 ((int*) ring_node_to_send->buffer_address)[ (i-(nbSamplesPart1_asLong * NB_WORDS_SWF_BLK)) ];
879 877 }
880 878 }
881 879
882 880 double computeCorrection( unsigned char *timePtr )
883 881 {
884 882 unsigned long long int acquisitionTime;
885 883 unsigned long long int centerTime;
886 884 unsigned long long int previousTick;
887 885 unsigned long long int nextTick;
888 886 unsigned long long int deltaPreviousTick;
889 887 unsigned long long int deltaNextTick;
890 888 double deltaPrevious_ms;
891 889 double deltaNext_ms;
892 890 double correctionInF2;
893 891
894 892 // get acquisition time in fine time ticks
895 893 acquisitionTime = get_acquisition_time( timePtr );
896 894
897 895 // compute center time
898 centerTime = acquisitionTime + 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
899 previousTick = centerTime - (centerTime & 0xffff);
900 nextTick = previousTick + 65536;
896 centerTime = acquisitionTime + DELTAT_F0; // (2048. / 24576. / 2.) * 65536. = 2730.667;
897 previousTick = centerTime - (centerTime & INT16_ALL_F);
898 nextTick = previousTick + TICKS_PER_S;
901 899
902 900 deltaPreviousTick = centerTime - previousTick;
903 901 deltaNextTick = nextTick - centerTime;
904 902
905 deltaPrevious_ms = ((double) deltaPreviousTick) / 65536. * 1000.;
906 deltaNext_ms = ((double) deltaNextTick) / 65536. * 1000.;
903 deltaPrevious_ms = (((double) deltaPreviousTick) / TICKS_PER_S) * MS_PER_S;
904 deltaNext_ms = (((double) deltaNextTick) / TICKS_PER_S) * MS_PER_S;
907 905
908 906 PRINTF2(" delta previous = %.3f ms, delta next = %.2f ms\n", deltaPrevious_ms, deltaNext_ms);
909 907
910 908 // which tick is the closest?
911 909 if (deltaPreviousTick > deltaNextTick)
912 910 {
913 911 // the snapshot center is just before the second => increase delta_snapshot
914 correctionInF2 = + (deltaNext_ms * 256. / 1000. );
912 correctionInF2 = + (deltaNext_ms * FREQ_F2 / MS_PER_S );
915 913 }
916 914 else
917 915 {
918 916 // the snapshot center is just after the second => decrease delta_snapshot
919 correctionInF2 = - (deltaPrevious_ms * 256. / 1000. );
917 correctionInF2 = - (deltaPrevious_ms * FREQ_F2 / MS_PER_S );
920 918 }
921 919
922 920 PRINTF1(" correctionInF2 = %.2f\n", correctionInF2);
923 921
924 922 return correctionInF2;
925 923 }
926 924
927 925 void applyCorrection( double correction )
928 926 {
929 927 int correctionInt;
930 928
931 929 if (correction >= 0.)
932 930 {
933 if ( (1. > correction) && (correction > 0.5) )
931 if ( (ONE_TICK_CORR_INTERVAL_0_MIN < correction) && (correction < ONE_TICK_CORR_INTERVAL_0_MAX) )
934 932 {
935 correctionInt = 1;
933 correctionInt = ONE_TICK_CORR;
936 934 }
937 935 else
938 936 {
939 correctionInt = 2 * floor(correction);
937 correctionInt = CORR_MULT * floor(correction);
940 938 }
941 939 }
942 940 else
943 941 {
944 if ( (-1. < correction) && (correction < -0.5) )
942 if ( (ONE_TICK_CORR_INTERVAL_1_MIN < correction) && (correction < ONE_TICK_CORR_INTERVAL_1_MAX) )
945 943 {
946 correctionInt = -1;
944 correctionInt = -ONE_TICK_CORR;
947 945 }
948 946 else
949 947 {
950 correctionInt = 2 * ceil(correction);
948 correctionInt = CORR_MULT * ceil(correction);
951 949 }
952 950 }
953 951 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot + correctionInt;
954 952 }
955 953
956 954 void snapshot_resynchronization( unsigned char *timePtr )
957 955 {
958 956 /** This function compute a correction to apply on delta_snapshot.
959 957 *
960 958 *
961 959 * @param timePtr is a pointer to the acquisition time of the snapshot being considered.
962 960 *
963 961 * @return void
964 962 *
965 963 */
966 964
967 static double correction = 0.;
965 static double correction = INIT_FLOAT;
968 966 static resynchro_state state = MEASURE;
969 967 static unsigned int nbSnapshots = 0;
970 968
971 969 int correctionInt;
972 970
973 971 correctionInt = 0;
974 972
975 973 switch (state)
976 974 {
977 975
978 976 case MEASURE:
979 977 // ********
980 978 PRINTF1("MEASURE === %d\n", nbSnapshots);
981 979 state = CORRECTION;
982 980 correction = computeCorrection( timePtr );
983 981 PRINTF1("MEASURE === correction = %.2f\n", correction );
984 982 applyCorrection( correction );
985 983 PRINTF1("MEASURE === delta_snapshot = %d\n", waveform_picker_regs->delta_snapshot);
986 984 //****
987 985 break;
988 986
989 987 case CORRECTION:
990 988 //************
991 989 PRINTF1("CORRECTION === %d\n", nbSnapshots);
992 990 state = MEASURE;
993 991 computeCorrection( timePtr );
994 992 set_wfp_delta_snapshot();
995 993 PRINTF1("CORRECTION === delta_snapshot = %d\n", waveform_picker_regs->delta_snapshot);
996 994 //****
997 995 break;
998 996
999 997 default:
1000 998 break;
1001 999
1002 1000 }
1003 1001
1004 1002 nbSnapshots++;
1005 1003 }
1006 1004
1007 1005 //**************
1008 1006 // wfp registers
1009 1007 void reset_wfp_burst_enable( void )
1010 1008 {
1011 1009 /** This function resets the waveform picker burst_enable register.
1012 1010 *
1013 1011 * The burst bits [f2 f1 f0] and the enable bits [f3 f2 f1 f0] are set to 0.
1014 1012 *
1015 1013 */
1016 1014
1017 1015 // [1000 000] burst f2, f1, f0 enable f3, f2, f1, f0
1018 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable & 0x80;
1016 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable & RST_BITS_RUN_BURST_EN;
1019 1017 }
1020 1018
1021 1019 void reset_wfp_status( void )
1022 1020 {
1023 1021 /** This function resets the waveform picker status register.
1024 1022 *
1025 1023 * All status bits are set to 0 [new_err full_err full].
1026 1024 *
1027 1025 */
1028 1026
1029 waveform_picker_regs->status = 0xffff;
1027 waveform_picker_regs->status = INT16_ALL_F;
1030 1028 }
1031 1029
1032 1030 void reset_wfp_buffer_addresses( void )
1033 1031 {
1034 1032 // F0
1035 1033 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->previous->buffer_address; // 0x08
1036 1034 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address; // 0x0c
1037 1035 // F1
1038 1036 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->previous->buffer_address; // 0x10
1039 1037 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address; // 0x14
1040 1038 // F2
1041 1039 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->previous->buffer_address; // 0x18
1042 1040 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; // 0x1c
1043 1041 // F3
1044 1042 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->previous->buffer_address; // 0x20
1045 1043 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address; // 0x24
1046 1044 }
1047 1045
1048 1046 void reset_waveform_picker_regs( void )
1049 1047 {
1050 1048 /** This function resets the waveform picker module registers.
1051 1049 *
1052 1050 * The registers affected by this function are located at the following offset addresses:
1053 1051 * - 0x00 data_shaping
1054 1052 * - 0x04 run_burst_enable
1055 1053 * - 0x08 addr_data_f0
1056 1054 * - 0x0C addr_data_f1
1057 1055 * - 0x10 addr_data_f2
1058 1056 * - 0x14 addr_data_f3
1059 1057 * - 0x18 status
1060 1058 * - 0x1C delta_snapshot
1061 1059 * - 0x20 delta_f0
1062 1060 * - 0x24 delta_f0_2
1063 1061 * - 0x28 delta_f1 (obsolet parameter)
1064 1062 * - 0x2c delta_f2
1065 1063 * - 0x30 nb_data_by_buffer
1066 1064 * - 0x34 nb_snapshot_param
1067 1065 * - 0x38 start_date
1068 1066 * - 0x3c nb_word_in_buffer
1069 1067 *
1070 1068 */
1071 1069
1072 1070 set_wfp_data_shaping(); // 0x00 *** R1 R0 SP1 SP0 BW
1073 1071
1074 1072 reset_wfp_burst_enable(); // 0x04 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
1075 1073
1076 1074 reset_wfp_buffer_addresses();
1077 1075
1078 1076 reset_wfp_status(); // 0x18
1079 1077
1080 1078 set_wfp_delta_snapshot(); // 0x1c *** 300 s => 0x12bff
1081 1079
1082 1080 set_wfp_delta_f0_f0_2(); // 0x20, 0x24
1083 1081
1084 1082 //the parameter delta_f1 [0x28] is not used anymore
1085 1083
1086 1084 set_wfp_delta_f2(); // 0x2c
1087 1085
1088 1086 DEBUG_PRINTF1("delta_snapshot %x\n", waveform_picker_regs->delta_snapshot);
1089 1087 DEBUG_PRINTF1("delta_f0 %x\n", waveform_picker_regs->delta_f0);
1090 1088 DEBUG_PRINTF1("delta_f0_2 %x\n", waveform_picker_regs->delta_f0_2);
1091 1089 DEBUG_PRINTF1("delta_f1 %x\n", waveform_picker_regs->delta_f1);
1092 1090 DEBUG_PRINTF1("delta_f2 %x\n", waveform_picker_regs->delta_f2);
1093 1091 // 2688 = 8 * 336
1094 waveform_picker_regs->nb_data_by_buffer = 0xa7f; // 0x30 *** 2688 - 1 => nb samples -1
1095 waveform_picker_regs->snapshot_param = 0xa80; // 0x34 *** 2688 => nb samples
1096 waveform_picker_regs->start_date = 0x7fffffff; // 0x38
1092 waveform_picker_regs->nb_data_by_buffer = DFLT_WFP_NB_DATA_BY_BUFFER; // 0x30 *** 2688 - 1 => nb samples -1
1093 waveform_picker_regs->snapshot_param = DFLT_WFP_SNAPSHOT_PARAM; // 0x34 *** 2688 => nb samples
1094 waveform_picker_regs->start_date = COARSE_TIME_MASK;
1097 1095 //
1098 1096 // coarse time and fine time registers are not initialized, they are volatile
1099 1097 //
1100 waveform_picker_regs->buffer_length = 0x1f8;// buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
1098 waveform_picker_regs->buffer_length = DFLT_WFP_BUFFER_LENGTH; // buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
1101 1099 }
1102 1100
1103 1101 void set_wfp_data_shaping( void )
1104 1102 {
1105 1103 /** This function sets the data_shaping register of the waveform picker module.
1106 1104 *
1107 1105 * The value is read from one field of the parameter_dump_packet structure:\n
1108 1106 * bw_sp0_sp1_r0_r1
1109 1107 *
1110 1108 */
1111 1109
1112 1110 unsigned char data_shaping;
1113 1111
1114 1112 // get the parameters for the data shaping [BW SP0 SP1 R0 R1] in sy_lfr_common1 and configure the register
1115 1113 // waveform picker : [R1 R0 SP1 SP0 BW]
1116 1114
1117 1115 data_shaping = parameter_dump_packet.sy_lfr_common_parameters;
1118 1116
1119 1117 waveform_picker_regs->data_shaping =
1120 ( (data_shaping & 0x20) >> 5 ) // BW
1121 + ( (data_shaping & 0x10) >> 3 ) // SP0
1122 + ( (data_shaping & 0x08) >> 1 ) // SP1
1123 + ( (data_shaping & 0x04) << 1 ) // R0
1124 + ( (data_shaping & 0x02) << 3 ) // R1
1125 + ( (data_shaping & 0x01) << 5 ); // R2
1118 ( (data_shaping & BIT_5) >> SHIFT_5_BITS ) // BW
1119 + ( (data_shaping & BIT_4) >> SHIFT_3_BITS ) // SP0
1120 + ( (data_shaping & BIT_3) >> 1 ) // SP1
1121 + ( (data_shaping & BIT_2) << 1 ) // R0
1122 + ( (data_shaping & BIT_1) << SHIFT_3_BITS ) // R1
1123 + ( (data_shaping & BIT_0) << SHIFT_5_BITS ); // R2
1126 1124 }
1127 1125
1128 1126 void set_wfp_burst_enable_register( unsigned char mode )
1129 1127 {
1130 1128 /** This function sets the waveform picker burst_enable register depending on the mode.
1131 1129 *
1132 1130 * @param mode is the LFR mode to launch.
1133 1131 *
1134 1132 * The burst bits shall be before the enable bits.
1135 1133 *
1136 1134 */
1137 1135
1138 1136 // [0000 0000] burst f2, f1, f0 enable f3 f2 f1 f0
1139 1137 // the burst bits shall be set first, before the enable bits
1140 1138 switch(mode) {
1141 1139 case LFR_MODE_NORMAL:
1142 1140 case LFR_MODE_SBM1:
1143 1141 case LFR_MODE_SBM2:
1144 waveform_picker_regs->run_burst_enable = 0x60; // [0110 0000] enable f2 and f1 burst
1142 waveform_picker_regs->run_burst_enable = RUN_BURST_ENABLE_SBM2; // [0110 0000] enable f2 and f1 burst
1145 1143 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1146 1144 break;
1147 1145 case LFR_MODE_BURST:
1148 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1146 waveform_picker_regs->run_burst_enable = RUN_BURST_ENABLE_BURST; // [0100 0000] f2 burst enabled
1149 1147 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0c; // [1100] enable f3 and f2
1150 1148 break;
1151 1149 default:
1152 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enabled, no waveform enabled
1150 waveform_picker_regs->run_burst_enable = INIT_CHAR; // [0000 0000] no burst enabled, no waveform enabled
1153 1151 break;
1154 1152 }
1155 1153 }
1156 1154
1157 1155 void set_wfp_delta_snapshot( void )
1158 1156 {
1159 1157 /** This function sets the delta_snapshot register of the waveform picker module.
1160 1158 *
1161 1159 * The value is read from two (unsigned char) of the parameter_dump_packet structure:
1162 1160 * - sy_lfr_n_swf_p[0]
1163 1161 * - sy_lfr_n_swf_p[1]
1164 1162 *
1165 1163 */
1166 1164
1167 1165 unsigned int delta_snapshot;
1168 1166 unsigned int delta_snapshot_in_T2;
1169 1167
1170 delta_snapshot = parameter_dump_packet.sy_lfr_n_swf_p[0]*256
1168 delta_snapshot = (parameter_dump_packet.sy_lfr_n_swf_p[0] * CONST_256)
1171 1169 + parameter_dump_packet.sy_lfr_n_swf_p[1];
1172 1170
1173 delta_snapshot_in_T2 = delta_snapshot * 256;
1171 delta_snapshot_in_T2 = delta_snapshot * FREQ_F2;
1174 1172 waveform_picker_regs->delta_snapshot = delta_snapshot_in_T2 - 1; // max 4 bytes
1175 1173 }
1176 1174
1177 1175 void set_wfp_delta_f0_f0_2( void )
1178 1176 {
1179 1177 unsigned int delta_snapshot;
1180 1178 unsigned int nb_samples_per_snapshot;
1181 1179 float delta_f0_in_float;
1182 1180
1183 1181 delta_snapshot = waveform_picker_regs->delta_snapshot;
1184 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1185 delta_f0_in_float = nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 24576.) * 256.;
1182 nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_swf_l[1];
1183 delta_f0_in_float = (nb_samples_per_snapshot / 2.) * ( (1. / FREQ_F2) - (1. / FREQ_F0) ) * FREQ_F2;
1186 1184
1187 1185 waveform_picker_regs->delta_f0 = delta_snapshot - floor( delta_f0_in_float );
1188 waveform_picker_regs->delta_f0_2 = 0x30; // 48 = 11 0000, max 7 bits
1186 waveform_picker_regs->delta_f0_2 = DFLT_WFP_DELTA_F0_2; // 48 = 11 0000, max 7 bits
1189 1187 }
1190 1188
1191 1189 void set_wfp_delta_f1( void )
1192 1190 {
1193 1191 /** Sets the value of the delta_f1 parameter
1194 1192 *
1195 1193 * @param void
1196 1194 *
1197 1195 * @return void
1198 1196 *
1199 1197 * delta_f1 is not used, the snapshots are extracted from CWF_F1 waveforms.
1200 1198 *
1201 1199 */
1202 1200
1203 1201 unsigned int delta_snapshot;
1204 1202 unsigned int nb_samples_per_snapshot;
1205 1203 float delta_f1_in_float;
1206 1204
1207 1205 delta_snapshot = waveform_picker_regs->delta_snapshot;
1208 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1209 delta_f1_in_float = nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 4096.) * 256.;
1206 nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_swf_l[1];
1207 delta_f1_in_float = (nb_samples_per_snapshot / 2.) * ( (1. / FREQ_F2) - (1. / FREQ_F1) ) * FREQ_F2;
1210 1208
1211 1209 waveform_picker_regs->delta_f1 = delta_snapshot - floor( delta_f1_in_float );
1212 1210 }
1213 1211
1214 1212 void set_wfp_delta_f2( void ) // parameter not used, only delta_f0 and delta_f0_2 are used
1215 1213 {
1216 1214 /** Sets the value of the delta_f2 parameter
1217 1215 *
1218 1216 * @param void
1219 1217 *
1220 1218 * @return void
1221 1219 *
1222 1220 * delta_f2 is used only for the first snapshot generation, even when the snapshots are extracted from CWF_F2
1223 1221 * waveforms (see lpp_waveform_snapshot_controler.vhd for details).
1224 1222 *
1225 1223 */
1226 1224
1227 1225 unsigned int delta_snapshot;
1228 1226 unsigned int nb_samples_per_snapshot;
1229 1227
1230 1228 delta_snapshot = waveform_picker_regs->delta_snapshot;
1231 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1229 nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_swf_l[1];
1232 1230
1233 waveform_picker_regs->delta_f2 = delta_snapshot - nb_samples_per_snapshot / 2 - 1;
1231 waveform_picker_regs->delta_f2 = delta_snapshot - (nb_samples_per_snapshot / 2) - 1;
1234 1232 }
1235 1233
1236 1234 //*****************
1237 1235 // local parameters
1238 1236
1239 1237 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid )
1240 1238 {
1241 1239 /** This function increments the parameter "sequence_cnt" depending on the sid passed in argument.
1242 1240 *
1243 1241 * @param packet_sequence_control is a pointer toward the parameter sequence_cnt to update.
1244 1242 * @param sid is the source identifier of the packet being updated.
1245 1243 *
1246 1244 * REQ-LFR-SRS-5240 / SSS-CP-FS-590
1247 1245 * The sequence counters shall wrap around from 2^14 to zero.
1248 1246 * The sequence counter shall start at zero at startup.
1249 1247 *
1250 1248 * REQ-LFR-SRS-5239 / SSS-CP-FS-580
1251 1249 * All TM_LFR_SCIENCE_ packets are sent to ground, i.e. destination id = 0
1252 1250 *
1253 1251 */
1254 1252
1255 1253 unsigned short *sequence_cnt;
1256 1254 unsigned short segmentation_grouping_flag;
1257 1255 unsigned short new_packet_sequence_control;
1258 1256 rtems_mode initial_mode_set;
1259 1257 rtems_mode current_mode_set;
1260 1258 rtems_status_code status;
1261 1259
1262 1260 //******************************************
1263 1261 // CHANGE THE MODE OF THE CALLING RTEMS TASK
1264 1262 status = rtems_task_mode( RTEMS_NO_PREEMPT, RTEMS_PREEMPT_MASK, &initial_mode_set );
1265 1263
1266 1264 if ( (sid == SID_NORM_SWF_F0) || (sid == SID_NORM_SWF_F1) || (sid == SID_NORM_SWF_F2)
1267 1265 || (sid == SID_NORM_CWF_F3) || (sid == SID_NORM_CWF_LONG_F3)
1268 1266 || (sid == SID_BURST_CWF_F2)
1269 1267 || (sid == SID_NORM_ASM_F0) || (sid == SID_NORM_ASM_F1) || (sid == SID_NORM_ASM_F2)
1270 1268 || (sid == SID_NORM_BP1_F0) || (sid == SID_NORM_BP1_F1) || (sid == SID_NORM_BP1_F2)
1271 1269 || (sid == SID_NORM_BP2_F0) || (sid == SID_NORM_BP2_F1) || (sid == SID_NORM_BP2_F2)
1272 1270 || (sid == SID_BURST_BP1_F0) || (sid == SID_BURST_BP2_F0)
1273 1271 || (sid == SID_BURST_BP1_F1) || (sid == SID_BURST_BP2_F1) )
1274 1272 {
1275 1273 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_NORMAL_BURST;
1276 1274 }
1277 1275 else if ( (sid ==SID_SBM1_CWF_F1) || (sid ==SID_SBM2_CWF_F2)
1278 1276 || (sid == SID_SBM1_BP1_F0) || (sid == SID_SBM1_BP2_F0)
1279 1277 || (sid == SID_SBM2_BP1_F0) || (sid == SID_SBM2_BP2_F0)
1280 1278 || (sid == SID_SBM2_BP1_F1) || (sid == SID_SBM2_BP2_F1) )
1281 1279 {
1282 1280 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_SBM1_SBM2;
1283 1281 }
1284 1282 else
1285 1283 {
1286 1284 sequence_cnt = (unsigned short *) NULL;
1287 1285 PRINTF1("in increment_seq_counter_source_id *** ERR apid_destid %d not known\n", sid)
1288 1286 }
1289 1287
1290 1288 if (sequence_cnt != NULL)
1291 1289 {
1292 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
1293 *sequence_cnt = (*sequence_cnt) & 0x3fff;
1290 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
1291 *sequence_cnt = (*sequence_cnt) & SEQ_CNT_MASK;
1294 1292
1295 1293 new_packet_sequence_control = segmentation_grouping_flag | (*sequence_cnt) ;
1296 1294
1297 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
1295 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> SHIFT_1_BYTE);
1298 1296 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1299 1297
1300 1298 // increment the sequence counter
1301 1299 if ( *sequence_cnt < SEQ_CNT_MAX)
1302 1300 {
1303 1301 *sequence_cnt = *sequence_cnt + 1;
1304 1302 }
1305 1303 else
1306 1304 {
1307 1305 *sequence_cnt = 0;
1308 1306 }
1309 1307 }
1310 1308
1311 1309 //*************************************
1312 1310 // RESTORE THE MODE OF THE CALLING TASK
1313 1311 status = rtems_task_mode( initial_mode_set, RTEMS_PREEMPT_MASK, &current_mode_set );
1314 1312 }
General Comments 0
You need to be logged in to leave comments. Login now