##// END OF EJS Templates
compliance with ICD 4.3...
paul -
r328:5023f9ef69f2 R3++ draft
parent child
Show More
@@ -1,2 +1,2
1 1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
2 7c46de6059673d3239fcc7103e16510727f35923 header/lfr_common_headers
2 058c1234c2defe215d3dd655a7ef65abb33c922d header/lfr_common_headers
@@ -1,64 +1,59
1 1 #ifndef FSW_INIT_H_INCLUDED
2 2 #define FSW_INIT_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <leon.h>
6 6
7 7 #include "fsw_params.h"
8 8 #include "fsw_misc.h"
9 9 #include "fsw_processing.h"
10 10
11 11 #include "tc_handler.h"
12 12 #include "wf_handler.h"
13 13 #include "fsw_spacewire.h"
14 14
15 15 #include "avf0_prc0.h"
16 16 #include "avf1_prc1.h"
17 17 #include "avf2_prc2.h"
18 18
19 19 extern rtems_name Task_name[]; /* array of task names */
20 20 extern rtems_id Task_id[]; /* array of task ids */
21 21 extern rtems_name timecode_timer_name;
22 22 extern rtems_id timecode_timer_id;
23 23 extern unsigned char pa_bia_status_info;
24 extern unsigned char cp_rpw_sc_rw_f_flags;
25 extern float cp_rpw_sc_rw1_f1;
26 extern float cp_rpw_sc_rw1_f2;
27 extern float cp_rpw_sc_rw2_f1;
28 extern float cp_rpw_sc_rw2_f2;
29 extern float cp_rpw_sc_rw3_f1;
30 extern float cp_rpw_sc_rw3_f2;
31 extern float cp_rpw_sc_rw4_f1;
32 extern float cp_rpw_sc_rw4_f2;
24 extern unsigned char cp_rpw_sc_rw1_rw2_f_flags;
25 extern unsigned char cp_rpw_sc_rw3_rw4_f_flags;
26
33 27 extern filterPar_t filterPar;
28 extern rw_f_t rw_f;
34 29
35 30 // RTEMS TASKS
36 31 rtems_task Init( rtems_task_argument argument);
37 32
38 33 // OTHER functions
39 34 void create_names( void );
40 35 int create_all_tasks( void );
41 36 int start_all_tasks( void );
42 37 //
43 38 rtems_status_code create_message_queues( void );
44 39 rtems_status_code create_timecode_timer( void );
45 40 rtems_status_code get_message_queue_id_send( rtems_id *queue_id );
46 41 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id );
47 42 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id );
48 43 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
49 44 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
50 45 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max );
51 46 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize );
52 47 //
53 48 int start_recv_send_tasks( void );
54 49 //
55 50 void init_local_mode_parameters( void );
56 51 void reset_local_time( void );
57 52
58 53 extern void rtems_cpu_usage_report( void );
59 54 extern void rtems_cpu_usage_reset( void );
60 55 extern void rtems_stack_checker_report_usage( void );
61 56
62 57 extern int sched_yield( void );
63 58
64 59 #endif // FSW_INIT_H_INCLUDED
@@ -1,101 +1,117
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 14 #define FLOAT_EQUAL_ZERO 0.001
15 15 #define NB_BINS_TO_REMOVE 3
16 16 #define FI_INTERVAL_COEFF 0.285
17 17 #define BIN_MIN 0
18 18 #define BIN_MAX 127
19 19 #define DELTAF_F0 96.
20 20 #define DELTAF_F1 16.
21 21 #define DELTAF_F2 1.
22 22
23 23 #define BIT_RW1_F1 0x80
24 24 #define BIT_RW1_F2 0x40
25 25 #define BIT_RW2_F1 0x20
26 26 #define BIT_RW2_F2 0x10
27 27 #define BIT_RW3_F1 0x08
28 28 #define BIT_RW3_F2 0x04
29 29 #define BIT_RW4_F1 0x02
30 30 #define BIT_RW4_F2 0x01
31 31
32 #define WHEEL_1 1
33 #define WHEEL_2 2
34 #define WHEEL_3 3
35 #define WHEEL_4 4
36 #define FREQ_1 1
37 #define FREQ_2 2
38 #define FREQ_3 3
39 #define FREQ_4 4
40 #define FLAG_OFFSET_WHEELS_1_3 8
41 #define FLAG_OFFSET_WHEELS_2_4 4
42
43 #define FLAG_NAN 0 // Not A NUMBER
44 #define FLAG_IAN 1 // Is A Number
45
32 46 #define SBM_KCOEFF_PER_NORM_KCOEFF 2
33 47
34 48 extern unsigned short sequenceCounterParameterDump;
35 49 extern unsigned short sequenceCounters_TM_DUMP[];
36 50 extern float k_coeff_intercalib_f0_norm[ ];
37 51 extern float k_coeff_intercalib_f0_sbm[ ];
38 52 extern float k_coeff_intercalib_f1_norm[ ];
39 53 extern float k_coeff_intercalib_f1_sbm[ ];
40 54 extern float k_coeff_intercalib_f2[ ];
41 55 extern fbins_masks_t fbins_masks;
42 56
43 57 int action_load_common_par( ccsdsTelecommandPacket_t *TC );
44 58 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
45 59 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
46 60 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
47 61 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
48 62 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
49 63 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
50 64 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
51 65 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
52 66 int action_dump_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
53 67
54 68 // NORMAL
55 69 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
56 70 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC );
57 71 int set_sy_lfr_n_swf_p( ccsdsTelecommandPacket_t *TC );
58 72 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC );
59 73 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC );
60 74 int set_sy_lfr_n_bp_p1( ccsdsTelecommandPacket_t *TC );
61 75 int set_sy_lfr_n_cwf_long_f3( ccsdsTelecommandPacket_t *TC );
62 76
63 77 // BURST
64 78 int set_sy_lfr_b_bp_p0( ccsdsTelecommandPacket_t *TC );
65 79 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC );
66 80
67 81 // SBM1
68 82 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC );
69 83 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC );
70 84
71 85 // SBM2
72 86 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC );
73 87 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC );
74 88
75 89 // TC_LFR_UPDATE_INFO
76 90 unsigned int check_update_info_hk_lfr_mode( unsigned char mode );
77 91 unsigned int check_update_info_hk_tds_mode( unsigned char mode );
78 92 unsigned int check_update_info_hk_thr_mode( unsigned char mode );
93 void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value );
94 void set_hk_lfr_sc_rw_f_flags( void );
79 95 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC );
80 96 void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag );
81 97 void build_sy_lfr_rw_mask( unsigned int channel );
82 98 void build_sy_lfr_rw_masks();
83 99 void merge_fbins_masks( void );
84 100
85 101 // FBINS_MASK
86 102 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC );
87 103
88 104 // TC_LFR_LOAD_PARS_FILTER_PAR
89 105 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
90 106
91 107 // KCOEFFICIENTS
92 108 int set_sy_lfr_kcoeff(ccsdsTelecommandPacket_t *TC , rtems_id queue_id);
93 109 void copyFloatByChar( unsigned char *destination, unsigned char *source );
94 110 void floatToChar( float value, unsigned char* ptr);
95 111
96 112 void init_parameter_dump( void );
97 113 void init_kcoefficients_dump( void );
98 114 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr );
99 115 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id );
100 116
101 117 #endif // TC_LOAD_DUMP_PARAMETERS_H
@@ -1,106 +1,100
1 1 /** Global variables of the LFR flight software.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * Among global variables, there are:
7 7 * - RTEMS names and id.
8 8 * - APB configuration registers.
9 9 * - waveforms global buffers, used by the waveform picker hardware module to store data.
10 10 * - spectral matrices buffesr, used by the hardware module to store data.
11 11 * - variable related to LFR modes parameters.
12 12 * - the global HK packet buffer.
13 13 * - the global dump parameter buffer.
14 14 *
15 15 */
16 16
17 17 #include <rtems.h>
18 18 #include <grspw.h>
19 19
20 20 #include "ccsds_types.h"
21 21 #include "grlib_regs.h"
22 22 #include "fsw_params.h"
23 23 #include "fsw_params_wf_handler.h"
24 24
25 25 #define NB_OF_TASKS 20
26 26 #define NB_OF_MISC_NAMES 5
27 27
28 28 // RTEMS GLOBAL VARIABLES
29 29 rtems_name misc_name[NB_OF_MISC_NAMES] = {0};
30 30 rtems_name Task_name[NB_OF_TASKS] = {0}; /* array of task names */
31 31 rtems_id Task_id[NB_OF_TASKS] = {0}; /* array of task ids */
32 32 rtems_name timecode_timer_name = 0;
33 33 rtems_id timecode_timer_id = RTEMS_ID_NONE;
34 34 rtems_name name_hk_rate_monotonic = 0; // name of the HK rate monotonic
35 35 rtems_id HK_id = RTEMS_ID_NONE;// id of the HK rate monotonic period
36 36 rtems_name name_avgv_rate_monotonic = 0; // name of the AVGV rate monotonic
37 37 rtems_id AVGV_id = RTEMS_ID_NONE;// id of the AVGV rate monotonic period
38 38 int fdSPW = 0;
39 39 int fdUART = 0;
40 40 unsigned char lfrCurrentMode = 0;
41 41 unsigned char pa_bia_status_info = 0;
42 42 unsigned char thisIsAnASMRestart = 0;
43 43 unsigned char oneTcLfrUpdateTimeReceived = 0;
44 44
45 45 // WAVEFORMS GLOBAL VARIABLES // 2048 * 3 * 4 + 2 * 4 = 24576 + 8 bytes = 24584
46 46 // 97 * 256 = 24832 => delta = 248 bytes = 62 words
47 47 // WAVEFORMS GLOBAL VARIABLES // 2688 * 3 * 4 + 2 * 4 = 32256 + 8 bytes = 32264
48 48 // 127 * 256 = 32512 => delta = 248 bytes = 62 words
49 49 // F0 F1 F2 F3
50 50 volatile int wf_buffer_f0[ NB_RING_NODES_F0 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
51 51 volatile int wf_buffer_f1[ NB_RING_NODES_F1 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
52 52 volatile int wf_buffer_f2[ NB_RING_NODES_F2 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
53 53 volatile int wf_buffer_f3[ NB_RING_NODES_F3 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
54 54
55 55 //***********************************
56 56 // SPECTRAL MATRICES GLOBAL VARIABLES
57 57
58 58 // alignment constraints for the spectral matrices buffers => the first data after the time (8 bytes) shall be aligned on 0x00
59 59 volatile int sm_f0[ NB_RING_NODES_SM_F0 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0};
60 60 volatile int sm_f1[ NB_RING_NODES_SM_F1 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0};
61 61 volatile int sm_f2[ NB_RING_NODES_SM_F2 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0};
62 62
63 63 // APB CONFIGURATION REGISTERS
64 64 time_management_regs_t *time_management_regs = (time_management_regs_t*) REGS_ADDR_TIME_MANAGEMENT;
65 65 gptimer_regs_t *gptimer_regs = (gptimer_regs_t *) REGS_ADDR_GPTIMER;
66 66 waveform_picker_regs_0_1_18_t *waveform_picker_regs = (waveform_picker_regs_0_1_18_t*) REGS_ADDR_WAVEFORM_PICKER;
67 67 spectral_matrix_regs_t *spectral_matrix_regs = (spectral_matrix_regs_t*) REGS_ADDR_SPECTRAL_MATRIX;
68 68
69 69 // MODE PARAMETERS
70 70 Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet = {0};
71 71 struct param_local_str param_local = {0};
72 72 unsigned int lastValidEnterModeTime = {0};
73 73
74 74 // HK PACKETS
75 75 Packet_TM_LFR_HK_t housekeeping_packet = {0};
76 unsigned char cp_rpw_sc_rw_f_flags = 0;
76 unsigned char cp_rpw_sc_rw1_rw2_f_flags = 0;
77 unsigned char cp_rpw_sc_rw3_rw4_f_flags = 0;
77 78 // message queues occupancy
78 79 unsigned char hk_lfr_q_sd_fifo_size_max = 0;
79 80 unsigned char hk_lfr_q_rv_fifo_size_max = 0;
80 81 unsigned char hk_lfr_q_p0_fifo_size_max = 0;
81 82 unsigned char hk_lfr_q_p1_fifo_size_max = 0;
82 83 unsigned char hk_lfr_q_p2_fifo_size_max = 0;
83 84 // sequence counters are incremented by APID (PID + CAT) and destination ID
84 85 unsigned short sequenceCounters_SCIENCE_NORMAL_BURST = 0;
85 86 unsigned short sequenceCounters_SCIENCE_SBM1_SBM2 = 0;
86 87 unsigned short sequenceCounters_TC_EXE[SEQ_CNT_NB_DEST_ID] = {0};
87 88 unsigned short sequenceCounters_TM_DUMP[SEQ_CNT_NB_DEST_ID] = {0};
88 89 unsigned short sequenceCounterHK = {0};
89 90 spw_stats grspw_stats = {0};
90 91
91 92 // TC_LFR_UPDATE_INFO
92 float cp_rpw_sc_rw1_f1 = INIT_FLOAT;
93 float cp_rpw_sc_rw1_f2 = INIT_FLOAT;
94 float cp_rpw_sc_rw2_f1 = INIT_FLOAT;
95 float cp_rpw_sc_rw2_f2 = INIT_FLOAT;
96 float cp_rpw_sc_rw3_f1 = INIT_FLOAT;
97 float cp_rpw_sc_rw3_f2 = INIT_FLOAT;
98 float cp_rpw_sc_rw4_f1 = INIT_FLOAT;
99 float cp_rpw_sc_rw4_f2 = INIT_FLOAT;
93 rw_f_t rw_f;
100 94
101 95 // TC_LFR_LOAD_FILTER_PAR
102 96 filterPar_t filterPar = {0};
103 97
104 98 fbins_masks_t fbins_masks = {0};
105 99 unsigned int acquisitionDurations[NB_ACQUISITION_DURATION]
106 100 = {ACQUISITION_DURATION_F0, ACQUISITION_DURATION_F1, ACQUISITION_DURATION_F2};
@@ -1,945 +1,959
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 old_isr_handler = NULL;
134 134
135 135 // UART settings
136 136 enable_apbuart_transmitter();
137 137 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
138 138
139 139 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
140 140
141 141
142 142 PRINTF("\n\n\n\n\n")
143 143
144 144 initCache();
145 145
146 146 PRINTF("*************************\n")
147 147 PRINTF("** LFR Flight Software **\n")
148 148
149 149 PRINTF1("** %d-", SW_VERSION_N1)
150 150 PRINTF1("%d-" , SW_VERSION_N2)
151 151 PRINTF1("%d-" , SW_VERSION_N3)
152 152 PRINTF1("%d **\n", SW_VERSION_N4)
153 153
154 154 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
155 155 PRINTF("** VHDL **\n")
156 156 PRINTF1("** %d-", vhdlVersion[1])
157 157 PRINTF1("%d-" , vhdlVersion[2])
158 158 PRINTF1("%d **\n", vhdlVersion[3])
159 159 PRINTF("*************************\n")
160 160 PRINTF("\n\n")
161 161
162 162 init_parameter_dump();
163 163 init_kcoefficients_dump();
164 164 init_local_mode_parameters();
165 165 init_housekeeping_parameters();
166 166 init_k_coefficients_prc0();
167 167 init_k_coefficients_prc1();
168 168 init_k_coefficients_prc2();
169 169 pa_bia_status_info = INIT_CHAR;
170 170 cp_rpw_sc_rw_f_flags = INIT_CHAR;
171 cp_rpw_sc_rw1_f1 = INIT_FLOAT;
172 cp_rpw_sc_rw1_f2 = INIT_FLOAT;
173 cp_rpw_sc_rw2_f1 = INIT_FLOAT;
174 cp_rpw_sc_rw2_f2 = INIT_FLOAT;
175 cp_rpw_sc_rw3_f1 = INIT_FLOAT;
176 cp_rpw_sc_rw3_f2 = INIT_FLOAT;
177 cp_rpw_sc_rw4_f1 = INIT_FLOAT;
178 cp_rpw_sc_rw4_f2 = INIT_FLOAT;
171
172 // initialize all reaction wheels frequencies to NaN
173 rw_f.cp_rpw_sc_rw1_f1 = NAN;
174 rw_f.cp_rpw_sc_rw1_f2 = NAN;
175 rw_f.cp_rpw_sc_rw1_f3 = NAN;
176 rw_f.cp_rpw_sc_rw1_f4 = NAN;
177 rw_f.cp_rpw_sc_rw2_f1 = NAN;
178 rw_f.cp_rpw_sc_rw2_f2 = NAN;
179 rw_f.cp_rpw_sc_rw2_f3 = NAN;
180 rw_f.cp_rpw_sc_rw2_f4 = NAN;
181 rw_f.cp_rpw_sc_rw3_f1 = NAN;
182 rw_f.cp_rpw_sc_rw3_f2 = NAN;
183 rw_f.cp_rpw_sc_rw3_f3 = NAN;
184 rw_f.cp_rpw_sc_rw3_f4 = NAN;
185 rw_f.cp_rpw_sc_rw4_f1 = NAN;
186 rw_f.cp_rpw_sc_rw4_f2 = NAN;
187 rw_f.cp_rpw_sc_rw4_f3 = NAN;
188 rw_f.cp_rpw_sc_rw4_f4 = NAN;
189
190 cp_rpw_sc_rw1_rw2_f_flags = INIT_CHAR;
191 cp_rpw_sc_rw3_rw4_f_flags = INIT_CHAR;
192
179 193 // initialize filtering parameters
180 194 filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED;
181 195 filterPar.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
182 196 filterPar.sy_lfr_pas_filter_tbad = DEFAULT_SY_LFR_PAS_FILTER_TBAD;
183 197 filterPar.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
184 198 filterPar.sy_lfr_pas_filter_shift = DEFAULT_SY_LFR_PAS_FILTER_SHIFT;
185 199 filterPar.sy_lfr_sc_rw_delta_f = DEFAULT_SY_LFR_SC_RW_DELTA_F;
186 200 update_last_valid_transition_date( DEFAULT_LAST_VALID_TRANSITION_DATE );
187 201
188 202 // waveform picker initialization
189 203 WFP_init_rings();
190 204 LEON_Clear_interrupt( IRQ_SPARC_GPTIMER_WATCHDOG ); // initialize the waveform rings
191 205 WFP_reset_current_ring_nodes();
192 206 reset_waveform_picker_regs();
193 207
194 208 // spectral matrices initialization
195 209 SM_init_rings(); // initialize spectral matrices rings
196 210 SM_reset_current_ring_nodes();
197 211 reset_spectral_matrix_regs();
198 212
199 213 // configure calibration
200 214 configureCalibration( false ); // true means interleaved mode, false is for normal mode
201 215
202 216 updateLFRCurrentMode( LFR_MODE_STANDBY );
203 217
204 218 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
205 219
206 220 create_names(); // create all names
207 221
208 222 status = create_timecode_timer(); // create the timer used by timecode_irq_handler
209 223 if (status != RTEMS_SUCCESSFUL)
210 224 {
211 225 PRINTF1("in INIT *** ERR in create_timer_timecode, code %d", status)
212 226 }
213 227
214 228 status = create_message_queues(); // create message queues
215 229 if (status != RTEMS_SUCCESSFUL)
216 230 {
217 231 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
218 232 }
219 233
220 234 status = create_all_tasks(); // create all tasks
221 235 if (status != RTEMS_SUCCESSFUL)
222 236 {
223 237 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
224 238 }
225 239
226 240 // **************************
227 241 // <SPACEWIRE INITIALIZATION>
228 242 status_spw = spacewire_open_link(); // (1) open the link
229 243 if ( status_spw != RTEMS_SUCCESSFUL )
230 244 {
231 245 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
232 246 }
233 247
234 248 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
235 249 {
236 250 status_spw = spacewire_configure_link( fdSPW );
237 251 if ( status_spw != RTEMS_SUCCESSFUL )
238 252 {
239 253 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
240 254 }
241 255 }
242 256
243 257 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
244 258 {
245 259 status_spw = spacewire_start_link( fdSPW );
246 260 if ( status_spw != RTEMS_SUCCESSFUL )
247 261 {
248 262 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
249 263 }
250 264 }
251 265 // </SPACEWIRE INITIALIZATION>
252 266 // ***************************
253 267
254 268 status = start_all_tasks(); // start all tasks
255 269 if (status != RTEMS_SUCCESSFUL)
256 270 {
257 271 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
258 272 }
259 273
260 274 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
261 275 status = start_recv_send_tasks();
262 276 if ( status != RTEMS_SUCCESSFUL )
263 277 {
264 278 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
265 279 }
266 280
267 281 // suspend science tasks, they will be restarted later depending on the mode
268 282 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
269 283 if (status != RTEMS_SUCCESSFUL)
270 284 {
271 285 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
272 286 }
273 287
274 288 // configure IRQ handling for the waveform picker unit
275 289 status = rtems_interrupt_catch( waveforms_isr,
276 290 IRQ_SPARC_WAVEFORM_PICKER,
277 291 &old_isr_handler) ;
278 292 // configure IRQ handling for the spectral matrices unit
279 293 status = rtems_interrupt_catch( spectral_matrices_isr,
280 294 IRQ_SPARC_SPECTRAL_MATRIX,
281 295 &old_isr_handler) ;
282 296
283 297 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
284 298 if ( status_spw != RTEMS_SUCCESSFUL )
285 299 {
286 300 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
287 301 if ( status != RTEMS_SUCCESSFUL ) {
288 302 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
289 303 }
290 304 }
291 305
292 306 BOOT_PRINTF("delete INIT\n")
293 307
294 308 set_hk_lfr_sc_potential_flag( true );
295 309
296 310 // start the timer to detect a missing spacewire timecode
297 311 // the timeout is larger because the spw IP needs to receive several valid timecodes before generating a tickout
298 312 // if a tickout is generated, the timer is restarted
299 313 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT_INIT, timecode_timer_routine, NULL );
300 314
301 315 grspw_timecode_callback = &timecode_irq_handler;
302 316
303 317 status = rtems_task_delete(RTEMS_SELF);
304 318
305 319 }
306 320
307 321 void init_local_mode_parameters( void )
308 322 {
309 323 /** This function initialize the param_local global variable with default values.
310 324 *
311 325 */
312 326
313 327 unsigned int i;
314 328
315 329 // LOCAL PARAMETERS
316 330
317 331 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
318 332 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
319 333
320 334 // init sequence counters
321 335
322 336 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
323 337 {
324 338 sequenceCounters_TC_EXE[i] = INIT_CHAR;
325 339 sequenceCounters_TM_DUMP[i] = INIT_CHAR;
326 340 }
327 341 sequenceCounters_SCIENCE_NORMAL_BURST = INIT_CHAR;
328 342 sequenceCounters_SCIENCE_SBM1_SBM2 = INIT_CHAR;
329 343 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << TM_PACKET_SEQ_SHIFT;
330 344 }
331 345
332 346 void reset_local_time( void )
333 347 {
334 348 time_management_regs->ctrl = time_management_regs->ctrl | VAL_SOFTWARE_RESET; // [0010] software reset, coarse time = 0x80000000
335 349 }
336 350
337 351 void create_names( void ) // create all names for tasks and queues
338 352 {
339 353 /** This function creates all RTEMS names used in the software for tasks and queues.
340 354 *
341 355 * @return RTEMS directive status codes:
342 356 * - RTEMS_SUCCESSFUL - successful completion
343 357 *
344 358 */
345 359
346 360 // task names
347 361 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
348 362 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
349 363 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
350 364 Task_name[TASKID_LOAD] = rtems_build_name( 'L', 'O', 'A', 'D' );
351 365 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
352 366 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
353 367 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
354 368 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
355 369 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
356 370 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
357 371 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
358 372 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
359 373 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
360 374 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
361 375 Task_name[TASKID_LINK] = rtems_build_name( 'L', 'I', 'N', 'K' );
362 376 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
363 377 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
364 378 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
365 379 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
366 380
367 381 // rate monotonic period names
368 382 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
369 383
370 384 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
371 385 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
372 386 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
373 387 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
374 388 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
375 389
376 390 timecode_timer_name = rtems_build_name( 'S', 'P', 'T', 'C' );
377 391 }
378 392
379 393 int create_all_tasks( void ) // create all tasks which run in the software
380 394 {
381 395 /** This function creates all RTEMS tasks used in the software.
382 396 *
383 397 * @return RTEMS directive status codes:
384 398 * - RTEMS_SUCCESSFUL - task created successfully
385 399 * - RTEMS_INVALID_ADDRESS - id is NULL
386 400 * - RTEMS_INVALID_NAME - invalid task name
387 401 * - RTEMS_INVALID_PRIORITY - invalid task priority
388 402 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
389 403 * - RTEMS_TOO_MANY - too many tasks created
390 404 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
391 405 * - RTEMS_TOO_MANY - too many global objects
392 406 *
393 407 */
394 408
395 409 rtems_status_code status;
396 410
397 411 //**********
398 412 // SPACEWIRE
399 413 // RECV
400 414 status = rtems_task_create(
401 415 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
402 416 RTEMS_DEFAULT_MODES,
403 417 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
404 418 );
405 419 if (status == RTEMS_SUCCESSFUL) // SEND
406 420 {
407 421 status = rtems_task_create(
408 422 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
409 423 RTEMS_DEFAULT_MODES,
410 424 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
411 425 );
412 426 }
413 427 if (status == RTEMS_SUCCESSFUL) // LINK
414 428 {
415 429 status = rtems_task_create(
416 430 Task_name[TASKID_LINK], TASK_PRIORITY_LINK, RTEMS_MINIMUM_STACK_SIZE,
417 431 RTEMS_DEFAULT_MODES,
418 432 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LINK]
419 433 );
420 434 }
421 435 if (status == RTEMS_SUCCESSFUL) // ACTN
422 436 {
423 437 status = rtems_task_create(
424 438 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
425 439 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
426 440 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
427 441 );
428 442 }
429 443 if (status == RTEMS_SUCCESSFUL) // SPIQ
430 444 {
431 445 status = rtems_task_create(
432 446 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
433 447 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
434 448 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
435 449 );
436 450 }
437 451
438 452 //******************
439 453 // SPECTRAL MATRICES
440 454 if (status == RTEMS_SUCCESSFUL) // AVF0
441 455 {
442 456 status = rtems_task_create(
443 457 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
444 458 RTEMS_DEFAULT_MODES,
445 459 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
446 460 );
447 461 }
448 462 if (status == RTEMS_SUCCESSFUL) // PRC0
449 463 {
450 464 status = rtems_task_create(
451 465 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
452 466 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
453 467 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
454 468 );
455 469 }
456 470 if (status == RTEMS_SUCCESSFUL) // AVF1
457 471 {
458 472 status = rtems_task_create(
459 473 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
460 474 RTEMS_DEFAULT_MODES,
461 475 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
462 476 );
463 477 }
464 478 if (status == RTEMS_SUCCESSFUL) // PRC1
465 479 {
466 480 status = rtems_task_create(
467 481 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
468 482 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
469 483 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
470 484 );
471 485 }
472 486 if (status == RTEMS_SUCCESSFUL) // AVF2
473 487 {
474 488 status = rtems_task_create(
475 489 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
476 490 RTEMS_DEFAULT_MODES,
477 491 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
478 492 );
479 493 }
480 494 if (status == RTEMS_SUCCESSFUL) // PRC2
481 495 {
482 496 status = rtems_task_create(
483 497 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
484 498 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
485 499 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
486 500 );
487 501 }
488 502
489 503 //****************
490 504 // WAVEFORM PICKER
491 505 if (status == RTEMS_SUCCESSFUL) // WFRM
492 506 {
493 507 status = rtems_task_create(
494 508 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
495 509 RTEMS_DEFAULT_MODES,
496 510 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
497 511 );
498 512 }
499 513 if (status == RTEMS_SUCCESSFUL) // CWF3
500 514 {
501 515 status = rtems_task_create(
502 516 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
503 517 RTEMS_DEFAULT_MODES,
504 518 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
505 519 );
506 520 }
507 521 if (status == RTEMS_SUCCESSFUL) // CWF2
508 522 {
509 523 status = rtems_task_create(
510 524 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
511 525 RTEMS_DEFAULT_MODES,
512 526 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
513 527 );
514 528 }
515 529 if (status == RTEMS_SUCCESSFUL) // CWF1
516 530 {
517 531 status = rtems_task_create(
518 532 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
519 533 RTEMS_DEFAULT_MODES,
520 534 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
521 535 );
522 536 }
523 537 if (status == RTEMS_SUCCESSFUL) // SWBD
524 538 {
525 539 status = rtems_task_create(
526 540 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
527 541 RTEMS_DEFAULT_MODES,
528 542 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
529 543 );
530 544 }
531 545
532 546 //*****
533 547 // MISC
534 548 if (status == RTEMS_SUCCESSFUL) // LOAD
535 549 {
536 550 status = rtems_task_create(
537 551 Task_name[TASKID_LOAD], TASK_PRIORITY_LOAD, RTEMS_MINIMUM_STACK_SIZE,
538 552 RTEMS_DEFAULT_MODES,
539 553 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LOAD]
540 554 );
541 555 }
542 556 if (status == RTEMS_SUCCESSFUL) // DUMB
543 557 {
544 558 status = rtems_task_create(
545 559 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
546 560 RTEMS_DEFAULT_MODES,
547 561 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
548 562 );
549 563 }
550 564 if (status == RTEMS_SUCCESSFUL) // HOUS
551 565 {
552 566 status = rtems_task_create(
553 567 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
554 568 RTEMS_DEFAULT_MODES,
555 569 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
556 570 );
557 571 }
558 572
559 573 return status;
560 574 }
561 575
562 576 int start_recv_send_tasks( void )
563 577 {
564 578 rtems_status_code status;
565 579
566 580 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
567 581 if (status!=RTEMS_SUCCESSFUL) {
568 582 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
569 583 }
570 584
571 585 if (status == RTEMS_SUCCESSFUL) // SEND
572 586 {
573 587 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
574 588 if (status!=RTEMS_SUCCESSFUL) {
575 589 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
576 590 }
577 591 }
578 592
579 593 return status;
580 594 }
581 595
582 596 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
583 597 {
584 598 /** This function starts all RTEMS tasks used in the software.
585 599 *
586 600 * @return RTEMS directive status codes:
587 601 * - RTEMS_SUCCESSFUL - ask started successfully
588 602 * - RTEMS_INVALID_ADDRESS - invalid task entry point
589 603 * - RTEMS_INVALID_ID - invalid task id
590 604 * - RTEMS_INCORRECT_STATE - task not in the dormant state
591 605 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
592 606 *
593 607 */
594 608 // starts all the tasks fot eh flight software
595 609
596 610 rtems_status_code status;
597 611
598 612 //**********
599 613 // SPACEWIRE
600 614 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
601 615 if (status!=RTEMS_SUCCESSFUL) {
602 616 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
603 617 }
604 618
605 619 if (status == RTEMS_SUCCESSFUL) // LINK
606 620 {
607 621 status = rtems_task_start( Task_id[TASKID_LINK], link_task, 1 );
608 622 if (status!=RTEMS_SUCCESSFUL) {
609 623 BOOT_PRINTF("in INIT *** Error starting TASK_LINK\n")
610 624 }
611 625 }
612 626
613 627 if (status == RTEMS_SUCCESSFUL) // ACTN
614 628 {
615 629 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
616 630 if (status!=RTEMS_SUCCESSFUL) {
617 631 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
618 632 }
619 633 }
620 634
621 635 //******************
622 636 // SPECTRAL MATRICES
623 637 if (status == RTEMS_SUCCESSFUL) // AVF0
624 638 {
625 639 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
626 640 if (status!=RTEMS_SUCCESSFUL) {
627 641 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
628 642 }
629 643 }
630 644 if (status == RTEMS_SUCCESSFUL) // PRC0
631 645 {
632 646 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
633 647 if (status!=RTEMS_SUCCESSFUL) {
634 648 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
635 649 }
636 650 }
637 651 if (status == RTEMS_SUCCESSFUL) // AVF1
638 652 {
639 653 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
640 654 if (status!=RTEMS_SUCCESSFUL) {
641 655 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
642 656 }
643 657 }
644 658 if (status == RTEMS_SUCCESSFUL) // PRC1
645 659 {
646 660 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
647 661 if (status!=RTEMS_SUCCESSFUL) {
648 662 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
649 663 }
650 664 }
651 665 if (status == RTEMS_SUCCESSFUL) // AVF2
652 666 {
653 667 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
654 668 if (status!=RTEMS_SUCCESSFUL) {
655 669 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
656 670 }
657 671 }
658 672 if (status == RTEMS_SUCCESSFUL) // PRC2
659 673 {
660 674 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
661 675 if (status!=RTEMS_SUCCESSFUL) {
662 676 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
663 677 }
664 678 }
665 679
666 680 //****************
667 681 // WAVEFORM PICKER
668 682 if (status == RTEMS_SUCCESSFUL) // WFRM
669 683 {
670 684 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
671 685 if (status!=RTEMS_SUCCESSFUL) {
672 686 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
673 687 }
674 688 }
675 689 if (status == RTEMS_SUCCESSFUL) // CWF3
676 690 {
677 691 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
678 692 if (status!=RTEMS_SUCCESSFUL) {
679 693 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
680 694 }
681 695 }
682 696 if (status == RTEMS_SUCCESSFUL) // CWF2
683 697 {
684 698 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
685 699 if (status!=RTEMS_SUCCESSFUL) {
686 700 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
687 701 }
688 702 }
689 703 if (status == RTEMS_SUCCESSFUL) // CWF1
690 704 {
691 705 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
692 706 if (status!=RTEMS_SUCCESSFUL) {
693 707 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
694 708 }
695 709 }
696 710 if (status == RTEMS_SUCCESSFUL) // SWBD
697 711 {
698 712 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
699 713 if (status!=RTEMS_SUCCESSFUL) {
700 714 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
701 715 }
702 716 }
703 717
704 718 //*****
705 719 // MISC
706 720 if (status == RTEMS_SUCCESSFUL) // HOUS
707 721 {
708 722 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
709 723 if (status!=RTEMS_SUCCESSFUL) {
710 724 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
711 725 }
712 726 }
713 727 if (status == RTEMS_SUCCESSFUL) // DUMB
714 728 {
715 729 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
716 730 if (status!=RTEMS_SUCCESSFUL) {
717 731 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
718 732 }
719 733 }
720 734 if (status == RTEMS_SUCCESSFUL) // LOAD
721 735 {
722 736 status = rtems_task_start( Task_id[TASKID_LOAD], load_task, 1 );
723 737 if (status!=RTEMS_SUCCESSFUL) {
724 738 BOOT_PRINTF("in INIT *** Error starting TASK_LOAD\n")
725 739 }
726 740 }
727 741
728 742 return status;
729 743 }
730 744
731 745 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
732 746 {
733 747 rtems_status_code status_recv;
734 748 rtems_status_code status_send;
735 749 rtems_status_code status_q_p0;
736 750 rtems_status_code status_q_p1;
737 751 rtems_status_code status_q_p2;
738 752 rtems_status_code ret;
739 753 rtems_id queue_id;
740 754
741 755 ret = RTEMS_SUCCESSFUL;
742 756 queue_id = RTEMS_ID_NONE;
743 757
744 758 //****************************************
745 759 // create the queue for handling valid TCs
746 760 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
747 761 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
748 762 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
749 763 if ( status_recv != RTEMS_SUCCESSFUL ) {
750 764 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
751 765 }
752 766
753 767 //************************************************
754 768 // create the queue for handling TM packet sending
755 769 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
756 770 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
757 771 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
758 772 if ( status_send != RTEMS_SUCCESSFUL ) {
759 773 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
760 774 }
761 775
762 776 //*****************************************************************************
763 777 // create the queue for handling averaged spectral matrices for processing @ f0
764 778 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
765 779 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
766 780 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
767 781 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
768 782 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
769 783 }
770 784
771 785 //*****************************************************************************
772 786 // create the queue for handling averaged spectral matrices for processing @ f1
773 787 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
774 788 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
775 789 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
776 790 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
777 791 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
778 792 }
779 793
780 794 //*****************************************************************************
781 795 // create the queue for handling averaged spectral matrices for processing @ f2
782 796 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
783 797 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
784 798 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
785 799 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
786 800 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
787 801 }
788 802
789 803 if ( status_recv != RTEMS_SUCCESSFUL )
790 804 {
791 805 ret = status_recv;
792 806 }
793 807 else if( status_send != RTEMS_SUCCESSFUL )
794 808 {
795 809 ret = status_send;
796 810 }
797 811 else if( status_q_p0 != RTEMS_SUCCESSFUL )
798 812 {
799 813 ret = status_q_p0;
800 814 }
801 815 else if( status_q_p1 != RTEMS_SUCCESSFUL )
802 816 {
803 817 ret = status_q_p1;
804 818 }
805 819 else
806 820 {
807 821 ret = status_q_p2;
808 822 }
809 823
810 824 return ret;
811 825 }
812 826
813 827 rtems_status_code create_timecode_timer( void )
814 828 {
815 829 rtems_status_code status;
816 830
817 831 status = rtems_timer_create( timecode_timer_name, &timecode_timer_id );
818 832
819 833 if ( status != RTEMS_SUCCESSFUL )
820 834 {
821 835 PRINTF1("in create_timer_timecode *** ERR creating SPTC timer, %d\n", status)
822 836 }
823 837 else
824 838 {
825 839 PRINTF("in create_timer_timecode *** OK creating SPTC timer\n")
826 840 }
827 841
828 842 return status;
829 843 }
830 844
831 845 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
832 846 {
833 847 rtems_status_code status;
834 848 rtems_name queue_name;
835 849
836 850 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
837 851
838 852 status = rtems_message_queue_ident( queue_name, 0, queue_id );
839 853
840 854 return status;
841 855 }
842 856
843 857 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
844 858 {
845 859 rtems_status_code status;
846 860 rtems_name queue_name;
847 861
848 862 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
849 863
850 864 status = rtems_message_queue_ident( queue_name, 0, queue_id );
851 865
852 866 return status;
853 867 }
854 868
855 869 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
856 870 {
857 871 rtems_status_code status;
858 872 rtems_name queue_name;
859 873
860 874 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
861 875
862 876 status = rtems_message_queue_ident( queue_name, 0, queue_id );
863 877
864 878 return status;
865 879 }
866 880
867 881 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
868 882 {
869 883 rtems_status_code status;
870 884 rtems_name queue_name;
871 885
872 886 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
873 887
874 888 status = rtems_message_queue_ident( queue_name, 0, queue_id );
875 889
876 890 return status;
877 891 }
878 892
879 893 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
880 894 {
881 895 rtems_status_code status;
882 896 rtems_name queue_name;
883 897
884 898 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
885 899
886 900 status = rtems_message_queue_ident( queue_name, 0, queue_id );
887 901
888 902 return status;
889 903 }
890 904
891 905 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max )
892 906 {
893 907 u_int32_t count;
894 908 rtems_status_code status;
895 909
896 910 count = 0;
897 911
898 912 status = rtems_message_queue_get_number_pending( queue_id, &count );
899 913
900 914 count = count + 1;
901 915
902 916 if (status != RTEMS_SUCCESSFUL)
903 917 {
904 918 PRINTF1("in update_queue_max_count *** ERR = %d\n", status)
905 919 }
906 920 else
907 921 {
908 922 if (count > *fifo_size_max)
909 923 {
910 924 *fifo_size_max = count;
911 925 }
912 926 }
913 927 }
914 928
915 929 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
916 930 {
917 931 unsigned char i;
918 932
919 933 //***************
920 934 // BUFFER ADDRESS
921 935 for(i=0; i<nbNodes; i++)
922 936 {
923 937 ring[i].coarseTime = INT32_ALL_F;
924 938 ring[i].fineTime = INT32_ALL_F;
925 939 ring[i].sid = INIT_CHAR;
926 940 ring[i].status = INIT_CHAR;
927 941 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
928 942 }
929 943
930 944 //*****
931 945 // NEXT
932 946 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
933 947 for(i=0; i<nbNodes-1; i++)
934 948 {
935 949 ring[i].next = (ring_node*) &ring[ i + 1 ];
936 950 }
937 951
938 952 //*********
939 953 // PREVIOUS
940 954 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
941 955 for(i=1; i<nbNodes; i++)
942 956 {
943 957 ring[i].previous = (ring_node*) &ring[ i - 1 ];
944 958 }
945 959 }
@@ -1,988 +1,1005
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 int16_t hk_lfr_sc_v_f3_as_int16 = 0;
11 int16_t hk_lfr_sc_e1_f3_as_int16 = 0;
12 int16_t hk_lfr_sc_e2_f3_as_int16 = 0;
13
10 14 void timer_configure(unsigned char timer, unsigned int clock_divider,
11 15 unsigned char interrupt_level, rtems_isr (*timer_isr)() )
12 16 {
13 17 /** This function configures a GPTIMER timer instantiated in the VHDL design.
14 18 *
15 19 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
16 20 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
17 21 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
18 22 * @param interrupt_level is the interrupt level that the timer drives.
19 23 * @param timer_isr is the interrupt subroutine that will be attached to the IRQ driven by the timer.
20 24 *
21 25 * Interrupt levels are described in the SPARC documentation sparcv8.pdf p.76
22 26 *
23 27 */
24 28
25 29 rtems_status_code status;
26 30 rtems_isr_entry old_isr_handler;
27 31
28 32 old_isr_handler = NULL;
29 33
30 34 gptimer_regs->timer[timer].ctrl = INIT_CHAR; // reset the control register
31 35
32 36 status = rtems_interrupt_catch( timer_isr, interrupt_level, &old_isr_handler) ; // see sparcv8.pdf p.76 for interrupt levels
33 37 if (status!=RTEMS_SUCCESSFUL)
34 38 {
35 39 PRINTF("in configure_timer *** ERR rtems_interrupt_catch\n")
36 40 }
37 41
38 42 timer_set_clock_divider( timer, clock_divider);
39 43 }
40 44
41 45 void timer_start(unsigned char timer)
42 46 {
43 47 /** This function starts a GPTIMER timer.
44 48 *
45 49 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
46 50 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
47 51 *
48 52 */
49 53
50 54 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_CLEAR_IRQ;
51 55 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_LD;
52 56 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_EN;
53 57 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_RS;
54 58 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_IE;
55 59 }
56 60
57 61 void timer_stop(unsigned char timer)
58 62 {
59 63 /** This function stops a GPTIMER timer.
60 64 *
61 65 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
62 66 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
63 67 *
64 68 */
65 69
66 70 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & GPTIMER_EN_MASK;
67 71 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & GPTIMER_IE_MASK;
68 72 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_CLEAR_IRQ;
69 73 }
70 74
71 75 void timer_set_clock_divider(unsigned char timer, unsigned int clock_divider)
72 76 {
73 77 /** This function sets the clock divider of a GPTIMER timer.
74 78 *
75 79 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
76 80 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
77 81 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
78 82 *
79 83 */
80 84
81 85 gptimer_regs->timer[timer].reload = clock_divider; // base clock frequency is 1 MHz
82 86 }
83 87
84 88 // WATCHDOG
85 89
86 90 rtems_isr watchdog_isr( rtems_vector_number vector )
87 91 {
88 92 rtems_status_code status_code;
89 93
90 94 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_12 );
91 95
92 96 PRINTF("watchdog_isr *** this is the end, exit(0)\n");
93 97
94 98 exit(0);
95 99 }
96 100
97 101 void watchdog_configure(void)
98 102 {
99 103 /** This function configure the watchdog.
100 104 *
101 105 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
102 106 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
103 107 *
104 108 * The watchdog is a timer provided by the GPTIMER IP core of the GRLIB.
105 109 *
106 110 */
107 111
108 112 LEON_Mask_interrupt( IRQ_GPTIMER_WATCHDOG ); // mask gptimer/watchdog interrupt during configuration
109 113
110 114 timer_configure( TIMER_WATCHDOG, CLKDIV_WATCHDOG, IRQ_SPARC_GPTIMER_WATCHDOG, watchdog_isr );
111 115
112 116 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG ); // clear gptimer/watchdog interrupt
113 117 }
114 118
115 119 void watchdog_stop(void)
116 120 {
117 121 LEON_Mask_interrupt( IRQ_GPTIMER_WATCHDOG ); // mask gptimer/watchdog interrupt line
118 122 timer_stop( TIMER_WATCHDOG );
119 123 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG ); // clear gptimer/watchdog interrupt
120 124 }
121 125
122 126 void watchdog_reload(void)
123 127 {
124 128 /** This function reloads the watchdog timer counter with the timer reload value.
125 129 *
126 130 * @param void
127 131 *
128 132 * @return void
129 133 *
130 134 */
131 135
132 136 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_LD;
133 137 }
134 138
135 139 void watchdog_start(void)
136 140 {
137 141 /** This function starts the watchdog timer.
138 142 *
139 143 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
140 144 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
141 145 *
142 146 */
143 147
144 148 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG );
145 149
146 150 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_CLEAR_IRQ;
147 151 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_LD;
148 152 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_EN;
149 153 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_IE;
150 154
151 155 LEON_Unmask_interrupt( IRQ_GPTIMER_WATCHDOG );
152 156
153 157 }
154 158
155 159 int enable_apbuart_transmitter( void ) // set the bit 1, TE Transmitter Enable to 1 in the APBUART control register
156 160 {
157 161 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
158 162
159 163 apbuart_regs->ctrl = APBUART_CTRL_REG_MASK_TE;
160 164
161 165 return 0;
162 166 }
163 167
164 168 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value)
165 169 {
166 170 /** This function sets the scaler reload register of the apbuart module
167 171 *
168 172 * @param regs is the address of the apbuart registers in memory
169 173 * @param value is the value that will be stored in the scaler register
170 174 *
171 175 * The value shall be set by the software to get data on the serial interface.
172 176 *
173 177 */
174 178
175 179 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) regs;
176 180
177 181 apbuart_regs->scaler = value;
178 182
179 183 BOOT_PRINTF1("OK *** apbuart port scaler reload register set to 0x%x\n", value)
180 184 }
181 185
182 186 //************
183 187 // RTEMS TASKS
184 188
185 189 rtems_task load_task(rtems_task_argument argument)
186 190 {
187 191 BOOT_PRINTF("in LOAD *** \n")
188 192
189 193 rtems_status_code status;
190 194 unsigned int i;
191 195 unsigned int j;
192 196 rtems_name name_watchdog_rate_monotonic; // name of the watchdog rate monotonic
193 197 rtems_id watchdog_period_id; // id of the watchdog rate monotonic period
194 198
195 199 watchdog_period_id = RTEMS_ID_NONE;
196 200
197 201 name_watchdog_rate_monotonic = rtems_build_name( 'L', 'O', 'A', 'D' );
198 202
199 203 status = rtems_rate_monotonic_create( name_watchdog_rate_monotonic, &watchdog_period_id );
200 204 if( status != RTEMS_SUCCESSFUL ) {
201 205 PRINTF1( "in LOAD *** rtems_rate_monotonic_create failed with status of %d\n", status )
202 206 }
203 207
204 208 i = 0;
205 209 j = 0;
206 210
207 211 watchdog_configure();
208 212
209 213 watchdog_start();
210 214
211 215 set_sy_lfr_watchdog_enabled( true );
212 216
213 217 while(1){
214 218 status = rtems_rate_monotonic_period( watchdog_period_id, WATCHDOG_PERIOD );
215 219 watchdog_reload();
216 220 i = i + 1;
217 221 if ( i == WATCHDOG_LOOP_PRINTF )
218 222 {
219 223 i = 0;
220 224 j = j + 1;
221 225 PRINTF1("%d\n", j)
222 226 }
223 227 #ifdef DEBUG_WATCHDOG
224 228 if (j == WATCHDOG_LOOP_DEBUG )
225 229 {
226 230 status = rtems_task_delete(RTEMS_SELF);
227 231 }
228 232 #endif
229 233 }
230 234 }
231 235
232 236 rtems_task hous_task(rtems_task_argument argument)
233 237 {
234 238 rtems_status_code status;
235 239 rtems_status_code spare_status;
236 240 rtems_id queue_id;
237 241 rtems_rate_monotonic_period_status period_status;
238 242 bool isSynchronized;
239 243
240 244 queue_id = RTEMS_ID_NONE;
241 245 memset(&period_status, 0, sizeof(rtems_rate_monotonic_period_status));
242 246 isSynchronized = false;
243 247
244 248 status = get_message_queue_id_send( &queue_id );
245 249 if (status != RTEMS_SUCCESSFUL)
246 250 {
247 251 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
248 252 }
249 253
250 254 BOOT_PRINTF("in HOUS ***\n");
251 255
252 256 if (rtems_rate_monotonic_ident( name_hk_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
253 257 status = rtems_rate_monotonic_create( name_hk_rate_monotonic, &HK_id );
254 258 if( status != RTEMS_SUCCESSFUL ) {
255 259 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status );
256 260 }
257 261 }
258 262
259 263 status = rtems_rate_monotonic_cancel(HK_id);
260 264 if( status != RTEMS_SUCCESSFUL ) {
261 265 PRINTF1( "ERR *** in HOUS *** rtems_rate_monotonic_cancel(HK_id) ***code: %d\n", status );
262 266 }
263 267 else {
264 268 DEBUG_PRINTF("OK *** in HOUS *** rtems_rate_monotonic_cancel(HK_id)\n");
265 269 }
266 270
267 271 // startup phase
268 272 status = rtems_rate_monotonic_period( HK_id, SY_LFR_TIME_SYN_TIMEOUT_in_ticks );
269 273 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
270 274 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
271 275 while( (period_status.state != RATE_MONOTONIC_EXPIRED)
272 276 && (isSynchronized == false) ) // after SY_LFR_TIME_SYN_TIMEOUT ms, starts HK anyway
273 277 {
274 278 if ((time_management_regs->coarse_time & VAL_LFR_SYNCHRONIZED) == INT32_ALL_0) // check time synchronization
275 279 {
276 280 isSynchronized = true;
277 281 }
278 282 else
279 283 {
280 284 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
281 285
282 286 status = rtems_task_wake_after( HK_SYNC_WAIT ); // wait HK_SYNCH_WAIT 100 ms = 10 * 10 ms
283 287 }
284 288 }
285 289 status = rtems_rate_monotonic_cancel(HK_id);
286 290 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
287 291
288 292 set_hk_lfr_reset_cause( POWER_ON );
289 293
290 294 while(1){ // launch the rate monotonic task
291 295 status = rtems_rate_monotonic_period( HK_id, HK_PERIOD );
292 296 if ( status != RTEMS_SUCCESSFUL ) {
293 297 PRINTF1( "in HOUS *** ERR period: %d\n", status);
294 298 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_6 );
295 299 }
296 300 else {
297 301 housekeeping_packet.packetSequenceControl[BYTE_0] = (unsigned char) (sequenceCounterHK >> SHIFT_1_BYTE);
298 302 housekeeping_packet.packetSequenceControl[BYTE_1] = (unsigned char) (sequenceCounterHK );
299 303 increment_seq_counter( &sequenceCounterHK );
300 304
301 305 housekeeping_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
302 306 housekeeping_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
303 307 housekeeping_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
304 308 housekeeping_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
305 309 housekeeping_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
306 310 housekeeping_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
307 311
308 312 spacewire_update_hk_lfr_link_state( &housekeeping_packet.lfr_status_word[0] );
309 313
310 314 spacewire_read_statistics();
311 315
312 316 update_hk_with_grspw_stats();
313 317
314 318 set_hk_lfr_time_not_synchro();
315 319
316 320 housekeeping_packet.hk_lfr_q_sd_fifo_size_max = hk_lfr_q_sd_fifo_size_max;
317 321 housekeeping_packet.hk_lfr_q_rv_fifo_size_max = hk_lfr_q_rv_fifo_size_max;
318 322 housekeeping_packet.hk_lfr_q_p0_fifo_size_max = hk_lfr_q_p0_fifo_size_max;
319 323 housekeeping_packet.hk_lfr_q_p1_fifo_size_max = hk_lfr_q_p1_fifo_size_max;
320 324 housekeeping_packet.hk_lfr_q_p2_fifo_size_max = hk_lfr_q_p2_fifo_size_max;
321 325
322 326 housekeeping_packet.sy_lfr_common_parameters_spare = parameter_dump_packet.sy_lfr_common_parameters_spare;
323 327 housekeeping_packet.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
324 328 get_temperatures( housekeeping_packet.hk_lfr_temp_scm );
325 329 get_v_e1_e2_f3( housekeeping_packet.hk_lfr_sc_v_f3 );
326 330 get_cpu_load( (unsigned char *) &housekeeping_packet.hk_lfr_cpu_load );
327 331
328 332 hk_lfr_le_me_he_update();
329 333
330 housekeeping_packet.hk_lfr_sc_rw_f_flags = cp_rpw_sc_rw_f_flags;
334 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = cp_rpw_sc_rw1_rw2_f_flags;
335 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = cp_rpw_sc_rw3_rw4_f_flags;
331 336
332 337 // SEND PACKET
333 338 status = rtems_message_queue_send( queue_id, &housekeeping_packet,
334 339 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
335 340 if (status != RTEMS_SUCCESSFUL) {
336 341 PRINTF1("in HOUS *** ERR send: %d\n", status)
337 342 }
338 343 }
339 344 }
340 345
341 346 PRINTF("in HOUS *** deleting task\n")
342 347
343 348 status = rtems_task_delete( RTEMS_SELF ); // should not return
344 349
345 350 return;
346 351 }
347 352
348 353 rtems_task avgv_task(rtems_task_argument argument)
349 354 {
350 355 #define MOVING_AVERAGE 16
351 356 rtems_status_code status;
352 357 static unsigned int v[MOVING_AVERAGE] = {0};
353 358 static unsigned int e1[MOVING_AVERAGE] = {0};
354 359 static unsigned int e2[MOVING_AVERAGE] = {0};
355 360 float average_v;
356 361 float average_e1;
357 362 float average_e2;
363 float newValue_v;
364 float newValue_e1;
365 float newValue_e2;
358 366 unsigned char k;
359 367 unsigned char indexOfOldValue;
360 368
361 369 BOOT_PRINTF("in AVGV ***\n");
362 370
363 371 if (rtems_rate_monotonic_ident( name_avgv_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
364 372 status = rtems_rate_monotonic_create( name_avgv_rate_monotonic, &AVGV_id );
365 373 if( status != RTEMS_SUCCESSFUL ) {
366 374 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status );
367 375 }
368 376 }
369 377
370 378 status = rtems_rate_monotonic_cancel(AVGV_id);
371 379 if( status != RTEMS_SUCCESSFUL ) {
372 380 PRINTF1( "ERR *** in AVGV *** rtems_rate_monotonic_cancel(AVGV_id) ***code: %d\n", status );
373 381 }
374 382 else {
375 383 DEBUG_PRINTF("OK *** in AVGV *** rtems_rate_monotonic_cancel(AVGV_id)\n");
376 384 }
377 385
378 386 // initialize values
379 387 indexOfOldValue = MOVING_AVERAGE - 1;
380 average_v = 0.;
381 average_e1 = 0.;
382 average_e2 = 0.;
388 average_v = INIT_FLOAT;
389 average_e1 = INIT_FLOAT;
390 average_e2 = INIT_FLOAT;
391 newValue_v = INIT_FLOAT;
392 newValue_e1 = INIT_FLOAT;
393 newValue_e2 = INIT_FLOAT;
383 394
384 k = 0;
395 k = INIT_CHAR;
385 396
386 while(1){ // launch the rate monotonic task
397 while(1)
398 { // launch the rate monotonic task
387 399 status = rtems_rate_monotonic_period( AVGV_id, AVGV_PERIOD );
388 if ( status != RTEMS_SUCCESSFUL ) {
400 if ( status != RTEMS_SUCCESSFUL )
401 {
389 402 PRINTF1( "in AVGV *** ERR period: %d\n", status);
390 403 }
391 else {
392 // store new value in buffer
393 v[k] = waveform_picker_regs->v;
394 e1[k] = waveform_picker_regs->e1;
395 e2[k] = waveform_picker_regs->e2;
396 if (k == (MOVING_AVERAGE - 1))
397 {
398 indexOfOldValue = 0;
399 }
400 else
401 {
402 indexOfOldValue = k + 1;
403 }
404 average_v = average_v + v[k] - v[indexOfOldValue];
405 average_e1 = average_e1 + e1[k] - e1[indexOfOldValue];
406 average_e2 = average_e2 + e2[k] - e2[indexOfOldValue];
404 else
405 {
406 // get new values
407 newValue_v = waveform_picker_regs->v;
408 newValue_e1 = waveform_picker_regs->e1;
409 newValue_e2 = waveform_picker_regs->e2;
410
411 // compute the moving average
412 average_v = average_v + newValue_v - v[k];
413 average_e1 = average_e1 + newValue_e1 - e1[k];
414 average_e2 = average_e2 + newValue_e2 - e2[k];
415
416 // store new values in buffers
417 v[k] = newValue_v;
418 e1[k] = newValue_e1;
419 e2[k] = newValue_e2;
407 420 }
408 421 if (k == (MOVING_AVERAGE-1))
409 422 {
410 423 k = 0;
411 424 printf("tick\n");
412 425 }
413 426 else
414 427 {
415 428 k++;
416 429 }
430 //update int16 values
431 hk_lfr_sc_v_f3_as_int16 = (int16_t) (average_v / ((float) MOVING_AVERAGE) );
432 hk_lfr_sc_e1_f3_as_int16 = (int16_t) (average_e1 / ((float) MOVING_AVERAGE) );
433 hk_lfr_sc_e2_f3_as_int16 = (int16_t) (average_e2 / ((float) MOVING_AVERAGE) );
417 434 }
418 435
419 PRINTF("in AVGV *** deleting task\n")
436 PRINTF("in AVGV *** deleting task\n");
420 437
421 status = rtems_task_delete( RTEMS_SELF ); // should not return
438 status = rtems_task_delete( RTEMS_SELF ); // should not return
422 439
423 440 return;
424 441 }
425 442
426 443 rtems_task dumb_task( rtems_task_argument unused )
427 444 {
428 445 /** This RTEMS taks is used to print messages without affecting the general behaviour of the software.
429 446 *
430 447 * @param unused is the starting argument of the RTEMS task
431 448 *
432 449 * The DUMB taks waits for RTEMS events and print messages depending on the incoming events.
433 450 *
434 451 */
435 452
436 453 unsigned int i;
437 454 unsigned int intEventOut;
438 455 unsigned int coarse_time = 0;
439 456 unsigned int fine_time = 0;
440 457 rtems_event_set event_out;
441 458
442 459 event_out = EVENT_SETS_NONE_PENDING;
443 460
444 461 BOOT_PRINTF("in DUMB *** \n")
445 462
446 463 while(1){
447 464 rtems_event_receive(RTEMS_EVENT_0 | RTEMS_EVENT_1 | RTEMS_EVENT_2 | RTEMS_EVENT_3
448 465 | RTEMS_EVENT_4 | RTEMS_EVENT_5 | RTEMS_EVENT_6 | RTEMS_EVENT_7
449 466 | RTEMS_EVENT_8 | RTEMS_EVENT_9 | RTEMS_EVENT_12 | RTEMS_EVENT_13
450 467 | RTEMS_EVENT_14,
451 468 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT
452 469 intEventOut = (unsigned int) event_out;
453 470 for ( i=0; i<NB_RTEMS_EVENTS; i++)
454 471 {
455 472 if ( ((intEventOut >> i) & 1) != 0)
456 473 {
457 474 coarse_time = time_management_regs->coarse_time;
458 475 fine_time = time_management_regs->fine_time;
459 476 if (i==EVENT_12)
460 477 {
461 478 PRINTF1("%s\n", DUMB_MESSAGE_12)
462 479 }
463 480 if (i==EVENT_13)
464 481 {
465 482 PRINTF1("%s\n", DUMB_MESSAGE_13)
466 483 }
467 484 if (i==EVENT_14)
468 485 {
469 486 PRINTF1("%s\n", DUMB_MESSAGE_1)
470 487 }
471 488 }
472 489 }
473 490 }
474 491 }
475 492
476 493 //*****************************
477 494 // init housekeeping parameters
478 495
479 496 void init_housekeeping_parameters( void )
480 497 {
481 498 /** This function initialize the housekeeping_packet global variable with default values.
482 499 *
483 500 */
484 501
485 502 unsigned int i = 0;
486 503 unsigned char *parameters;
487 504 unsigned char sizeOfHK;
488 505
489 506 sizeOfHK = sizeof( Packet_TM_LFR_HK_t );
490 507
491 508 parameters = (unsigned char*) &housekeeping_packet;
492 509
493 510 for(i = 0; i< sizeOfHK; i++)
494 511 {
495 512 parameters[i] = INIT_CHAR;
496 513 }
497 514
498 515 housekeeping_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
499 516 housekeeping_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
500 517 housekeeping_packet.reserved = DEFAULT_RESERVED;
501 518 housekeeping_packet.userApplication = CCSDS_USER_APP;
502 519 housekeeping_packet.packetID[0] = (unsigned char) (APID_TM_HK >> SHIFT_1_BYTE);
503 520 housekeeping_packet.packetID[1] = (unsigned char) (APID_TM_HK);
504 521 housekeeping_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
505 522 housekeeping_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
506 523 housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> SHIFT_1_BYTE);
507 524 housekeeping_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
508 525 housekeeping_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
509 526 housekeeping_packet.serviceType = TM_TYPE_HK;
510 527 housekeeping_packet.serviceSubType = TM_SUBTYPE_HK;
511 528 housekeeping_packet.destinationID = TM_DESTINATION_ID_GROUND;
512 529 housekeeping_packet.sid = SID_HK;
513 530
514 531 // init status word
515 532 housekeeping_packet.lfr_status_word[0] = DEFAULT_STATUS_WORD_BYTE0;
516 533 housekeeping_packet.lfr_status_word[1] = DEFAULT_STATUS_WORD_BYTE1;
517 534 // init software version
518 535 housekeeping_packet.lfr_sw_version[0] = SW_VERSION_N1;
519 536 housekeeping_packet.lfr_sw_version[1] = SW_VERSION_N2;
520 537 housekeeping_packet.lfr_sw_version[BYTE_2] = SW_VERSION_N3;
521 538 housekeeping_packet.lfr_sw_version[BYTE_3] = SW_VERSION_N4;
522 539 // init fpga version
523 540 parameters = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
524 541 housekeeping_packet.lfr_fpga_version[BYTE_0] = parameters[BYTE_1]; // n1
525 542 housekeeping_packet.lfr_fpga_version[BYTE_1] = parameters[BYTE_2]; // n2
526 543 housekeeping_packet.lfr_fpga_version[BYTE_2] = parameters[BYTE_3]; // n3
527 544
528 545 housekeeping_packet.hk_lfr_q_sd_fifo_size = MSG_QUEUE_COUNT_SEND;
529 546 housekeeping_packet.hk_lfr_q_rv_fifo_size = MSG_QUEUE_COUNT_RECV;
530 547 housekeeping_packet.hk_lfr_q_p0_fifo_size = MSG_QUEUE_COUNT_PRC0;
531 548 housekeeping_packet.hk_lfr_q_p1_fifo_size = MSG_QUEUE_COUNT_PRC1;
532 549 housekeeping_packet.hk_lfr_q_p2_fifo_size = MSG_QUEUE_COUNT_PRC2;
533 550 }
534 551
535 552 void increment_seq_counter( unsigned short *packetSequenceControl )
536 553 {
537 554 /** This function increment the sequence counter passes in argument.
538 555 *
539 556 * The increment does not affect the grouping flag. In case of an overflow, the counter is reset to 0.
540 557 *
541 558 */
542 559
543 560 unsigned short segmentation_grouping_flag;
544 561 unsigned short sequence_cnt;
545 562
546 563 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE; // keep bits 7 downto 6
547 564 sequence_cnt = (*packetSequenceControl) & SEQ_CNT_MASK; // [0011 1111 1111 1111]
548 565
549 566 if ( sequence_cnt < SEQ_CNT_MAX)
550 567 {
551 568 sequence_cnt = sequence_cnt + 1;
552 569 }
553 570 else
554 571 {
555 572 sequence_cnt = 0;
556 573 }
557 574
558 575 *packetSequenceControl = segmentation_grouping_flag | sequence_cnt ;
559 576 }
560 577
561 578 void getTime( unsigned char *time)
562 579 {
563 580 /** This function write the current local time in the time buffer passed in argument.
564 581 *
565 582 */
566 583
567 584 time[0] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_3_BYTES);
568 585 time[1] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_2_BYTES);
569 586 time[2] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_1_BYTE);
570 587 time[3] = (unsigned char) (time_management_regs->coarse_time);
571 588 time[4] = (unsigned char) (time_management_regs->fine_time>>SHIFT_1_BYTE);
572 589 time[5] = (unsigned char) (time_management_regs->fine_time);
573 590 }
574 591
575 592 unsigned long long int getTimeAsUnsignedLongLongInt( )
576 593 {
577 594 /** This function write the current local time in the time buffer passed in argument.
578 595 *
579 596 */
580 597 unsigned long long int time;
581 598
582 599 time = ( (unsigned long long int) (time_management_regs->coarse_time & COARSE_TIME_MASK) << SHIFT_2_BYTES )
583 600 + time_management_regs->fine_time;
584 601
585 602 return time;
586 603 }
587 604
588 605 void send_dumb_hk( void )
589 606 {
590 607 Packet_TM_LFR_HK_t dummy_hk_packet;
591 608 unsigned char *parameters;
592 609 unsigned int i;
593 610 rtems_id queue_id;
594 611
595 612 queue_id = RTEMS_ID_NONE;
596 613
597 614 dummy_hk_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
598 615 dummy_hk_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
599 616 dummy_hk_packet.reserved = DEFAULT_RESERVED;
600 617 dummy_hk_packet.userApplication = CCSDS_USER_APP;
601 618 dummy_hk_packet.packetID[0] = (unsigned char) (APID_TM_HK >> SHIFT_1_BYTE);
602 619 dummy_hk_packet.packetID[1] = (unsigned char) (APID_TM_HK);
603 620 dummy_hk_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
604 621 dummy_hk_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
605 622 dummy_hk_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> SHIFT_1_BYTE);
606 623 dummy_hk_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
607 624 dummy_hk_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
608 625 dummy_hk_packet.serviceType = TM_TYPE_HK;
609 626 dummy_hk_packet.serviceSubType = TM_SUBTYPE_HK;
610 627 dummy_hk_packet.destinationID = TM_DESTINATION_ID_GROUND;
611 628 dummy_hk_packet.time[0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
612 629 dummy_hk_packet.time[1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
613 630 dummy_hk_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
614 631 dummy_hk_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
615 632 dummy_hk_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
616 633 dummy_hk_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
617 634 dummy_hk_packet.sid = SID_HK;
618 635
619 636 // init status word
620 637 dummy_hk_packet.lfr_status_word[0] = INT8_ALL_F;
621 638 dummy_hk_packet.lfr_status_word[1] = INT8_ALL_F;
622 639 // init software version
623 640 dummy_hk_packet.lfr_sw_version[0] = SW_VERSION_N1;
624 641 dummy_hk_packet.lfr_sw_version[1] = SW_VERSION_N2;
625 642 dummy_hk_packet.lfr_sw_version[BYTE_2] = SW_VERSION_N3;
626 643 dummy_hk_packet.lfr_sw_version[BYTE_3] = SW_VERSION_N4;
627 644 // init fpga version
628 645 parameters = (unsigned char *) (REGS_ADDR_WAVEFORM_PICKER + APB_OFFSET_VHDL_REV);
629 646 dummy_hk_packet.lfr_fpga_version[BYTE_0] = parameters[BYTE_1]; // n1
630 647 dummy_hk_packet.lfr_fpga_version[BYTE_1] = parameters[BYTE_2]; // n2
631 648 dummy_hk_packet.lfr_fpga_version[BYTE_2] = parameters[BYTE_3]; // n3
632 649
633 650 parameters = (unsigned char *) &dummy_hk_packet.hk_lfr_cpu_load;
634 651
635 652 for (i=0; i<(BYTE_POS_HK_REACTION_WHEELS_FREQUENCY - BYTE_POS_HK_LFR_CPU_LOAD); i++)
636 653 {
637 654 parameters[i] = INT8_ALL_F;
638 655 }
639 656
640 657 get_message_queue_id_send( &queue_id );
641 658
642 659 rtems_message_queue_send( queue_id, &dummy_hk_packet,
643 660 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
644 661 }
645 662
646 663 void get_temperatures( unsigned char *temperatures )
647 664 {
648 665 unsigned char* temp_scm_ptr;
649 666 unsigned char* temp_pcb_ptr;
650 667 unsigned char* temp_fpga_ptr;
651 668
652 669 // SEL1 SEL0
653 670 // 0 0 => PCB
654 671 // 0 1 => FPGA
655 672 // 1 0 => SCM
656 673
657 674 temp_scm_ptr = (unsigned char *) &time_management_regs->temp_scm;
658 675 temp_pcb_ptr = (unsigned char *) &time_management_regs->temp_pcb;
659 676 temp_fpga_ptr = (unsigned char *) &time_management_regs->temp_fpga;
660 677
661 678 temperatures[ BYTE_0 ] = temp_scm_ptr[ BYTE_2 ];
662 679 temperatures[ BYTE_1 ] = temp_scm_ptr[ BYTE_3 ];
663 680 temperatures[ BYTE_2 ] = temp_pcb_ptr[ BYTE_2 ];
664 681 temperatures[ BYTE_3 ] = temp_pcb_ptr[ BYTE_3 ];
665 682 temperatures[ BYTE_4 ] = temp_fpga_ptr[ BYTE_2 ];
666 683 temperatures[ BYTE_5 ] = temp_fpga_ptr[ BYTE_3 ];
667 684 }
668 685
669 686 void get_v_e1_e2_f3( unsigned char *spacecraft_potential )
670 687 {
671 688 unsigned char* v_ptr;
672 689 unsigned char* e1_ptr;
673 690 unsigned char* e2_ptr;
674 691
675 v_ptr = (unsigned char *) &waveform_picker_regs->v;
676 e1_ptr = (unsigned char *) &waveform_picker_regs->e1;
677 e2_ptr = (unsigned char *) &waveform_picker_regs->e2;
692 v_ptr = (unsigned char *) &hk_lfr_sc_v_f3_as_int16;
693 e1_ptr = (unsigned char *) &hk_lfr_sc_e1_f3_as_int16;
694 e2_ptr = (unsigned char *) &hk_lfr_sc_e2_f3_as_int16;
678 695
679 spacecraft_potential[ BYTE_0 ] = v_ptr[ BYTE_2 ];
680 spacecraft_potential[ BYTE_1 ] = v_ptr[ BYTE_3 ];
681 spacecraft_potential[ BYTE_2 ] = e1_ptr[ BYTE_2 ];
682 spacecraft_potential[ BYTE_3 ] = e1_ptr[ BYTE_3 ];
683 spacecraft_potential[ BYTE_4 ] = e2_ptr[ BYTE_2 ];
684 spacecraft_potential[ BYTE_5 ] = e2_ptr[ BYTE_3 ];
696 spacecraft_potential[BYTE_0] = v_ptr[0];
697 spacecraft_potential[BYTE_1] = v_ptr[1];
698 spacecraft_potential[BYTE_2] = e1_ptr[0];
699 spacecraft_potential[BYTE_3] = e1_ptr[1];
700 spacecraft_potential[BYTE_4] = e2_ptr[0];
701 spacecraft_potential[BYTE_5] = e2_ptr[1];
685 702 }
686 703
687 704 void get_cpu_load( unsigned char *resource_statistics )
688 705 {
689 706 unsigned char cpu_load;
690 707
691 708 cpu_load = lfr_rtems_cpu_usage_report();
692 709
693 710 // HK_LFR_CPU_LOAD
694 711 resource_statistics[0] = cpu_load;
695 712
696 713 // HK_LFR_CPU_LOAD_MAX
697 714 if (cpu_load > resource_statistics[1])
698 715 {
699 716 resource_statistics[1] = cpu_load;
700 717 }
701 718
702 719 // CPU_LOAD_AVE
703 720 resource_statistics[BYTE_2] = 0;
704 721
705 722 #ifndef PRINT_TASK_STATISTICS
706 723 rtems_cpu_usage_reset();
707 724 #endif
708 725
709 726 }
710 727
711 728 void set_hk_lfr_sc_potential_flag( bool state )
712 729 {
713 730 if (state == true)
714 731 {
715 732 housekeeping_packet.lfr_status_word[1] =
716 733 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_SC_POTENTIAL_FLAG_BIT; // [0100 0000]
717 734 }
718 735 else
719 736 {
720 737 housekeeping_packet.lfr_status_word[1] =
721 738 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_SC_POTENTIAL_FLAG_MASK; // [1011 1111]
722 739 }
723 740 }
724 741
725 742 void set_sy_lfr_pas_filter_enabled( bool state )
726 743 {
727 744 if (state == true)
728 745 {
729 746 housekeeping_packet.lfr_status_word[1] =
730 747 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_SC_POTENTIAL_FLAG_BIT; // [0010 0000]
731 748 }
732 749 else
733 750 {
734 751 housekeeping_packet.lfr_status_word[1] =
735 752 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_SC_POTENTIAL_FLAG_MASK; // [1101 1111]
736 753 }
737 754 }
738 755
739 756 void set_sy_lfr_watchdog_enabled( bool state )
740 757 {
741 758 if (state == true)
742 759 {
743 760 housekeeping_packet.lfr_status_word[1] =
744 761 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_WATCHDOG_BIT; // [0001 0000]
745 762 }
746 763 else
747 764 {
748 765 housekeeping_packet.lfr_status_word[1] =
749 766 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_WATCHDOG_MASK; // [1110 1111]
750 767 }
751 768 }
752 769
753 770 void set_hk_lfr_calib_enable( bool state )
754 771 {
755 772 if (state == true)
756 773 {
757 774 housekeeping_packet.lfr_status_word[1] =
758 775 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_CALIB_BIT; // [0000 1000]
759 776 }
760 777 else
761 778 {
762 779 housekeeping_packet.lfr_status_word[1] =
763 780 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_CALIB_MASK; // [1111 0111]
764 781 }
765 782 }
766 783
767 784 void set_hk_lfr_reset_cause( enum lfr_reset_cause_t lfr_reset_cause )
768 785 {
769 786 housekeeping_packet.lfr_status_word[1] =
770 787 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_RESET_CAUSE_MASK; // [1111 1000]
771 788
772 789 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1]
773 790 | (lfr_reset_cause & STATUS_WORD_RESET_CAUSE_BITS ); // [0000 0111]
774 791
775 792 }
776 793
777 794 void increment_hk_counter( unsigned char newValue, unsigned char oldValue, unsigned int *counter )
778 795 {
779 796 int delta;
780 797
781 798 delta = 0;
782 799
783 800 if (newValue >= oldValue)
784 801 {
785 802 delta = newValue - oldValue;
786 803 }
787 804 else
788 805 {
789 806 delta = (255 - oldValue) + newValue;
790 807 }
791 808
792 809 *counter = *counter + delta;
793 810 }
794 811
795 812 void hk_lfr_le_update( void )
796 813 {
797 814 static hk_lfr_le_t old_hk_lfr_le = {0};
798 815 hk_lfr_le_t new_hk_lfr_le;
799 816 unsigned int counter;
800 817
801 818 counter = (((unsigned int) housekeeping_packet.hk_lfr_le_cnt[0]) * 256) + housekeeping_packet.hk_lfr_le_cnt[1];
802 819
803 820 // DPU
804 821 new_hk_lfr_le.dpu_spw_parity = housekeeping_packet.hk_lfr_dpu_spw_parity;
805 822 new_hk_lfr_le.dpu_spw_disconnect= housekeeping_packet.hk_lfr_dpu_spw_disconnect;
806 823 new_hk_lfr_le.dpu_spw_escape = housekeeping_packet.hk_lfr_dpu_spw_escape;
807 824 new_hk_lfr_le.dpu_spw_credit = housekeeping_packet.hk_lfr_dpu_spw_credit;
808 825 new_hk_lfr_le.dpu_spw_write_sync= housekeeping_packet.hk_lfr_dpu_spw_write_sync;
809 826 // TIMECODE
810 827 new_hk_lfr_le.timecode_erroneous= housekeeping_packet.hk_lfr_timecode_erroneous;
811 828 new_hk_lfr_le.timecode_missing = housekeeping_packet.hk_lfr_timecode_missing;
812 829 new_hk_lfr_le.timecode_invalid = housekeeping_packet.hk_lfr_timecode_invalid;
813 830 // TIME
814 831 new_hk_lfr_le.time_timecode_it = housekeeping_packet.hk_lfr_time_timecode_it;
815 832 new_hk_lfr_le.time_not_synchro = housekeeping_packet.hk_lfr_time_not_synchro;
816 833 new_hk_lfr_le.time_timecode_ctr = housekeeping_packet.hk_lfr_time_timecode_ctr;
817 834 //AHB
818 835 new_hk_lfr_le.ahb_correctable = housekeeping_packet.hk_lfr_ahb_correctable;
819 836 // housekeeping_packet.hk_lfr_dpu_spw_rx_ahb => not handled by the grspw driver
820 837 // housekeeping_packet.hk_lfr_dpu_spw_tx_ahb => not handled by the grspw driver
821 838
822 839 // update the le counter
823 840 // DPU
824 841 increment_hk_counter( new_hk_lfr_le.dpu_spw_parity, old_hk_lfr_le.dpu_spw_parity, &counter );
825 842 increment_hk_counter( new_hk_lfr_le.dpu_spw_disconnect,old_hk_lfr_le.dpu_spw_disconnect, &counter );
826 843 increment_hk_counter( new_hk_lfr_le.dpu_spw_escape, old_hk_lfr_le.dpu_spw_escape, &counter );
827 844 increment_hk_counter( new_hk_lfr_le.dpu_spw_credit, old_hk_lfr_le.dpu_spw_credit, &counter );
828 845 increment_hk_counter( new_hk_lfr_le.dpu_spw_write_sync,old_hk_lfr_le.dpu_spw_write_sync, &counter );
829 846 // TIMECODE
830 847 increment_hk_counter( new_hk_lfr_le.timecode_erroneous,old_hk_lfr_le.timecode_erroneous, &counter );
831 848 increment_hk_counter( new_hk_lfr_le.timecode_missing, old_hk_lfr_le.timecode_missing, &counter );
832 849 increment_hk_counter( new_hk_lfr_le.timecode_invalid, old_hk_lfr_le.timecode_invalid, &counter );
833 850 // TIME
834 851 increment_hk_counter( new_hk_lfr_le.time_timecode_it, old_hk_lfr_le.time_timecode_it, &counter );
835 852 increment_hk_counter( new_hk_lfr_le.time_not_synchro, old_hk_lfr_le.time_not_synchro, &counter );
836 853 increment_hk_counter( new_hk_lfr_le.time_timecode_ctr, old_hk_lfr_le.time_timecode_ctr, &counter );
837 854 // AHB
838 855 increment_hk_counter( new_hk_lfr_le.ahb_correctable, old_hk_lfr_le.ahb_correctable, &counter );
839 856
840 857 // DPU
841 858 old_hk_lfr_le.dpu_spw_parity = new_hk_lfr_le.dpu_spw_parity;
842 859 old_hk_lfr_le.dpu_spw_disconnect= new_hk_lfr_le.dpu_spw_disconnect;
843 860 old_hk_lfr_le.dpu_spw_escape = new_hk_lfr_le.dpu_spw_escape;
844 861 old_hk_lfr_le.dpu_spw_credit = new_hk_lfr_le.dpu_spw_credit;
845 862 old_hk_lfr_le.dpu_spw_write_sync= new_hk_lfr_le.dpu_spw_write_sync;
846 863 // TIMECODE
847 864 old_hk_lfr_le.timecode_erroneous= new_hk_lfr_le.timecode_erroneous;
848 865 old_hk_lfr_le.timecode_missing = new_hk_lfr_le.timecode_missing;
849 866 old_hk_lfr_le.timecode_invalid = new_hk_lfr_le.timecode_invalid;
850 867 // TIME
851 868 old_hk_lfr_le.time_timecode_it = new_hk_lfr_le.time_timecode_it;
852 869 old_hk_lfr_le.time_not_synchro = new_hk_lfr_le.time_not_synchro;
853 870 old_hk_lfr_le.time_timecode_ctr = new_hk_lfr_le.time_timecode_ctr;
854 871 //AHB
855 872 old_hk_lfr_le.ahb_correctable = new_hk_lfr_le.ahb_correctable;
856 873 // housekeeping_packet.hk_lfr_dpu_spw_rx_ahb => not handled by the grspw driver
857 874 // housekeeping_packet.hk_lfr_dpu_spw_tx_ahb => not handled by the grspw driver
858 875
859 876 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
860 877 // LE
861 878 housekeeping_packet.hk_lfr_le_cnt[0] = (unsigned char) ((counter & BYTE0_MASK) >> SHIFT_1_BYTE);
862 879 housekeeping_packet.hk_lfr_le_cnt[1] = (unsigned char) (counter & BYTE1_MASK);
863 880 }
864 881
865 882 void hk_lfr_me_update( void )
866 883 {
867 884 static hk_lfr_me_t old_hk_lfr_me = {0};
868 885 hk_lfr_me_t new_hk_lfr_me;
869 886 unsigned int counter;
870 887
871 888 counter = (((unsigned int) housekeeping_packet.hk_lfr_me_cnt[0]) * 256) + housekeeping_packet.hk_lfr_me_cnt[1];
872 889
873 890 // get the current values
874 891 new_hk_lfr_me.dpu_spw_early_eop = housekeeping_packet.hk_lfr_dpu_spw_early_eop;
875 892 new_hk_lfr_me.dpu_spw_invalid_addr = housekeeping_packet.hk_lfr_dpu_spw_invalid_addr;
876 893 new_hk_lfr_me.dpu_spw_eep = housekeeping_packet.hk_lfr_dpu_spw_eep;
877 894 new_hk_lfr_me.dpu_spw_rx_too_big = housekeeping_packet.hk_lfr_dpu_spw_rx_too_big;
878 895
879 896 // update the me counter
880 897 increment_hk_counter( new_hk_lfr_me.dpu_spw_early_eop, old_hk_lfr_me.dpu_spw_early_eop, &counter );
881 898 increment_hk_counter( new_hk_lfr_me.dpu_spw_invalid_addr, old_hk_lfr_me.dpu_spw_invalid_addr, &counter );
882 899 increment_hk_counter( new_hk_lfr_me.dpu_spw_eep, old_hk_lfr_me.dpu_spw_eep, &counter );
883 900 increment_hk_counter( new_hk_lfr_me.dpu_spw_rx_too_big, old_hk_lfr_me.dpu_spw_rx_too_big, &counter );
884 901
885 902 // store the counters for the next time
886 903 old_hk_lfr_me.dpu_spw_early_eop = new_hk_lfr_me.dpu_spw_early_eop;
887 904 old_hk_lfr_me.dpu_spw_invalid_addr = new_hk_lfr_me.dpu_spw_invalid_addr;
888 905 old_hk_lfr_me.dpu_spw_eep = new_hk_lfr_me.dpu_spw_eep;
889 906 old_hk_lfr_me.dpu_spw_rx_too_big = new_hk_lfr_me.dpu_spw_rx_too_big;
890 907
891 908 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
892 909 // ME
893 910 housekeeping_packet.hk_lfr_me_cnt[0] = (unsigned char) ((counter & BYTE0_MASK) >> SHIFT_1_BYTE);
894 911 housekeeping_packet.hk_lfr_me_cnt[1] = (unsigned char) (counter & BYTE1_MASK);
895 912 }
896 913
897 914 void hk_lfr_le_me_he_update()
898 915 {
899 916
900 917 unsigned int hk_lfr_he_cnt;
901 918
902 919 hk_lfr_he_cnt = (((unsigned int) housekeeping_packet.hk_lfr_he_cnt[0]) * 256) + housekeeping_packet.hk_lfr_he_cnt[1];
903 920
904 921 //update the low severity error counter
905 922 hk_lfr_le_update( );
906 923
907 924 //update the medium severity error counter
908 925 hk_lfr_me_update();
909 926
910 927 //update the high severity error counter
911 928 hk_lfr_he_cnt = 0;
912 929
913 930 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
914 931 // HE
915 932 housekeeping_packet.hk_lfr_he_cnt[0] = (unsigned char) ((hk_lfr_he_cnt & BYTE0_MASK) >> SHIFT_1_BYTE);
916 933 housekeeping_packet.hk_lfr_he_cnt[1] = (unsigned char) (hk_lfr_he_cnt & BYTE1_MASK);
917 934
918 935 }
919 936
920 937 void set_hk_lfr_time_not_synchro()
921 938 {
922 939 static unsigned char synchroLost = 1;
923 940 int synchronizationBit;
924 941
925 942 // get the synchronization bit
926 943 synchronizationBit =
927 944 (time_management_regs->coarse_time & VAL_LFR_SYNCHRONIZED) >> BIT_SYNCHRONIZATION; // 1000 0000 0000 0000
928 945
929 946 switch (synchronizationBit)
930 947 {
931 948 case 0:
932 949 if (synchroLost == 1)
933 950 {
934 951 synchroLost = 0;
935 952 }
936 953 break;
937 954 case 1:
938 955 if (synchroLost == 0 )
939 956 {
940 957 synchroLost = 1;
941 958 increase_unsigned_char_counter(&housekeeping_packet.hk_lfr_time_not_synchro);
942 959 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_NOT_SYNCHRO );
943 960 }
944 961 break;
945 962 default:
946 963 PRINTF1("in hk_lfr_time_not_synchro *** unexpected value for synchronizationBit = %d\n", synchronizationBit);
947 964 break;
948 965 }
949 966
950 967 }
951 968
952 969 void set_hk_lfr_ahb_correctable() // CRITICITY L
953 970 {
954 971 /** This function builds the error counter hk_lfr_ahb_correctable using the statistics provided
955 972 * by the Cache Control Register (ASI 2, offset 0) and in the Register Protection Control Register (ASR16) on the
956 973 * detected errors in the cache, in the integer unit and in the floating point unit.
957 974 *
958 975 * @param void
959 976 *
960 977 * @return void
961 978 *
962 979 * All errors are summed to set the value of the hk_lfr_ahb_correctable counter.
963 980 *
964 981 */
965 982
966 983 unsigned int ahb_correctable;
967 984 unsigned int instructionErrorCounter;
968 985 unsigned int dataErrorCounter;
969 986 unsigned int fprfErrorCounter;
970 987 unsigned int iurfErrorCounter;
971 988
972 989 instructionErrorCounter = 0;
973 990 dataErrorCounter = 0;
974 991 fprfErrorCounter = 0;
975 992 iurfErrorCounter = 0;
976 993
977 994 CCR_getInstructionAndDataErrorCounters( &instructionErrorCounter, &dataErrorCounter);
978 995 ASR16_get_FPRF_IURF_ErrorCounters( &fprfErrorCounter, &iurfErrorCounter);
979 996
980 997 ahb_correctable = instructionErrorCounter
981 998 + dataErrorCounter
982 999 + fprfErrorCounter
983 1000 + iurfErrorCounter
984 1001 + housekeeping_packet.hk_lfr_ahb_correctable;
985 1002
986 1003 housekeeping_packet.hk_lfr_ahb_correctable = (unsigned char) (ahb_correctable & INT8_ALL_F); // [1111 1111]
987 1004
988 1005 }
@@ -1,1663 +1,1664
1 1 /** Functions and tasks related to TeleCommand handling.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TeleCommands:\n
7 7 * action launching\n
8 8 * TC parsing\n
9 9 * ...
10 10 *
11 11 */
12 12
13 13 #include "tc_handler.h"
14 14 #include "math.h"
15 15
16 16 //***********
17 17 // RTEMS TASK
18 18
19 19 rtems_task actn_task( rtems_task_argument unused )
20 20 {
21 21 /** This RTEMS task is responsible for launching actions upton the reception of valid TeleCommands.
22 22 *
23 23 * @param unused is the starting argument of the RTEMS task
24 24 *
25 25 * The ACTN task waits for data coming from an RTEMS msesage queue. When data arrives, it launches specific actions depending
26 26 * on the incoming TeleCommand.
27 27 *
28 28 */
29 29
30 30 int result;
31 31 rtems_status_code status; // RTEMS status code
32 32 ccsdsTelecommandPacket_t TC; // TC sent to the ACTN task
33 33 size_t size; // size of the incoming TC packet
34 34 unsigned char subtype; // subtype of the current TC packet
35 35 unsigned char time[BYTES_PER_TIME];
36 36 rtems_id queue_rcv_id;
37 37 rtems_id queue_snd_id;
38 38
39 39 memset(&TC, 0, sizeof(ccsdsTelecommandPacket_t));
40 40 size = 0;
41 41 queue_rcv_id = RTEMS_ID_NONE;
42 42 queue_snd_id = RTEMS_ID_NONE;
43 43
44 44 status = get_message_queue_id_recv( &queue_rcv_id );
45 45 if (status != RTEMS_SUCCESSFUL)
46 46 {
47 47 PRINTF1("in ACTN *** ERR get_message_queue_id_recv %d\n", status)
48 48 }
49 49
50 50 status = get_message_queue_id_send( &queue_snd_id );
51 51 if (status != RTEMS_SUCCESSFUL)
52 52 {
53 53 PRINTF1("in ACTN *** ERR get_message_queue_id_send %d\n", status)
54 54 }
55 55
56 56 result = LFR_SUCCESSFUL;
57 57 subtype = 0; // subtype of the current TC packet
58 58
59 59 BOOT_PRINTF("in ACTN *** \n");
60 60
61 61 while(1)
62 62 {
63 63 status = rtems_message_queue_receive( queue_rcv_id, (char*) &TC, &size,
64 64 RTEMS_WAIT, RTEMS_NO_TIMEOUT);
65 65 getTime( time ); // set time to the current time
66 66 if (status!=RTEMS_SUCCESSFUL)
67 67 {
68 68 PRINTF1("ERR *** in task ACTN *** error receiving a message, code %d \n", status)
69 69 }
70 70 else
71 71 {
72 72 subtype = TC.serviceSubType;
73 73 switch(subtype)
74 74 {
75 75 case TC_SUBTYPE_RESET:
76 76 result = action_reset( &TC, queue_snd_id, time );
77 77 close_action( &TC, result, queue_snd_id );
78 78 break;
79 79 case TC_SUBTYPE_LOAD_COMM:
80 80 result = action_load_common_par( &TC );
81 81 close_action( &TC, result, queue_snd_id );
82 82 break;
83 83 case TC_SUBTYPE_LOAD_NORM:
84 84 result = action_load_normal_par( &TC, queue_snd_id, time );
85 85 close_action( &TC, result, queue_snd_id );
86 86 break;
87 87 case TC_SUBTYPE_LOAD_BURST:
88 88 result = action_load_burst_par( &TC, queue_snd_id, time );
89 89 close_action( &TC, result, queue_snd_id );
90 90 break;
91 91 case TC_SUBTYPE_LOAD_SBM1:
92 92 result = action_load_sbm1_par( &TC, queue_snd_id, time );
93 93 close_action( &TC, result, queue_snd_id );
94 94 break;
95 95 case TC_SUBTYPE_LOAD_SBM2:
96 96 result = action_load_sbm2_par( &TC, queue_snd_id, time );
97 97 close_action( &TC, result, queue_snd_id );
98 98 break;
99 99 case TC_SUBTYPE_DUMP:
100 100 result = action_dump_par( &TC, queue_snd_id );
101 101 close_action( &TC, result, queue_snd_id );
102 102 break;
103 103 case TC_SUBTYPE_ENTER:
104 104 result = action_enter_mode( &TC, queue_snd_id );
105 105 close_action( &TC, result, queue_snd_id );
106 106 break;
107 107 case TC_SUBTYPE_UPDT_INFO:
108 108 result = action_update_info( &TC, queue_snd_id );
109 109 close_action( &TC, result, queue_snd_id );
110 110 break;
111 111 case TC_SUBTYPE_EN_CAL:
112 112 result = action_enable_calibration( &TC, queue_snd_id, time );
113 113 close_action( &TC, result, queue_snd_id );
114 114 break;
115 115 case TC_SUBTYPE_DIS_CAL:
116 116 result = action_disable_calibration( &TC, queue_snd_id, time );
117 117 close_action( &TC, result, queue_snd_id );
118 118 break;
119 119 case TC_SUBTYPE_LOAD_K:
120 120 result = action_load_kcoefficients( &TC, queue_snd_id, time );
121 121 close_action( &TC, result, queue_snd_id );
122 122 break;
123 123 case TC_SUBTYPE_DUMP_K:
124 124 result = action_dump_kcoefficients( &TC, queue_snd_id, time );
125 125 close_action( &TC, result, queue_snd_id );
126 126 break;
127 127 case TC_SUBTYPE_LOAD_FBINS:
128 128 result = action_load_fbins_mask( &TC, queue_snd_id, time );
129 129 close_action( &TC, result, queue_snd_id );
130 130 break;
131 131 case TC_SUBTYPE_LOAD_FILTER_PAR:
132 132 result = action_load_filter_par( &TC, queue_snd_id, time );
133 133 close_action( &TC, result, queue_snd_id );
134 134 break;
135 135 case TC_SUBTYPE_UPDT_TIME:
136 136 result = action_update_time( &TC );
137 137 close_action( &TC, result, queue_snd_id );
138 138 break;
139 139 default:
140 140 break;
141 141 }
142 142 }
143 143 }
144 144 }
145 145
146 146 //***********
147 147 // TC ACTIONS
148 148
149 149 int action_reset(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
150 150 {
151 151 /** This function executes specific actions when a TC_LFR_RESET TeleCommand has been received.
152 152 *
153 153 * @param TC points to the TeleCommand packet that is being processed
154 154 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
155 155 *
156 156 */
157 157
158 158 PRINTF("this is the end!!!\n");
159 159 exit(0);
160 160
161 161 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
162 162
163 163 return LFR_DEFAULT;
164 164 }
165 165
166 166 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
167 167 {
168 168 /** This function executes specific actions when a TC_LFR_ENTER_MODE TeleCommand has been received.
169 169 *
170 170 * @param TC points to the TeleCommand packet that is being processed
171 171 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
172 172 *
173 173 */
174 174
175 175 rtems_status_code status;
176 176 unsigned char requestedMode;
177 177 unsigned int *transitionCoarseTime_ptr;
178 178 unsigned int transitionCoarseTime;
179 179 unsigned char * bytePosPtr;
180 180
181 181 bytePosPtr = (unsigned char *) &TC->packetID;
182 182
183 183 requestedMode = bytePosPtr[ BYTE_POS_CP_MODE_LFR_SET ];
184 184 transitionCoarseTime_ptr = (unsigned int *) ( &bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME ] );
185 185 transitionCoarseTime = (*transitionCoarseTime_ptr) & COARSE_TIME_MASK;
186 186
187 187 status = check_mode_value( requestedMode );
188 188
189 189 if ( status != LFR_SUCCESSFUL ) // the mode value is inconsistent
190 190 {
191 191 send_tm_lfr_tc_exe_inconsistent( TC, queue_id, BYTE_POS_CP_MODE_LFR_SET, requestedMode );
192 192 }
193 193
194 194 else // the mode value is valid, check the transition
195 195 {
196 196 status = check_mode_transition(requestedMode);
197 197 if (status != LFR_SUCCESSFUL)
198 198 {
199 199 PRINTF("ERR *** in action_enter_mode *** check_mode_transition\n")
200 200 send_tm_lfr_tc_exe_not_executable( TC, queue_id );
201 201 }
202 202 }
203 203
204 204 if ( status == LFR_SUCCESSFUL ) // the transition is valid, check the date
205 205 {
206 206 status = check_transition_date( transitionCoarseTime );
207 207 if (status != LFR_SUCCESSFUL)
208 208 {
209 209 PRINTF("ERR *** in action_enter_mode *** check_transition_date\n");
210 210 send_tm_lfr_tc_exe_not_executable(TC, queue_id );
211 211 }
212 212 }
213 213
214 214 if ( status == LFR_SUCCESSFUL ) // the date is valid, enter the mode
215 215 {
216 216 PRINTF1("OK *** in action_enter_mode *** enter mode %d\n", requestedMode);
217 217
218 218 switch(requestedMode)
219 219 {
220 220 case LFR_MODE_STANDBY:
221 221 status = enter_mode_standby();
222 222 break;
223 223 case LFR_MODE_NORMAL:
224 224 status = enter_mode_normal( transitionCoarseTime );
225 225 break;
226 226 case LFR_MODE_BURST:
227 227 status = enter_mode_burst( transitionCoarseTime );
228 228 break;
229 229 case LFR_MODE_SBM1:
230 230 status = enter_mode_sbm1( transitionCoarseTime );
231 231 break;
232 232 case LFR_MODE_SBM2:
233 233 status = enter_mode_sbm2( transitionCoarseTime );
234 234 break;
235 235 default:
236 236 break;
237 237 }
238 238
239 239 if (status != RTEMS_SUCCESSFUL)
240 240 {
241 241 status = LFR_EXE_ERROR;
242 242 }
243 243 }
244 244
245 245 return status;
246 246 }
247 247
248 248 int action_update_info(ccsdsTelecommandPacket_t *TC, rtems_id queue_id)
249 249 {
250 250 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
251 251 *
252 252 * @param TC points to the TeleCommand packet that is being processed
253 253 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
254 254 *
255 255 * @return LFR directive status code:
256 256 * - LFR_DEFAULT
257 257 * - LFR_SUCCESSFUL
258 258 *
259 259 */
260 260
261 261 unsigned int val;
262 262 int result;
263 263 unsigned int status;
264 264 unsigned char mode;
265 265 unsigned char * bytePosPtr;
266 266
267 267 bytePosPtr = (unsigned char *) &TC->packetID;
268 268
269 269 // check LFR mode
270 270 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET5 ] & BITS_LFR_MODE) >> SHIFT_LFR_MODE;
271 271 status = check_update_info_hk_lfr_mode( mode );
272 272 if (status == LFR_SUCCESSFUL) // check TDS mode
273 273 {
274 274 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & BITS_TDS_MODE) >> SHIFT_TDS_MODE;
275 275 status = check_update_info_hk_tds_mode( mode );
276 276 }
277 277 if (status == LFR_SUCCESSFUL) // check THR mode
278 278 {
279 279 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & BITS_THR_MODE);
280 280 status = check_update_info_hk_thr_mode( mode );
281 281 }
282 282 if (status == LFR_SUCCESSFUL) // if the parameter check is successful
283 283 {
284 284 val = (housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * CONST_256)
285 285 + housekeeping_packet.hk_lfr_update_info_tc_cnt[1];
286 286 val++;
287 287 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
288 288 housekeeping_packet.hk_lfr_update_info_tc_cnt[1] = (unsigned char) (val);
289 289 }
290 290
291 291 // pa_bia_status_info
292 292 // => pa_bia_mode_mux_set 3 bits
293 293 // => pa_bia_mode_hv_enabled 1 bit
294 294 // => pa_bia_mode_bias1_enabled 1 bit
295 295 // => pa_bia_mode_bias2_enabled 1 bit
296 296 // => pa_bia_mode_bias3_enabled 1 bit
297 297 // => pa_bia_on_off (cp_dpu_bias_on_off)
298 298 pa_bia_status_info = bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET2 ] & BITS_BIA; // [1111 1110]
299 299 pa_bia_status_info = pa_bia_status_info
300 300 | (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET1 ] & 1);
301 301
302 302 // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets)
303 303
304 cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ];
304 //cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ];
305 305 getReactionWheelsFrequencies( TC );
306 set_hk_lfr_sc_rw_f_flags();
306 307 build_sy_lfr_rw_masks();
307 308
308 309 // once the masks are built, they have to be merged with the fbins_mask
309 310 merge_fbins_masks();
310 311
311 312 result = status;
312 313
313 314 return result;
314 315 }
315 316
316 317 int action_enable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
317 318 {
318 319 /** This function executes specific actions when a TC_LFR_ENABLE_CALIBRATION TeleCommand has been received.
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 transmission by the SpaceWire driver
322 323 *
323 324 */
324 325
325 326 int result;
326 327
327 328 result = LFR_DEFAULT;
328 329
329 330 setCalibration( true );
330 331
331 332 result = LFR_SUCCESSFUL;
332 333
333 334 return result;
334 335 }
335 336
336 337 int action_disable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
337 338 {
338 339 /** This function executes specific actions when a TC_LFR_DISABLE_CALIBRATION TeleCommand has been received.
339 340 *
340 341 * @param TC points to the TeleCommand packet that is being processed
341 342 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
342 343 *
343 344 */
344 345
345 346 int result;
346 347
347 348 result = LFR_DEFAULT;
348 349
349 350 setCalibration( false );
350 351
351 352 result = LFR_SUCCESSFUL;
352 353
353 354 return result;
354 355 }
355 356
356 357 int action_update_time(ccsdsTelecommandPacket_t *TC)
357 358 {
358 359 /** This function executes specific actions when a TC_LFR_UPDATE_TIME TeleCommand has been received.
359 360 *
360 361 * @param TC points to the TeleCommand packet that is being processed
361 362 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
362 363 *
363 364 * @return LFR_SUCCESSFUL
364 365 *
365 366 */
366 367
367 368 unsigned int val;
368 369
369 370 time_management_regs->coarse_time_load = (TC->dataAndCRC[BYTE_0] << SHIFT_3_BYTES)
370 371 + (TC->dataAndCRC[BYTE_1] << SHIFT_2_BYTES)
371 372 + (TC->dataAndCRC[BYTE_2] << SHIFT_1_BYTE)
372 373 + TC->dataAndCRC[BYTE_3];
373 374
374 375 val = (housekeeping_packet.hk_lfr_update_time_tc_cnt[0] * CONST_256)
375 376 + housekeeping_packet.hk_lfr_update_time_tc_cnt[1];
376 377 val++;
377 378 housekeeping_packet.hk_lfr_update_time_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
378 379 housekeeping_packet.hk_lfr_update_time_tc_cnt[1] = (unsigned char) (val);
379 380
380 381 oneTcLfrUpdateTimeReceived = 1;
381 382
382 383 return LFR_SUCCESSFUL;
383 384 }
384 385
385 386 //*******************
386 387 // ENTERING THE MODES
387 388 int check_mode_value( unsigned char requestedMode )
388 389 {
389 390 int status;
390 391
391 392 status = LFR_DEFAULT;
392 393
393 394 if ( (requestedMode != LFR_MODE_STANDBY)
394 395 && (requestedMode != LFR_MODE_NORMAL) && (requestedMode != LFR_MODE_BURST)
395 396 && (requestedMode != LFR_MODE_SBM1) && (requestedMode != LFR_MODE_SBM2) )
396 397 {
397 398 status = LFR_DEFAULT;
398 399 }
399 400 else
400 401 {
401 402 status = LFR_SUCCESSFUL;
402 403 }
403 404
404 405 return status;
405 406 }
406 407
407 408 int check_mode_transition( unsigned char requestedMode )
408 409 {
409 410 /** This function checks the validity of the transition requested by the TC_LFR_ENTER_MODE.
410 411 *
411 412 * @param requestedMode is the mode requested by the TC_LFR_ENTER_MODE
412 413 *
413 414 * @return LFR directive status codes:
414 415 * - LFR_SUCCESSFUL - the transition is authorized
415 416 * - LFR_DEFAULT - the transition is not authorized
416 417 *
417 418 */
418 419
419 420 int status;
420 421
421 422 switch (requestedMode)
422 423 {
423 424 case LFR_MODE_STANDBY:
424 425 if ( lfrCurrentMode == LFR_MODE_STANDBY ) {
425 426 status = LFR_DEFAULT;
426 427 }
427 428 else
428 429 {
429 430 status = LFR_SUCCESSFUL;
430 431 }
431 432 break;
432 433 case LFR_MODE_NORMAL:
433 434 if ( lfrCurrentMode == LFR_MODE_NORMAL ) {
434 435 status = LFR_DEFAULT;
435 436 }
436 437 else {
437 438 status = LFR_SUCCESSFUL;
438 439 }
439 440 break;
440 441 case LFR_MODE_BURST:
441 442 if ( lfrCurrentMode == LFR_MODE_BURST ) {
442 443 status = LFR_DEFAULT;
443 444 }
444 445 else {
445 446 status = LFR_SUCCESSFUL;
446 447 }
447 448 break;
448 449 case LFR_MODE_SBM1:
449 450 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
450 451 status = LFR_DEFAULT;
451 452 }
452 453 else {
453 454 status = LFR_SUCCESSFUL;
454 455 }
455 456 break;
456 457 case LFR_MODE_SBM2:
457 458 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
458 459 status = LFR_DEFAULT;
459 460 }
460 461 else {
461 462 status = LFR_SUCCESSFUL;
462 463 }
463 464 break;
464 465 default:
465 466 status = LFR_DEFAULT;
466 467 break;
467 468 }
468 469
469 470 return status;
470 471 }
471 472
472 473 void update_last_valid_transition_date( unsigned int transitionCoarseTime )
473 474 {
474 475 if (transitionCoarseTime == 0)
475 476 {
476 477 lastValidEnterModeTime = time_management_regs->coarse_time + 1;
477 478 PRINTF1("lastValidEnterModeTime = 0x%x (transitionCoarseTime = 0 => coarse_time+1)\n", lastValidEnterModeTime);
478 479 }
479 480 else
480 481 {
481 482 lastValidEnterModeTime = transitionCoarseTime;
482 483 PRINTF1("lastValidEnterModeTime = 0x%x\n", transitionCoarseTime);
483 484 }
484 485 }
485 486
486 487 int check_transition_date( unsigned int transitionCoarseTime )
487 488 {
488 489 int status;
489 490 unsigned int localCoarseTime;
490 491 unsigned int deltaCoarseTime;
491 492
492 493 status = LFR_SUCCESSFUL;
493 494
494 495 if (transitionCoarseTime == 0) // transition time = 0 means an instant transition
495 496 {
496 497 status = LFR_SUCCESSFUL;
497 498 }
498 499 else
499 500 {
500 501 localCoarseTime = time_management_regs->coarse_time & COARSE_TIME_MASK;
501 502
502 503 PRINTF2("localTime = %x, transitionTime = %x\n", localCoarseTime, transitionCoarseTime);
503 504
504 505 if ( transitionCoarseTime <= localCoarseTime ) // SSS-CP-EQS-322
505 506 {
506 507 status = LFR_DEFAULT;
507 508 PRINTF("ERR *** in check_transition_date *** transitionCoarseTime <= localCoarseTime\n");
508 509 }
509 510
510 511 if (status == LFR_SUCCESSFUL)
511 512 {
512 513 deltaCoarseTime = transitionCoarseTime - localCoarseTime;
513 514 if ( deltaCoarseTime > MAX_DELTA_COARSE_TIME ) // SSS-CP-EQS-323
514 515 {
515 516 status = LFR_DEFAULT;
516 517 PRINTF1("ERR *** in check_transition_date *** deltaCoarseTime = %x\n", deltaCoarseTime)
517 518 }
518 519 }
519 520 }
520 521
521 522 return status;
522 523 }
523 524
524 525 int restart_asm_activities( unsigned char lfrRequestedMode )
525 526 {
526 527 rtems_status_code status;
527 528
528 529 status = stop_spectral_matrices();
529 530
530 531 thisIsAnASMRestart = 1;
531 532
532 533 status = restart_asm_tasks( lfrRequestedMode );
533 534
534 535 launch_spectral_matrix();
535 536
536 537 return status;
537 538 }
538 539
539 540 int stop_spectral_matrices( void )
540 541 {
541 542 /** This function stops and restarts the current mode average spectral matrices activities.
542 543 *
543 544 * @return RTEMS directive status codes:
544 545 * - RTEMS_SUCCESSFUL - task restarted successfully
545 546 * - RTEMS_INVALID_ID - task id invalid
546 547 * - RTEMS_ALREADY_SUSPENDED - task already suspended
547 548 *
548 549 */
549 550
550 551 rtems_status_code status;
551 552
552 553 status = RTEMS_SUCCESSFUL;
553 554
554 555 // (1) mask interruptions
555 556 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // mask spectral matrix interrupt
556 557
557 558 // (2) reset spectral matrices registers
558 559 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
559 560 reset_sm_status();
560 561
561 562 // (3) clear interruptions
562 563 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
563 564
564 565 // suspend several tasks
565 566 if (lfrCurrentMode != LFR_MODE_STANDBY) {
566 567 status = suspend_asm_tasks();
567 568 }
568 569
569 570 if (status != RTEMS_SUCCESSFUL)
570 571 {
571 572 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
572 573 }
573 574
574 575 return status;
575 576 }
576 577
577 578 int stop_current_mode( void )
578 579 {
579 580 /** This function stops the current mode by masking interrupt lines and suspending science tasks.
580 581 *
581 582 * @return RTEMS directive status codes:
582 583 * - RTEMS_SUCCESSFUL - task restarted successfully
583 584 * - RTEMS_INVALID_ID - task id invalid
584 585 * - RTEMS_ALREADY_SUSPENDED - task already suspended
585 586 *
586 587 */
587 588
588 589 rtems_status_code status;
589 590
590 591 status = RTEMS_SUCCESSFUL;
591 592
592 593 // (1) mask interruptions
593 594 LEON_Mask_interrupt( IRQ_WAVEFORM_PICKER ); // mask waveform picker interrupt
594 595 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
595 596
596 597 // (2) reset waveform picker registers
597 598 reset_wfp_burst_enable(); // reset burst and enable bits
598 599 reset_wfp_status(); // reset all the status bits
599 600
600 601 // (3) reset spectral matrices registers
601 602 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
602 603 reset_sm_status();
603 604
604 605 // reset lfr VHDL module
605 606 reset_lfr();
606 607
607 608 reset_extractSWF(); // reset the extractSWF flag to false
608 609
609 610 // (4) clear interruptions
610 611 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER ); // clear waveform picker interrupt
611 612 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
612 613
613 614 // suspend several tasks
614 615 if (lfrCurrentMode != LFR_MODE_STANDBY) {
615 616 status = suspend_science_tasks();
616 617 }
617 618
618 619 if (status != RTEMS_SUCCESSFUL)
619 620 {
620 621 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
621 622 }
622 623
623 624 return status;
624 625 }
625 626
626 627 int enter_mode_standby( void )
627 628 {
628 629 /** This function is used to put LFR in the STANDBY mode.
629 630 *
630 631 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
631 632 *
632 633 * @return RTEMS directive status codes:
633 634 * - RTEMS_SUCCESSFUL - task restarted successfully
634 635 * - RTEMS_INVALID_ID - task id invalid
635 636 * - RTEMS_INCORRECT_STATE - task never started
636 637 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
637 638 *
638 639 * The STANDBY mode does not depends on a specific transition date, the effect of the TC_LFR_ENTER_MODE
639 640 * is immediate.
640 641 *
641 642 */
642 643
643 644 int status;
644 645
645 646 status = stop_current_mode(); // STOP THE CURRENT MODE
646 647
647 648 #ifdef PRINT_TASK_STATISTICS
648 649 rtems_cpu_usage_report();
649 650 #endif
650 651
651 652 #ifdef PRINT_STACK_REPORT
652 653 PRINTF("stack report selected\n")
653 654 rtems_stack_checker_report_usage();
654 655 #endif
655 656
656 657 return status;
657 658 }
658 659
659 660 int enter_mode_normal( unsigned int transitionCoarseTime )
660 661 {
661 662 /** This function is used to start the NORMAL mode.
662 663 *
663 664 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
664 665 *
665 666 * @return RTEMS directive status codes:
666 667 * - RTEMS_SUCCESSFUL - task restarted successfully
667 668 * - RTEMS_INVALID_ID - task id invalid
668 669 * - RTEMS_INCORRECT_STATE - task never started
669 670 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
670 671 *
671 672 * The way the NORMAL mode is started depends on the LFR current mode. If LFR is in SBM1 or SBM2,
672 673 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected.
673 674 *
674 675 */
675 676
676 677 int status;
677 678
678 679 #ifdef PRINT_TASK_STATISTICS
679 680 rtems_cpu_usage_reset();
680 681 #endif
681 682
682 683 status = RTEMS_UNSATISFIED;
683 684
684 685 switch( lfrCurrentMode )
685 686 {
686 687 case LFR_MODE_STANDBY:
687 688 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart science tasks
688 689 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
689 690 {
690 691 launch_spectral_matrix( );
691 692 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
692 693 }
693 694 break;
694 695 case LFR_MODE_BURST:
695 696 status = stop_current_mode(); // stop the current mode
696 697 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart the science tasks
697 698 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
698 699 {
699 700 launch_spectral_matrix( );
700 701 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
701 702 }
702 703 break;
703 704 case LFR_MODE_SBM1:
704 705 status = restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
705 706 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
706 707 update_last_valid_transition_date( transitionCoarseTime );
707 708 break;
708 709 case LFR_MODE_SBM2:
709 710 status = restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
710 711 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
711 712 update_last_valid_transition_date( transitionCoarseTime );
712 713 break;
713 714 default:
714 715 break;
715 716 }
716 717
717 718 if (status != RTEMS_SUCCESSFUL)
718 719 {
719 720 PRINTF1("ERR *** in enter_mode_normal *** status = %d\n", status)
720 721 status = RTEMS_UNSATISFIED;
721 722 }
722 723
723 724 return status;
724 725 }
725 726
726 727 int enter_mode_burst( unsigned int transitionCoarseTime )
727 728 {
728 729 /** This function is used to start the BURST mode.
729 730 *
730 731 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
731 732 *
732 733 * @return RTEMS directive status codes:
733 734 * - RTEMS_SUCCESSFUL - task restarted successfully
734 735 * - RTEMS_INVALID_ID - task id invalid
735 736 * - RTEMS_INCORRECT_STATE - task never started
736 737 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
737 738 *
738 739 * The way the BURST mode is started does not depend on the LFR current mode.
739 740 *
740 741 */
741 742
742 743
743 744 int status;
744 745
745 746 #ifdef PRINT_TASK_STATISTICS
746 747 rtems_cpu_usage_reset();
747 748 #endif
748 749
749 750 status = stop_current_mode(); // stop the current mode
750 751 status = restart_science_tasks( LFR_MODE_BURST ); // restart the science tasks
751 752 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
752 753 {
753 754 launch_spectral_matrix( );
754 755 launch_waveform_picker( LFR_MODE_BURST, transitionCoarseTime );
755 756 }
756 757
757 758 if (status != RTEMS_SUCCESSFUL)
758 759 {
759 760 PRINTF1("ERR *** in enter_mode_burst *** status = %d\n", status)
760 761 status = RTEMS_UNSATISFIED;
761 762 }
762 763
763 764 return status;
764 765 }
765 766
766 767 int enter_mode_sbm1( unsigned int transitionCoarseTime )
767 768 {
768 769 /** This function is used to start the SBM1 mode.
769 770 *
770 771 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
771 772 *
772 773 * @return RTEMS directive status codes:
773 774 * - RTEMS_SUCCESSFUL - task restarted successfully
774 775 * - RTEMS_INVALID_ID - task id invalid
775 776 * - RTEMS_INCORRECT_STATE - task never started
776 777 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
777 778 *
778 779 * The way the SBM1 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM2,
779 780 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
780 781 * cases, the acquisition is completely restarted.
781 782 *
782 783 */
783 784
784 785 int status;
785 786
786 787 #ifdef PRINT_TASK_STATISTICS
787 788 rtems_cpu_usage_reset();
788 789 #endif
789 790
790 791 status = RTEMS_UNSATISFIED;
791 792
792 793 switch( lfrCurrentMode )
793 794 {
794 795 case LFR_MODE_STANDBY:
795 796 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart science tasks
796 797 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
797 798 {
798 799 launch_spectral_matrix( );
799 800 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
800 801 }
801 802 break;
802 803 case LFR_MODE_NORMAL: // lfrCurrentMode will be updated after the execution of close_action
803 804 status = restart_asm_activities( LFR_MODE_SBM1 );
804 805 status = LFR_SUCCESSFUL;
805 806 update_last_valid_transition_date( transitionCoarseTime );
806 807 break;
807 808 case LFR_MODE_BURST:
808 809 status = stop_current_mode(); // stop the current mode
809 810 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart the science tasks
810 811 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
811 812 {
812 813 launch_spectral_matrix( );
813 814 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
814 815 }
815 816 break;
816 817 case LFR_MODE_SBM2:
817 818 status = restart_asm_activities( LFR_MODE_SBM1 );
818 819 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
819 820 update_last_valid_transition_date( transitionCoarseTime );
820 821 break;
821 822 default:
822 823 break;
823 824 }
824 825
825 826 if (status != RTEMS_SUCCESSFUL)
826 827 {
827 828 PRINTF1("ERR *** in enter_mode_sbm1 *** status = %d\n", status);
828 829 status = RTEMS_UNSATISFIED;
829 830 }
830 831
831 832 return status;
832 833 }
833 834
834 835 int enter_mode_sbm2( unsigned int transitionCoarseTime )
835 836 {
836 837 /** This function is used to start the SBM2 mode.
837 838 *
838 839 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
839 840 *
840 841 * @return RTEMS directive status codes:
841 842 * - RTEMS_SUCCESSFUL - task restarted successfully
842 843 * - RTEMS_INVALID_ID - task id invalid
843 844 * - RTEMS_INCORRECT_STATE - task never started
844 845 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
845 846 *
846 847 * The way the SBM2 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM1,
847 848 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
848 849 * cases, the acquisition is completely restarted.
849 850 *
850 851 */
851 852
852 853 int status;
853 854
854 855 #ifdef PRINT_TASK_STATISTICS
855 856 rtems_cpu_usage_reset();
856 857 #endif
857 858
858 859 status = RTEMS_UNSATISFIED;
859 860
860 861 switch( lfrCurrentMode )
861 862 {
862 863 case LFR_MODE_STANDBY:
863 864 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart science tasks
864 865 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
865 866 {
866 867 launch_spectral_matrix( );
867 868 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
868 869 }
869 870 break;
870 871 case LFR_MODE_NORMAL:
871 872 status = restart_asm_activities( LFR_MODE_SBM2 );
872 873 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
873 874 update_last_valid_transition_date( transitionCoarseTime );
874 875 break;
875 876 case LFR_MODE_BURST:
876 877 status = stop_current_mode(); // stop the current mode
877 878 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart the science tasks
878 879 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
879 880 {
880 881 launch_spectral_matrix( );
881 882 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
882 883 }
883 884 break;
884 885 case LFR_MODE_SBM1:
885 886 status = restart_asm_activities( LFR_MODE_SBM2 );
886 887 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
887 888 update_last_valid_transition_date( transitionCoarseTime );
888 889 break;
889 890 default:
890 891 break;
891 892 }
892 893
893 894 if (status != RTEMS_SUCCESSFUL)
894 895 {
895 896 PRINTF1("ERR *** in enter_mode_sbm2 *** status = %d\n", status)
896 897 status = RTEMS_UNSATISFIED;
897 898 }
898 899
899 900 return status;
900 901 }
901 902
902 903 int restart_science_tasks( unsigned char lfrRequestedMode )
903 904 {
904 905 /** This function is used to restart all science tasks.
905 906 *
906 907 * @return RTEMS directive status codes:
907 908 * - RTEMS_SUCCESSFUL - task restarted successfully
908 909 * - RTEMS_INVALID_ID - task id invalid
909 910 * - RTEMS_INCORRECT_STATE - task never started
910 911 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
911 912 *
912 913 * Science tasks are AVF0, PRC0, WFRM, CWF3, CW2, CWF1
913 914 *
914 915 */
915 916
916 917 rtems_status_code status[NB_SCIENCE_TASKS];
917 918 rtems_status_code ret;
918 919
919 920 ret = RTEMS_SUCCESSFUL;
920 921
921 922 status[STATUS_0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
922 923 if (status[STATUS_0] != RTEMS_SUCCESSFUL)
923 924 {
924 925 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[STATUS_0])
925 926 }
926 927
927 928 status[STATUS_1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
928 929 if (status[STATUS_1] != RTEMS_SUCCESSFUL)
929 930 {
930 931 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[STATUS_1])
931 932 }
932 933
933 934 status[STATUS_2] = rtems_task_restart( Task_id[TASKID_WFRM],1 );
934 935 if (status[STATUS_2] != RTEMS_SUCCESSFUL)
935 936 {
936 937 PRINTF1("in restart_science_task *** WFRM ERR %d\n", status[STATUS_2])
937 938 }
938 939
939 940 status[STATUS_3] = rtems_task_restart( Task_id[TASKID_CWF3],1 );
940 941 if (status[STATUS_3] != RTEMS_SUCCESSFUL)
941 942 {
942 943 PRINTF1("in restart_science_task *** CWF3 ERR %d\n", status[STATUS_3])
943 944 }
944 945
945 946 status[STATUS_4] = rtems_task_restart( Task_id[TASKID_CWF2],1 );
946 947 if (status[STATUS_4] != RTEMS_SUCCESSFUL)
947 948 {
948 949 PRINTF1("in restart_science_task *** CWF2 ERR %d\n", status[STATUS_4])
949 950 }
950 951
951 952 status[STATUS_5] = rtems_task_restart( Task_id[TASKID_CWF1],1 );
952 953 if (status[STATUS_5] != RTEMS_SUCCESSFUL)
953 954 {
954 955 PRINTF1("in restart_science_task *** CWF1 ERR %d\n", status[STATUS_5])
955 956 }
956 957
957 958 status[STATUS_6] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
958 959 if (status[STATUS_6] != RTEMS_SUCCESSFUL)
959 960 {
960 961 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[STATUS_6])
961 962 }
962 963
963 964 status[STATUS_7] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
964 965 if (status[STATUS_7] != RTEMS_SUCCESSFUL)
965 966 {
966 967 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[STATUS_7])
967 968 }
968 969
969 970 status[STATUS_8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
970 971 if (status[STATUS_8] != RTEMS_SUCCESSFUL)
971 972 {
972 973 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[STATUS_8])
973 974 }
974 975
975 976 status[STATUS_9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
976 977 if (status[STATUS_9] != RTEMS_SUCCESSFUL)
977 978 {
978 979 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[STATUS_9])
979 980 }
980 981
981 982 if ( (status[STATUS_0] != RTEMS_SUCCESSFUL) || (status[STATUS_1] != RTEMS_SUCCESSFUL) ||
982 983 (status[STATUS_2] != RTEMS_SUCCESSFUL) || (status[STATUS_3] != RTEMS_SUCCESSFUL) ||
983 984 (status[STATUS_4] != RTEMS_SUCCESSFUL) || (status[STATUS_5] != RTEMS_SUCCESSFUL) ||
984 985 (status[STATUS_6] != RTEMS_SUCCESSFUL) || (status[STATUS_7] != RTEMS_SUCCESSFUL) ||
985 986 (status[STATUS_8] != RTEMS_SUCCESSFUL) || (status[STATUS_9] != RTEMS_SUCCESSFUL) )
986 987 {
987 988 ret = RTEMS_UNSATISFIED;
988 989 }
989 990
990 991 return ret;
991 992 }
992 993
993 994 int restart_asm_tasks( unsigned char lfrRequestedMode )
994 995 {
995 996 /** This function is used to restart average spectral matrices tasks.
996 997 *
997 998 * @return RTEMS directive status codes:
998 999 * - RTEMS_SUCCESSFUL - task restarted successfully
999 1000 * - RTEMS_INVALID_ID - task id invalid
1000 1001 * - RTEMS_INCORRECT_STATE - task never started
1001 1002 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
1002 1003 *
1003 1004 * ASM tasks are AVF0, PRC0, AVF1, PRC1, AVF2 and PRC2
1004 1005 *
1005 1006 */
1006 1007
1007 1008 rtems_status_code status[NB_ASM_TASKS];
1008 1009 rtems_status_code ret;
1009 1010
1010 1011 ret = RTEMS_SUCCESSFUL;
1011 1012
1012 1013 status[STATUS_0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
1013 1014 if (status[STATUS_0] != RTEMS_SUCCESSFUL)
1014 1015 {
1015 1016 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[STATUS_0])
1016 1017 }
1017 1018
1018 1019 status[STATUS_1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
1019 1020 if (status[STATUS_1] != RTEMS_SUCCESSFUL)
1020 1021 {
1021 1022 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[STATUS_1])
1022 1023 }
1023 1024
1024 1025 status[STATUS_2] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
1025 1026 if (status[STATUS_2] != RTEMS_SUCCESSFUL)
1026 1027 {
1027 1028 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[STATUS_2])
1028 1029 }
1029 1030
1030 1031 status[STATUS_3] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
1031 1032 if (status[STATUS_3] != RTEMS_SUCCESSFUL)
1032 1033 {
1033 1034 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[STATUS_3])
1034 1035 }
1035 1036
1036 1037 status[STATUS_4] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
1037 1038 if (status[STATUS_4] != RTEMS_SUCCESSFUL)
1038 1039 {
1039 1040 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[STATUS_4])
1040 1041 }
1041 1042
1042 1043 status[STATUS_5] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
1043 1044 if (status[STATUS_5] != RTEMS_SUCCESSFUL)
1044 1045 {
1045 1046 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[STATUS_5])
1046 1047 }
1047 1048
1048 1049 if ( (status[STATUS_0] != RTEMS_SUCCESSFUL) || (status[STATUS_1] != RTEMS_SUCCESSFUL) ||
1049 1050 (status[STATUS_2] != RTEMS_SUCCESSFUL) || (status[STATUS_3] != RTEMS_SUCCESSFUL) ||
1050 1051 (status[STATUS_4] != RTEMS_SUCCESSFUL) || (status[STATUS_5] != RTEMS_SUCCESSFUL) )
1051 1052 {
1052 1053 ret = RTEMS_UNSATISFIED;
1053 1054 }
1054 1055
1055 1056 return ret;
1056 1057 }
1057 1058
1058 1059 int suspend_science_tasks( void )
1059 1060 {
1060 1061 /** This function suspends the science tasks.
1061 1062 *
1062 1063 * @return RTEMS directive status codes:
1063 1064 * - RTEMS_SUCCESSFUL - task restarted successfully
1064 1065 * - RTEMS_INVALID_ID - task id invalid
1065 1066 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1066 1067 *
1067 1068 */
1068 1069
1069 1070 rtems_status_code status;
1070 1071
1071 1072 PRINTF("in suspend_science_tasks\n")
1072 1073
1073 1074 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1074 1075 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1075 1076 {
1076 1077 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1077 1078 }
1078 1079 else
1079 1080 {
1080 1081 status = RTEMS_SUCCESSFUL;
1081 1082 }
1082 1083 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1083 1084 {
1084 1085 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1085 1086 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1086 1087 {
1087 1088 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1088 1089 }
1089 1090 else
1090 1091 {
1091 1092 status = RTEMS_SUCCESSFUL;
1092 1093 }
1093 1094 }
1094 1095 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1095 1096 {
1096 1097 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1097 1098 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1098 1099 {
1099 1100 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1100 1101 }
1101 1102 else
1102 1103 {
1103 1104 status = RTEMS_SUCCESSFUL;
1104 1105 }
1105 1106 }
1106 1107 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1107 1108 {
1108 1109 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1109 1110 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1110 1111 {
1111 1112 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1112 1113 }
1113 1114 else
1114 1115 {
1115 1116 status = RTEMS_SUCCESSFUL;
1116 1117 }
1117 1118 }
1118 1119 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1119 1120 {
1120 1121 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1121 1122 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1122 1123 {
1123 1124 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1124 1125 }
1125 1126 else
1126 1127 {
1127 1128 status = RTEMS_SUCCESSFUL;
1128 1129 }
1129 1130 }
1130 1131 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1131 1132 {
1132 1133 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1133 1134 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1134 1135 {
1135 1136 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1136 1137 }
1137 1138 else
1138 1139 {
1139 1140 status = RTEMS_SUCCESSFUL;
1140 1141 }
1141 1142 }
1142 1143 if (status == RTEMS_SUCCESSFUL) // suspend WFRM
1143 1144 {
1144 1145 status = rtems_task_suspend( Task_id[TASKID_WFRM] );
1145 1146 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1146 1147 {
1147 1148 PRINTF1("in suspend_science_task *** WFRM ERR %d\n", status)
1148 1149 }
1149 1150 else
1150 1151 {
1151 1152 status = RTEMS_SUCCESSFUL;
1152 1153 }
1153 1154 }
1154 1155 if (status == RTEMS_SUCCESSFUL) // suspend CWF3
1155 1156 {
1156 1157 status = rtems_task_suspend( Task_id[TASKID_CWF3] );
1157 1158 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1158 1159 {
1159 1160 PRINTF1("in suspend_science_task *** CWF3 ERR %d\n", status)
1160 1161 }
1161 1162 else
1162 1163 {
1163 1164 status = RTEMS_SUCCESSFUL;
1164 1165 }
1165 1166 }
1166 1167 if (status == RTEMS_SUCCESSFUL) // suspend CWF2
1167 1168 {
1168 1169 status = rtems_task_suspend( Task_id[TASKID_CWF2] );
1169 1170 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1170 1171 {
1171 1172 PRINTF1("in suspend_science_task *** CWF2 ERR %d\n", status)
1172 1173 }
1173 1174 else
1174 1175 {
1175 1176 status = RTEMS_SUCCESSFUL;
1176 1177 }
1177 1178 }
1178 1179 if (status == RTEMS_SUCCESSFUL) // suspend CWF1
1179 1180 {
1180 1181 status = rtems_task_suspend( Task_id[TASKID_CWF1] );
1181 1182 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1182 1183 {
1183 1184 PRINTF1("in suspend_science_task *** CWF1 ERR %d\n", status)
1184 1185 }
1185 1186 else
1186 1187 {
1187 1188 status = RTEMS_SUCCESSFUL;
1188 1189 }
1189 1190 }
1190 1191
1191 1192 return status;
1192 1193 }
1193 1194
1194 1195 int suspend_asm_tasks( void )
1195 1196 {
1196 1197 /** This function suspends the science tasks.
1197 1198 *
1198 1199 * @return RTEMS directive status codes:
1199 1200 * - RTEMS_SUCCESSFUL - task restarted successfully
1200 1201 * - RTEMS_INVALID_ID - task id invalid
1201 1202 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1202 1203 *
1203 1204 */
1204 1205
1205 1206 rtems_status_code status;
1206 1207
1207 1208 PRINTF("in suspend_science_tasks\n")
1208 1209
1209 1210 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1210 1211 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1211 1212 {
1212 1213 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1213 1214 }
1214 1215 else
1215 1216 {
1216 1217 status = RTEMS_SUCCESSFUL;
1217 1218 }
1218 1219
1219 1220 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1220 1221 {
1221 1222 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1222 1223 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1223 1224 {
1224 1225 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1225 1226 }
1226 1227 else
1227 1228 {
1228 1229 status = RTEMS_SUCCESSFUL;
1229 1230 }
1230 1231 }
1231 1232
1232 1233 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1233 1234 {
1234 1235 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1235 1236 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1236 1237 {
1237 1238 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1238 1239 }
1239 1240 else
1240 1241 {
1241 1242 status = RTEMS_SUCCESSFUL;
1242 1243 }
1243 1244 }
1244 1245
1245 1246 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1246 1247 {
1247 1248 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1248 1249 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1249 1250 {
1250 1251 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1251 1252 }
1252 1253 else
1253 1254 {
1254 1255 status = RTEMS_SUCCESSFUL;
1255 1256 }
1256 1257 }
1257 1258
1258 1259 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1259 1260 {
1260 1261 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1261 1262 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1262 1263 {
1263 1264 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1264 1265 }
1265 1266 else
1266 1267 {
1267 1268 status = RTEMS_SUCCESSFUL;
1268 1269 }
1269 1270 }
1270 1271
1271 1272 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1272 1273 {
1273 1274 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1274 1275 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1275 1276 {
1276 1277 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1277 1278 }
1278 1279 else
1279 1280 {
1280 1281 status = RTEMS_SUCCESSFUL;
1281 1282 }
1282 1283 }
1283 1284
1284 1285 return status;
1285 1286 }
1286 1287
1287 1288 void launch_waveform_picker( unsigned char mode, unsigned int transitionCoarseTime )
1288 1289 {
1289 1290
1290 1291 WFP_reset_current_ring_nodes();
1291 1292
1292 1293 reset_waveform_picker_regs();
1293 1294
1294 1295 set_wfp_burst_enable_register( mode );
1295 1296
1296 1297 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER );
1297 1298 LEON_Unmask_interrupt( IRQ_WAVEFORM_PICKER );
1298 1299
1299 1300 if (transitionCoarseTime == 0)
1300 1301 {
1301 1302 // instant transition means transition on the next valid date
1302 1303 // this is mandatory to have a good snapshot period and a good correction of the snapshot period
1303 1304 waveform_picker_regs->start_date = time_management_regs->coarse_time + 1;
1304 1305 }
1305 1306 else
1306 1307 {
1307 1308 waveform_picker_regs->start_date = transitionCoarseTime;
1308 1309 }
1309 1310
1310 1311 update_last_valid_transition_date(waveform_picker_regs->start_date);
1311 1312
1312 1313 }
1313 1314
1314 1315 void launch_spectral_matrix( void )
1315 1316 {
1316 1317 SM_reset_current_ring_nodes();
1317 1318
1318 1319 reset_spectral_matrix_regs();
1319 1320
1320 1321 reset_nb_sm();
1321 1322
1322 1323 set_sm_irq_onNewMatrix( 1 );
1323 1324
1324 1325 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX );
1325 1326 LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRIX );
1326 1327
1327 1328 }
1328 1329
1329 1330 void set_sm_irq_onNewMatrix( unsigned char value )
1330 1331 {
1331 1332 if (value == 1)
1332 1333 {
1333 1334 spectral_matrix_regs->config = spectral_matrix_regs->config | BIT_IRQ_ON_NEW_MATRIX;
1334 1335 }
1335 1336 else
1336 1337 {
1337 1338 spectral_matrix_regs->config = spectral_matrix_regs->config & MASK_IRQ_ON_NEW_MATRIX; // 1110
1338 1339 }
1339 1340 }
1340 1341
1341 1342 void set_sm_irq_onError( unsigned char value )
1342 1343 {
1343 1344 if (value == 1)
1344 1345 {
1345 1346 spectral_matrix_regs->config = spectral_matrix_regs->config | BIT_IRQ_ON_ERROR;
1346 1347 }
1347 1348 else
1348 1349 {
1349 1350 spectral_matrix_regs->config = spectral_matrix_regs->config & MASK_IRQ_ON_ERROR; // 1101
1350 1351 }
1351 1352 }
1352 1353
1353 1354 //*****************************
1354 1355 // CONFIGURE CALIBRATION SIGNAL
1355 1356 void setCalibrationPrescaler( unsigned int prescaler )
1356 1357 {
1357 1358 // prescaling of the master clock (25 MHz)
1358 1359 // master clock is divided by 2^prescaler
1359 1360 time_management_regs->calPrescaler = prescaler;
1360 1361 }
1361 1362
1362 1363 void setCalibrationDivisor( unsigned int divisionFactor )
1363 1364 {
1364 1365 // division of the prescaled clock by the division factor
1365 1366 time_management_regs->calDivisor = divisionFactor;
1366 1367 }
1367 1368
1368 1369 void setCalibrationData( void )
1369 1370 {
1370 1371 /** This function is used to store the values used to drive the DAC in order to generate the SCM calibration signal
1371 1372 *
1372 1373 * @param void
1373 1374 *
1374 1375 * @return void
1375 1376 *
1376 1377 */
1377 1378
1378 1379 unsigned int k;
1379 1380 unsigned short data;
1380 1381 float val;
1381 1382 float Ts;
1382 1383
1383 1384 time_management_regs->calDataPtr = INIT_CHAR;
1384 1385
1385 1386 Ts = 1 / CAL_FS;
1386 1387
1387 1388 // build the signal for the SCM calibration
1388 1389 for (k = 0; k < CAL_NB_PTS; k++)
1389 1390 {
1390 1391 val = sin( 2 * pi * CAL_F0 * k * Ts )
1391 1392 + sin( 2 * pi * CAL_F1 * k * Ts );
1392 1393 data = (unsigned short) ((val * CAL_SCALE_FACTOR) + CONST_2048);
1393 1394 time_management_regs->calData = data & CAL_DATA_MASK;
1394 1395 }
1395 1396 }
1396 1397
1397 1398 void setCalibrationDataInterleaved( void )
1398 1399 {
1399 1400 /** This function is used to store the values used to drive the DAC in order to generate the SCM calibration signal
1400 1401 *
1401 1402 * @param void
1402 1403 *
1403 1404 * @return void
1404 1405 *
1405 1406 * In interleaved mode, one can store more values than in normal mode.
1406 1407 * The data are stored in bunch of 18 bits, 12 bits from one sample and 6 bits from another sample.
1407 1408 * T store 3 values, one need two write operations.
1408 1409 * s1 [ b11 b10 b9 b8 b7 b6 ] s0 [ b11 b10 b9 b8 b7 b6 b5 b3 b2 b1 b0 ]
1409 1410 * s1 [ b5 b4 b3 b2 b1 b0 ] s2 [ b11 b10 b9 b8 b7 b6 b5 b3 b2 b1 b0 ]
1410 1411 *
1411 1412 */
1412 1413
1413 1414 unsigned int k;
1414 1415 float val;
1415 1416 float Ts;
1416 1417 unsigned short data[CAL_NB_PTS_INTER];
1417 1418 unsigned char *dataPtr;
1418 1419
1419 1420 Ts = 1 / CAL_FS_INTER;
1420 1421
1421 1422 time_management_regs->calDataPtr = INIT_CHAR;
1422 1423
1423 1424 // build the signal for the SCM calibration
1424 1425 for (k=0; k<CAL_NB_PTS_INTER; k++)
1425 1426 {
1426 1427 val = sin( 2 * pi * CAL_F0 * k * Ts )
1427 1428 + sin( 2 * pi * CAL_F1 * k * Ts );
1428 1429 data[k] = (unsigned short) ((val * CONST_512) + CONST_2048);
1429 1430 }
1430 1431
1431 1432 // write the signal in interleaved mode
1432 1433 for (k=0; k < STEPS_FOR_STORAGE_INTER; k++)
1433 1434 {
1434 1435 dataPtr = (unsigned char*) &data[ (k * BYTES_FOR_2_SAMPLES) + 2 ];
1435 1436 time_management_regs->calData = ( data[ k * BYTES_FOR_2_SAMPLES ] & CAL_DATA_MASK )
1436 1437 + ( (dataPtr[0] & CAL_DATA_MASK_INTER) << CAL_DATA_SHIFT_INTER);
1437 1438 time_management_regs->calData = ( data[(k * BYTES_FOR_2_SAMPLES) + 1] & CAL_DATA_MASK )
1438 1439 + ( (dataPtr[1] & CAL_DATA_MASK_INTER) << CAL_DATA_SHIFT_INTER);
1439 1440 }
1440 1441 }
1441 1442
1442 1443 void setCalibrationReload( bool state)
1443 1444 {
1444 1445 if (state == true)
1445 1446 {
1446 1447 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | BIT_CAL_RELOAD; // [0001 0000]
1447 1448 }
1448 1449 else
1449 1450 {
1450 1451 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & MASK_CAL_RELOAD; // [1110 1111]
1451 1452 }
1452 1453 }
1453 1454
1454 1455 void setCalibrationEnable( bool state )
1455 1456 {
1456 1457 // this bit drives the multiplexer
1457 1458 if (state == true)
1458 1459 {
1459 1460 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | BIT_CAL_ENABLE; // [0100 0000]
1460 1461 }
1461 1462 else
1462 1463 {
1463 1464 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & MASK_CAL_ENABLE; // [1011 1111]
1464 1465 }
1465 1466 }
1466 1467
1467 1468 void setCalibrationInterleaved( bool state )
1468 1469 {
1469 1470 // this bit drives the multiplexer
1470 1471 if (state == true)
1471 1472 {
1472 1473 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | BIT_SET_INTERLEAVED; // [0010 0000]
1473 1474 }
1474 1475 else
1475 1476 {
1476 1477 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & MASK_SET_INTERLEAVED; // [1101 1111]
1477 1478 }
1478 1479 }
1479 1480
1480 1481 void setCalibration( bool state )
1481 1482 {
1482 1483 if (state == true)
1483 1484 {
1484 1485 setCalibrationEnable( true );
1485 1486 setCalibrationReload( false );
1486 1487 set_hk_lfr_calib_enable( true );
1487 1488 }
1488 1489 else
1489 1490 {
1490 1491 setCalibrationEnable( false );
1491 1492 setCalibrationReload( true );
1492 1493 set_hk_lfr_calib_enable( false );
1493 1494 }
1494 1495 }
1495 1496
1496 1497 void configureCalibration( bool interleaved )
1497 1498 {
1498 1499 setCalibration( false );
1499 1500 if ( interleaved == true )
1500 1501 {
1501 1502 setCalibrationInterleaved( true );
1502 1503 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1503 1504 setCalibrationDivisor( CAL_F_DIVISOR_INTER ); // => 240 384
1504 1505 setCalibrationDataInterleaved();
1505 1506 }
1506 1507 else
1507 1508 {
1508 1509 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1509 1510 setCalibrationDivisor( CAL_F_DIVISOR ); // => 160 256 (39 - 1)
1510 1511 setCalibrationData();
1511 1512 }
1512 1513 }
1513 1514
1514 1515 //****************
1515 1516 // CLOSING ACTIONS
1516 1517 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC, unsigned char * time )
1517 1518 {
1518 1519 /** This function is used to update the HK packets statistics after a successful TC execution.
1519 1520 *
1520 1521 * @param TC points to the TC being processed
1521 1522 * @param time is the time used to date the TC execution
1522 1523 *
1523 1524 */
1524 1525
1525 1526 unsigned int val;
1526 1527
1527 1528 housekeeping_packet.hk_lfr_last_exe_tc_id[0] = TC->packetID[0];
1528 1529 housekeeping_packet.hk_lfr_last_exe_tc_id[1] = TC->packetID[1];
1529 1530 housekeeping_packet.hk_lfr_last_exe_tc_type[0] = INIT_CHAR;
1530 1531 housekeeping_packet.hk_lfr_last_exe_tc_type[1] = TC->serviceType;
1531 1532 housekeeping_packet.hk_lfr_last_exe_tc_subtype[0] = INIT_CHAR;
1532 1533 housekeeping_packet.hk_lfr_last_exe_tc_subtype[1] = TC->serviceSubType;
1533 1534 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_0] = time[BYTE_0];
1534 1535 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_1] = time[BYTE_1];
1535 1536 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_2] = time[BYTE_2];
1536 1537 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_3] = time[BYTE_3];
1537 1538 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_4] = time[BYTE_4];
1538 1539 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_5] = time[BYTE_5];
1539 1540
1540 1541 val = (housekeeping_packet.hk_lfr_exe_tc_cnt[0] * CONST_256) + housekeeping_packet.hk_lfr_exe_tc_cnt[1];
1541 1542 val++;
1542 1543 housekeeping_packet.hk_lfr_exe_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
1543 1544 housekeeping_packet.hk_lfr_exe_tc_cnt[1] = (unsigned char) (val);
1544 1545 }
1545 1546
1546 1547 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC, unsigned char * time )
1547 1548 {
1548 1549 /** This function is used to update the HK packets statistics after a TC rejection.
1549 1550 *
1550 1551 * @param TC points to the TC being processed
1551 1552 * @param time is the time used to date the TC rejection
1552 1553 *
1553 1554 */
1554 1555
1555 1556 unsigned int val;
1556 1557
1557 1558 housekeeping_packet.hk_lfr_last_rej_tc_id[0] = TC->packetID[0];
1558 1559 housekeeping_packet.hk_lfr_last_rej_tc_id[1] = TC->packetID[1];
1559 1560 housekeeping_packet.hk_lfr_last_rej_tc_type[0] = INIT_CHAR;
1560 1561 housekeeping_packet.hk_lfr_last_rej_tc_type[1] = TC->serviceType;
1561 1562 housekeeping_packet.hk_lfr_last_rej_tc_subtype[0] = INIT_CHAR;
1562 1563 housekeeping_packet.hk_lfr_last_rej_tc_subtype[1] = TC->serviceSubType;
1563 1564 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_0] = time[BYTE_0];
1564 1565 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_1] = time[BYTE_1];
1565 1566 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_2] = time[BYTE_2];
1566 1567 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_3] = time[BYTE_3];
1567 1568 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_4] = time[BYTE_4];
1568 1569 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_5] = time[BYTE_5];
1569 1570
1570 1571 val = (housekeeping_packet.hk_lfr_rej_tc_cnt[0] * CONST_256) + housekeeping_packet.hk_lfr_rej_tc_cnt[1];
1571 1572 val++;
1572 1573 housekeeping_packet.hk_lfr_rej_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
1573 1574 housekeeping_packet.hk_lfr_rej_tc_cnt[1] = (unsigned char) (val);
1574 1575 }
1575 1576
1576 1577 void close_action(ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id )
1577 1578 {
1578 1579 /** This function is the last step of the TC execution workflow.
1579 1580 *
1580 1581 * @param TC points to the TC being processed
1581 1582 * @param result is the result of the TC execution (LFR_SUCCESSFUL / LFR_DEFAULT)
1582 1583 * @param queue_id is the id of the RTEMS message queue used to send TM packets
1583 1584 * @param time is the time used to date the TC execution
1584 1585 *
1585 1586 */
1586 1587
1587 1588 unsigned char requestedMode;
1588 1589
1589 1590 if (result == LFR_SUCCESSFUL)
1590 1591 {
1591 1592 if ( !( (TC->serviceType==TC_TYPE_TIME) & (TC->serviceSubType==TC_SUBTYPE_UPDT_TIME) )
1592 1593 &
1593 1594 !( (TC->serviceType==TC_TYPE_GEN) & (TC->serviceSubType==TC_SUBTYPE_UPDT_INFO))
1594 1595 )
1595 1596 {
1596 1597 send_tm_lfr_tc_exe_success( TC, queue_id );
1597 1598 }
1598 1599 if ( (TC->serviceType == TC_TYPE_GEN) & (TC->serviceSubType == TC_SUBTYPE_ENTER) )
1599 1600 {
1600 1601 //**********************************
1601 1602 // UPDATE THE LFRMODE LOCAL VARIABLE
1602 1603 requestedMode = TC->dataAndCRC[1];
1603 1604 updateLFRCurrentMode( requestedMode );
1604 1605 }
1605 1606 }
1606 1607 else if (result == LFR_EXE_ERROR)
1607 1608 {
1608 1609 send_tm_lfr_tc_exe_error( TC, queue_id );
1609 1610 }
1610 1611 }
1611 1612
1612 1613 //***************************
1613 1614 // Interrupt Service Routines
1614 1615 rtems_isr commutation_isr1( rtems_vector_number vector )
1615 1616 {
1616 1617 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1617 1618 PRINTF("In commutation_isr1 *** Error sending event to DUMB\n")
1618 1619 }
1619 1620 }
1620 1621
1621 1622 rtems_isr commutation_isr2( rtems_vector_number vector )
1622 1623 {
1623 1624 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1624 1625 PRINTF("In commutation_isr2 *** Error sending event to DUMB\n")
1625 1626 }
1626 1627 }
1627 1628
1628 1629 //****************
1629 1630 // OTHER FUNCTIONS
1630 1631 void updateLFRCurrentMode( unsigned char requestedMode )
1631 1632 {
1632 1633 /** This function updates the value of the global variable lfrCurrentMode.
1633 1634 *
1634 1635 * lfrCurrentMode is a parameter used by several functions to know in which mode LFR is running.
1635 1636 *
1636 1637 */
1637 1638
1638 1639 // update the local value of lfrCurrentMode with the value contained in the housekeeping_packet structure
1639 1640 housekeeping_packet.lfr_status_word[0] = (housekeeping_packet.lfr_status_word[0] & STATUS_WORD_LFR_MODE_MASK)
1640 1641 + (unsigned char) ( requestedMode << STATUS_WORD_LFR_MODE_SHIFT );
1641 1642 lfrCurrentMode = requestedMode;
1642 1643 }
1643 1644
1644 1645 void set_lfr_soft_reset( unsigned char value )
1645 1646 {
1646 1647 if (value == 1)
1647 1648 {
1648 1649 time_management_regs->ctrl = time_management_regs->ctrl | BIT_SOFT_RESET; // [0100]
1649 1650 }
1650 1651 else
1651 1652 {
1652 1653 time_management_regs->ctrl = time_management_regs->ctrl & MASK_SOFT_RESET; // [1011]
1653 1654 }
1654 1655 }
1655 1656
1656 1657 void reset_lfr( void )
1657 1658 {
1658 1659 set_lfr_soft_reset( 1 );
1659 1660
1660 1661 set_lfr_soft_reset( 0 );
1661 1662
1662 1663 set_hk_lfr_sc_potential_flag( true );
1663 1664 }
@@ -1,1657 +1,1780
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 = {0};
18 18 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2 = {0};
19 19 ring_node kcoefficient_node_1 = {0};
20 20 ring_node kcoefficient_node_2 = {0};
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 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 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 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 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 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 192 aux = ( (float ) sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE) )
193 193 - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE));
194 194 if (aux > FLOAT_EQUAL_ZERO)
195 195 {
196 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 );
197 197 flag = LFR_DEFAULT;
198 198 }
199 199 }
200 200
201 201 // SET THE PARAMETERS
202 202 if (flag == LFR_SUCCESSFUL)
203 203 {
204 204 flag = set_sy_lfr_s1_bp_p0( TC );
205 205 flag = set_sy_lfr_s1_bp_p1( TC );
206 206 }
207 207
208 208 return flag;
209 209 }
210 210
211 211 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
212 212 {
213 213 /** This function updates the LFR registers with the incoming sbm2 parameters.
214 214 *
215 215 * @param TC points to the TeleCommand packet that is being processed
216 216 * @param queue_id is the id of the queue which handles TM related to this execution step
217 217 *
218 218 */
219 219
220 220 int flag;
221 221 rtems_status_code status;
222 222 unsigned char sy_lfr_s2_bp_p0;
223 223 unsigned char sy_lfr_s2_bp_p1;
224 224 float aux;
225 225
226 226 flag = LFR_SUCCESSFUL;
227 227
228 228 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
229 229 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
230 230 flag = LFR_DEFAULT;
231 231 }
232 232
233 233 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
234 234 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
235 235
236 236 // sy_lfr_s2_bp_p0
237 237 if (flag == LFR_SUCCESSFUL)
238 238 {
239 239 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
240 240 {
241 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 );
242 242 flag = WRONG_APP_DATA;
243 243 }
244 244 }
245 245 // sy_lfr_s2_bp_p1
246 246 if (flag == LFR_SUCCESSFUL)
247 247 {
248 248 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
249 249 {
250 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 );
251 251 flag = WRONG_APP_DATA;
252 252 }
253 253 }
254 254 //******************************************************************
255 255 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
256 256 if (flag == LFR_SUCCESSFUL)
257 257 {
258 258 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
259 259 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
260 260 aux = ( (float ) sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0 ) - floor(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
261 261 if (aux > FLOAT_EQUAL_ZERO)
262 262 {
263 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 );
264 264 flag = LFR_DEFAULT;
265 265 }
266 266 }
267 267
268 268 // SET THE PARAMETERS
269 269 if (flag == LFR_SUCCESSFUL)
270 270 {
271 271 flag = set_sy_lfr_s2_bp_p0( TC );
272 272 flag = set_sy_lfr_s2_bp_p1( TC );
273 273 }
274 274
275 275 return flag;
276 276 }
277 277
278 278 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
279 279 {
280 280 /** This function updates the LFR registers with the incoming sbm2 parameters.
281 281 *
282 282 * @param TC points to the TeleCommand packet that is being processed
283 283 * @param queue_id is the id of the queue which handles TM related to this execution step
284 284 *
285 285 */
286 286
287 287 int flag;
288 288
289 289 flag = LFR_DEFAULT;
290 290
291 291 flag = set_sy_lfr_kcoeff( TC, queue_id );
292 292
293 293 return flag;
294 294 }
295 295
296 296 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
297 297 {
298 298 /** This function updates the LFR registers with the incoming sbm2 parameters.
299 299 *
300 300 * @param TC points to the TeleCommand packet that is being processed
301 301 * @param queue_id is the id of the queue which handles TM related to this execution step
302 302 *
303 303 */
304 304
305 305 int flag;
306 306
307 307 flag = LFR_DEFAULT;
308 308
309 309 flag = set_sy_lfr_fbins( TC );
310 310
311 311 // once the fbins masks have been stored, they have to be merged with the masks which handle the reaction wheels frequencies filtering
312 312 merge_fbins_masks();
313 313
314 314 return flag;
315 315 }
316 316
317 317 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
318 318 {
319 319 /** This function updates the LFR registers with the incoming sbm2 parameters.
320 320 *
321 321 * @param TC points to the TeleCommand packet that is being processed
322 322 * @param queue_id is the id of the queue which handles TM related to this execution step
323 323 *
324 324 */
325 325
326 326 int flag;
327 unsigned char k;
327 328
328 329 flag = LFR_DEFAULT;
330 k = INIT_CHAR;
329 331
330 332 flag = check_sy_lfr_filter_parameters( TC, queue_id );
331 333
332 334 if (flag == LFR_SUCCESSFUL)
333 335 {
334 336 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ];
335 337 parameter_dump_packet.sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
336 338 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_0 ];
337 339 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_1 ];
338 340 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_2 ];
339 341 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_3 ];
340 342 parameter_dump_packet.sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
341 343 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_0 ];
342 344 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_1 ];
343 345 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_2 ];
344 346 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_3 ];
345 347 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_0 ];
346 348 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_1 ];
347 349 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_2 ];
348 350 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_3 ];
349 351
350 352 //****************************
351 353 // store PAS filter parameters
352 354 // sy_lfr_pas_filter_enabled
353 355 filterPar.spare_sy_lfr_pas_filter_enabled = parameter_dump_packet.spare_sy_lfr_pas_filter_enabled;
354 356 set_sy_lfr_pas_filter_enabled( parameter_dump_packet.spare_sy_lfr_pas_filter_enabled & BIT_PAS_FILTER_ENABLED );
355 357 // sy_lfr_pas_filter_modulus
356 358 filterPar.sy_lfr_pas_filter_modulus = parameter_dump_packet.sy_lfr_pas_filter_modulus;
357 359 // sy_lfr_pas_filter_tbad
358 360 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_tbad,
359 361 parameter_dump_packet.sy_lfr_pas_filter_tbad );
360 362 // sy_lfr_pas_filter_offset
361 363 filterPar.sy_lfr_pas_filter_offset = parameter_dump_packet.sy_lfr_pas_filter_offset;
362 364 // sy_lfr_pas_filter_shift
363 365 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_shift,
364 366 parameter_dump_packet.sy_lfr_pas_filter_shift );
365 367
366 368 //****************************************************
367 369 // store the parameter sy_lfr_sc_rw_delta_f as a float
368 370 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f,
369 371 parameter_dump_packet.sy_lfr_sc_rw_delta_f );
372
373 // copy rw.._k.. from the incoming TC to the local parameter_dump_packet
374 for (k = 0; k < NB_RW_K_COEFFS * NB_BYTES_PER_RW_K_COEFF; k++)
375 {
376 parameter_dump_packet.sy_lfr_rw1_k1[k] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_RW1_K1 + k ];
377 }
378
379 //***********************************************
380 // store the parameter sy_lfr_rw.._k.. as a float
381 // rw1_k
382 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k1, parameter_dump_packet.sy_lfr_rw1_k1 );
383 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k2, parameter_dump_packet.sy_lfr_rw1_k2 );
384 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k3, parameter_dump_packet.sy_lfr_rw1_k3 );
385 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k4, parameter_dump_packet.sy_lfr_rw1_k4 );
386 // rw2_k
387 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k1, parameter_dump_packet.sy_lfr_rw2_k1 );
388 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k2, parameter_dump_packet.sy_lfr_rw2_k2 );
389 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k3, parameter_dump_packet.sy_lfr_rw2_k3 );
390 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k4, parameter_dump_packet.sy_lfr_rw2_k4 );
391 // rw3_k
392 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k1, parameter_dump_packet.sy_lfr_rw3_k1 );
393 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k2, parameter_dump_packet.sy_lfr_rw3_k2 );
394 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k3, parameter_dump_packet.sy_lfr_rw3_k3 );
395 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k4, parameter_dump_packet.sy_lfr_rw3_k4 );
396 // rw4_k
397 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k1, parameter_dump_packet.sy_lfr_rw4_k1 );
398 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k2, parameter_dump_packet.sy_lfr_rw4_k2 );
399 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k3, parameter_dump_packet.sy_lfr_rw4_k3 );
400 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k4, parameter_dump_packet.sy_lfr_rw4_k4 );
401
370 402 }
371 403
372 404 return flag;
373 405 }
374 406
375 407 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
376 408 {
377 409 /** This function updates the LFR registers with the incoming sbm2 parameters.
378 410 *
379 411 * @param TC points to the TeleCommand packet that is being processed
380 412 * @param queue_id is the id of the queue which handles TM related to this execution step
381 413 *
382 414 */
383 415
384 416 unsigned int address;
385 417 rtems_status_code status;
386 418 unsigned int freq;
387 419 unsigned int bin;
388 420 unsigned int coeff;
389 421 unsigned char *kCoeffPtr;
390 422 unsigned char *kCoeffDumpPtr;
391 423
392 424 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
393 425 // F0 => 11 bins
394 426 // F1 => 13 bins
395 427 // F2 => 12 bins
396 428 // 36 bins to dump in two packets (30 bins max per packet)
397 429
398 430 //*********
399 431 // PACKET 1
400 432 // 11 F0 bins, 13 F1 bins and 6 F2 bins
401 433 kcoefficients_dump_1.destinationID = TC->sourceID;
402 434 increment_seq_counter_destination_id_dump( kcoefficients_dump_1.packetSequenceControl, TC->sourceID );
403 435 for( freq = 0;
404 436 freq < NB_BINS_COMPRESSED_SM_F0;
405 437 freq++ )
406 438 {
407 439 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1] = freq;
408 440 bin = freq;
409 441 // printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
410 442 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
411 443 {
412 444 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
413 445 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
414 446 ]; // 2 for the kcoeff_frequency
415 447 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
416 448 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
417 449 }
418 450 }
419 451 for( freq = NB_BINS_COMPRESSED_SM_F0;
420 452 freq < ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
421 453 freq++ )
422 454 {
423 455 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = freq;
424 456 bin = freq - NB_BINS_COMPRESSED_SM_F0;
425 457 // printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
426 458 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
427 459 {
428 460 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
429 461 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
430 462 ]; // 2 for the kcoeff_frequency
431 463 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
432 464 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
433 465 }
434 466 }
435 467 for( freq = ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
436 468 freq < KCOEFF_BLK_NR_PKT1 ;
437 469 freq++ )
438 470 {
439 471 kcoefficients_dump_1.kcoeff_blks[ (freq * KCOEFF_BLK_SIZE) + 1 ] = freq;
440 472 bin = freq - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
441 473 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
442 474 for ( coeff = 0; coeff <NB_K_COEFF_PER_BIN; coeff++ )
443 475 {
444 476 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
445 477 (freq * KCOEFF_BLK_SIZE) + (coeff * NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
446 478 ]; // 2 for the kcoeff_frequency
447 479 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
448 480 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
449 481 }
450 482 }
451 483 kcoefficients_dump_1.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
452 484 kcoefficients_dump_1.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
453 485 kcoefficients_dump_1.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
454 486 kcoefficients_dump_1.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
455 487 kcoefficients_dump_1.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
456 488 kcoefficients_dump_1.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
457 489 // SEND DATA
458 490 kcoefficient_node_1.status = 1;
459 491 address = (unsigned int) &kcoefficient_node_1;
460 492 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
461 493 if (status != RTEMS_SUCCESSFUL) {
462 494 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
463 495 }
464 496
465 497 //********
466 498 // PACKET 2
467 499 // 6 F2 bins
468 500 kcoefficients_dump_2.destinationID = TC->sourceID;
469 501 increment_seq_counter_destination_id_dump( kcoefficients_dump_2.packetSequenceControl, TC->sourceID );
470 502 for( freq = 0;
471 503 freq < KCOEFF_BLK_NR_PKT2;
472 504 freq++ )
473 505 {
474 506 kcoefficients_dump_2.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = KCOEFF_BLK_NR_PKT1 + freq;
475 507 bin = freq + KCOEFF_BLK_NR_PKT2;
476 508 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
477 509 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
478 510 {
479 511 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[
480 512 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ ]; // 2 for the kcoeff_frequency
481 513 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
482 514 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
483 515 }
484 516 }
485 517 kcoefficients_dump_2.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
486 518 kcoefficients_dump_2.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
487 519 kcoefficients_dump_2.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
488 520 kcoefficients_dump_2.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
489 521 kcoefficients_dump_2.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
490 522 kcoefficients_dump_2.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
491 523 // SEND DATA
492 524 kcoefficient_node_2.status = 1;
493 525 address = (unsigned int) &kcoefficient_node_2;
494 526 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
495 527 if (status != RTEMS_SUCCESSFUL) {
496 528 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
497 529 }
498 530
499 531 return status;
500 532 }
501 533
502 534 int action_dump_par( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
503 535 {
504 536 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
505 537 *
506 538 * @param queue_id is the id of the queue which handles TM related to this execution step.
507 539 *
508 540 * @return RTEMS directive status codes:
509 541 * - RTEMS_SUCCESSFUL - message sent successfully
510 542 * - RTEMS_INVALID_ID - invalid queue id
511 543 * - RTEMS_INVALID_SIZE - invalid message size
512 544 * - RTEMS_INVALID_ADDRESS - buffer is NULL
513 545 * - RTEMS_UNSATISFIED - out of message buffers
514 546 * - RTEMS_TOO_MANY - queue s limit has been reached
515 547 *
516 548 */
517 549
518 550 int status;
519 551
520 552 increment_seq_counter_destination_id_dump( parameter_dump_packet.packetSequenceControl, TC->sourceID );
521 553 parameter_dump_packet.destinationID = TC->sourceID;
522 554
523 555 // UPDATE TIME
524 556 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
525 557 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
526 558 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
527 559 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
528 560 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
529 561 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
530 562 // SEND DATA
531 563 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
532 564 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
533 565 if (status != RTEMS_SUCCESSFUL) {
534 566 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
535 567 }
536 568
537 569 return status;
538 570 }
539 571
540 572 //***********************
541 573 // NORMAL MODE PARAMETERS
542 574
543 575 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
544 576 {
545 577 unsigned char msb;
546 578 unsigned char lsb;
547 579 int flag;
548 580 float aux;
549 581 rtems_status_code status;
550 582
551 583 unsigned int sy_lfr_n_swf_l;
552 584 unsigned int sy_lfr_n_swf_p;
553 585 unsigned int sy_lfr_n_asm_p;
554 586 unsigned char sy_lfr_n_bp_p0;
555 587 unsigned char sy_lfr_n_bp_p1;
556 588 unsigned char sy_lfr_n_cwf_long_f3;
557 589
558 590 flag = LFR_SUCCESSFUL;
559 591
560 592 //***************
561 593 // get parameters
562 594 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
563 595 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
564 596 sy_lfr_n_swf_l = (msb * CONST_256) + lsb;
565 597
566 598 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
567 599 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
568 600 sy_lfr_n_swf_p = (msb * CONST_256) + lsb;
569 601
570 602 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
571 603 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
572 604 sy_lfr_n_asm_p = (msb * CONST_256) + lsb;
573 605
574 606 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
575 607
576 608 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
577 609
578 610 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
579 611
580 612 //******************
581 613 // check consistency
582 614 // sy_lfr_n_swf_l
583 615 if (sy_lfr_n_swf_l != DFLT_SY_LFR_N_SWF_L)
584 616 {
585 617 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L + DATAFIELD_OFFSET, sy_lfr_n_swf_l );
586 618 flag = WRONG_APP_DATA;
587 619 }
588 620 // sy_lfr_n_swf_p
589 621 if (flag == LFR_SUCCESSFUL)
590 622 {
591 623 if ( sy_lfr_n_swf_p < MIN_SY_LFR_N_SWF_P )
592 624 {
593 625 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P + DATAFIELD_OFFSET, sy_lfr_n_swf_p );
594 626 flag = WRONG_APP_DATA;
595 627 }
596 628 }
597 629 // sy_lfr_n_bp_p0
598 630 if (flag == LFR_SUCCESSFUL)
599 631 {
600 632 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
601 633 {
602 634 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0 + DATAFIELD_OFFSET, sy_lfr_n_bp_p0 );
603 635 flag = WRONG_APP_DATA;
604 636 }
605 637 }
606 638 // sy_lfr_n_asm_p
607 639 if (flag == LFR_SUCCESSFUL)
608 640 {
609 641 if (sy_lfr_n_asm_p == 0)
610 642 {
611 643 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 644 flag = WRONG_APP_DATA;
613 645 }
614 646 }
615 647 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
616 648 if (flag == LFR_SUCCESSFUL)
617 649 {
618 650 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
619 651 if (aux > FLOAT_EQUAL_ZERO)
620 652 {
621 653 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
622 654 flag = WRONG_APP_DATA;
623 655 }
624 656 }
625 657 // sy_lfr_n_bp_p1
626 658 if (flag == LFR_SUCCESSFUL)
627 659 {
628 660 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
629 661 {
630 662 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 663 flag = WRONG_APP_DATA;
632 664 }
633 665 }
634 666 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
635 667 if (flag == LFR_SUCCESSFUL)
636 668 {
637 669 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
638 670 if (aux > FLOAT_EQUAL_ZERO)
639 671 {
640 672 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
641 673 flag = LFR_DEFAULT;
642 674 }
643 675 }
644 676 // sy_lfr_n_cwf_long_f3
645 677
646 678 return flag;
647 679 }
648 680
649 681 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
650 682 {
651 683 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
652 684 *
653 685 * @param TC points to the TeleCommand packet that is being processed
654 686 * @param queue_id is the id of the queue which handles TM related to this execution step
655 687 *
656 688 */
657 689
658 690 int result;
659 691
660 692 result = LFR_SUCCESSFUL;
661 693
662 694 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
663 695 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
664 696
665 697 return result;
666 698 }
667 699
668 700 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
669 701 {
670 702 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
671 703 *
672 704 * @param TC points to the TeleCommand packet that is being processed
673 705 * @param queue_id is the id of the queue which handles TM related to this execution step
674 706 *
675 707 */
676 708
677 709 int result;
678 710
679 711 result = LFR_SUCCESSFUL;
680 712
681 713 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
682 714 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
683 715
684 716 return result;
685 717 }
686 718
687 719 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
688 720 {
689 721 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
690 722 *
691 723 * @param TC points to the TeleCommand packet that is being processed
692 724 * @param queue_id is the id of the queue which handles TM related to this execution step
693 725 *
694 726 */
695 727
696 728 int result;
697 729
698 730 result = LFR_SUCCESSFUL;
699 731
700 732 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
701 733 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
702 734
703 735 return result;
704 736 }
705 737
706 738 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
707 739 {
708 740 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
709 741 *
710 742 * @param TC points to the TeleCommand packet that is being processed
711 743 * @param queue_id is the id of the queue which handles TM related to this execution step
712 744 *
713 745 */
714 746
715 747 int status;
716 748
717 749 status = LFR_SUCCESSFUL;
718 750
719 751 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
720 752
721 753 return status;
722 754 }
723 755
724 756 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
725 757 {
726 758 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
727 759 *
728 760 * @param TC points to the TeleCommand packet that is being processed
729 761 * @param queue_id is the id of the queue which handles TM related to this execution step
730 762 *
731 763 */
732 764
733 765 int status;
734 766
735 767 status = LFR_SUCCESSFUL;
736 768
737 769 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
738 770
739 771 return status;
740 772 }
741 773
742 774 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
743 775 {
744 776 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
745 777 *
746 778 * @param TC points to the TeleCommand packet that is being processed
747 779 * @param queue_id is the id of the queue which handles TM related to this execution step
748 780 *
749 781 */
750 782
751 783 int status;
752 784
753 785 status = LFR_SUCCESSFUL;
754 786
755 787 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
756 788
757 789 return status;
758 790 }
759 791
760 792 //**********************
761 793 // BURST MODE PARAMETERS
762 794 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
763 795 {
764 796 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
765 797 *
766 798 * @param TC points to the TeleCommand packet that is being processed
767 799 * @param queue_id is the id of the queue which handles TM related to this execution step
768 800 *
769 801 */
770 802
771 803 int status;
772 804
773 805 status = LFR_SUCCESSFUL;
774 806
775 807 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
776 808
777 809 return status;
778 810 }
779 811
780 812 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
781 813 {
782 814 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
783 815 *
784 816 * @param TC points to the TeleCommand packet that is being processed
785 817 * @param queue_id is the id of the queue which handles TM related to this execution step
786 818 *
787 819 */
788 820
789 821 int status;
790 822
791 823 status = LFR_SUCCESSFUL;
792 824
793 825 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
794 826
795 827 return status;
796 828 }
797 829
798 830 //*********************
799 831 // SBM1 MODE PARAMETERS
800 832 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
801 833 {
802 834 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
803 835 *
804 836 * @param TC points to the TeleCommand packet that is being processed
805 837 * @param queue_id is the id of the queue which handles TM related to this execution step
806 838 *
807 839 */
808 840
809 841 int status;
810 842
811 843 status = LFR_SUCCESSFUL;
812 844
813 845 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
814 846
815 847 return status;
816 848 }
817 849
818 850 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
819 851 {
820 852 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
821 853 *
822 854 * @param TC points to the TeleCommand packet that is being processed
823 855 * @param queue_id is the id of the queue which handles TM related to this execution step
824 856 *
825 857 */
826 858
827 859 int status;
828 860
829 861 status = LFR_SUCCESSFUL;
830 862
831 863 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
832 864
833 865 return status;
834 866 }
835 867
836 868 //*********************
837 869 // SBM2 MODE PARAMETERS
838 870 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC )
839 871 {
840 872 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
841 873 *
842 874 * @param TC points to the TeleCommand packet that is being processed
843 875 * @param queue_id is the id of the queue which handles TM related to this execution step
844 876 *
845 877 */
846 878
847 879 int status;
848 880
849 881 status = LFR_SUCCESSFUL;
850 882
851 883 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
852 884
853 885 return status;
854 886 }
855 887
856 888 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
857 889 {
858 890 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
859 891 *
860 892 * @param TC points to the TeleCommand packet that is being processed
861 893 * @param queue_id is the id of the queue which handles TM related to this execution step
862 894 *
863 895 */
864 896
865 897 int status;
866 898
867 899 status = LFR_SUCCESSFUL;
868 900
869 901 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
870 902
871 903 return status;
872 904 }
873 905
874 906 //*******************
875 907 // TC_LFR_UPDATE_INFO
876 908 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
877 909 {
878 910 unsigned int status;
879 911
880 912 status = LFR_DEFAULT;
881 913
882 914 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
883 915 || (mode == LFR_MODE_BURST)
884 916 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
885 917 {
886 918 status = LFR_SUCCESSFUL;
887 919 }
888 920 else
889 921 {
890 922 status = LFR_DEFAULT;
891 923 }
892 924
893 925 return status;
894 926 }
895 927
896 928 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
897 929 {
898 930 unsigned int status;
899 931
900 932 status = LFR_DEFAULT;
901 933
902 934 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
903 935 || (mode == TDS_MODE_BURST)
904 936 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
905 937 || (mode == TDS_MODE_LFM))
906 938 {
907 939 status = LFR_SUCCESSFUL;
908 940 }
909 941 else
910 942 {
911 943 status = LFR_DEFAULT;
912 944 }
913 945
914 946 return status;
915 947 }
916 948
917 949 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
918 950 {
919 951 unsigned int status;
920 952
921 953 status = LFR_DEFAULT;
922 954
923 955 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
924 956 || (mode == THR_MODE_BURST))
925 957 {
926 958 status = LFR_SUCCESSFUL;
927 959 }
928 960 else
929 961 {
930 962 status = LFR_DEFAULT;
931 963 }
932 964
933 965 return status;
934 966 }
935 967
968 void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value )
969 {
970 unsigned char flag;
971 unsigned char flagPosInByte;
972 unsigned char newFlag;
973 unsigned char flagMask;
974
975 // if the frequency value is not a number, the flag is set to 0 and the frequency RWx_Fy is not filtered
976 if (isnan(value))
977 {
978 flag = FLAG_NAN;
979 }
980 else
981 {
982 flag = FLAG_IAN;
983 }
984
985 switch(wheel)
986 {
987 case WHEEL_1:
988 flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
989 flagMask = ~(1 << flagPosInByte);
990 newFlag = flag << flagPosInByte;
991 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
992 break;
993 case WHEEL_2:
994 flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
995 flagMask = ~(1 << flagPosInByte);
996 newFlag = flag << flagPosInByte;
997 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
998 break;
999 case WHEEL_3:
1000 flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
1001 flagMask = ~(1 << flagPosInByte);
1002 newFlag = flag << flagPosInByte;
1003 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1004 break;
1005 case WHEEL_4:
1006 flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
1007 flagMask = ~(1 << flagPosInByte);
1008 newFlag = flag << flagPosInByte;
1009 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1010 break;
1011 default:
1012 break;
1013 }
1014 }
1015
1016 void set_hk_lfr_sc_rw_f_flags( void )
1017 {
1018 // RW1
1019 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_1, rw_f.cp_rpw_sc_rw1_f1 );
1020 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_2, rw_f.cp_rpw_sc_rw1_f2 );
1021 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_3, rw_f.cp_rpw_sc_rw1_f3 );
1022 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_4, rw_f.cp_rpw_sc_rw1_f4 );
1023
1024 // RW2
1025 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_1, rw_f.cp_rpw_sc_rw2_f1 );
1026 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_2, rw_f.cp_rpw_sc_rw2_f2 );
1027 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_3, rw_f.cp_rpw_sc_rw2_f3 );
1028 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_4, rw_f.cp_rpw_sc_rw2_f4 );
1029
1030 // RW3
1031 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_1, rw_f.cp_rpw_sc_rw3_f1 );
1032 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_2, rw_f.cp_rpw_sc_rw3_f2 );
1033 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_3, rw_f.cp_rpw_sc_rw3_f3 );
1034 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_4, rw_f.cp_rpw_sc_rw3_f4 );
1035
1036 // RW4
1037 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_1, rw_f.cp_rpw_sc_rw4_f1 );
1038 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_2, rw_f.cp_rpw_sc_rw4_f2 );
1039 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_3, rw_f.cp_rpw_sc_rw4_f3 );
1040 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_4, rw_f.cp_rpw_sc_rw4_f4 );
1041 }
1042
936 1043 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC )
937 1044 {
938 1045 /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally.
939 1046 *
940 1047 * @param TC points to the TeleCommand packet that is being processed
941 1048 *
942 1049 */
943 1050
944 1051 unsigned char * bytePosPtr; // pointer to the beginning of the incoming TC packet
945 1052
946 1053 bytePosPtr = (unsigned char *) &TC->packetID;
947 1054
948 // cp_rpw_sc_rw1_f1
949 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f1,
950 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] );
951
952 // cp_rpw_sc_rw1_f2
953 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f2,
954 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] );
1055 // rw1_f
1056 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] );
1057 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] );
1058 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3 ] );
1059 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4 ] );
955 1060
956 // cp_rpw_sc_rw2_f1
957 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f1,
958 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] );
959
960 // cp_rpw_sc_rw2_f2
961 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f2,
962 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] );
1061 // rw2_f
1062 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] );
1063 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] );
1064 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3 ] );
1065 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4 ] );
963 1066
964 // cp_rpw_sc_rw3_f1
965 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f1,
966 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] );
967
968 // cp_rpw_sc_rw3_f2
969 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f2,
970 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] );
1067 // rw3_f
1068 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] );
1069 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] );
1070 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3 ] );
1071 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4 ] );
971 1072
972 // cp_rpw_sc_rw4_f1
973 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f1,
974 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] );
1073 // rw4_f
1074 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] );
1075 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] );
1076 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3 ] );
1077 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4 ] );
975 1078
976 // cp_rpw_sc_rw4_f2
977 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f2,
978 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] );
1079 // test each reaction wheel frequency value. NaN means that the frequency is not filtered
1080
979 1081 }
980 1082
981 1083 void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag )
982 1084 {
983 1085 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
984 1086 *
985 1087 * @param fbins_mask
986 1088 * @param rw_f is the reaction wheel frequency to filter
987 1089 * @param delta_f is the frequency step between the frequency bins, it depends on the frequency channel
988 1090 * @param flag [true] filtering enabled [false] filtering disabled
989 1091 *
990 1092 * @return void
991 1093 *
992 1094 */
993 1095
994 1096 float f_RW_min;
995 1097 float f_RW_MAX;
996 1098 float fi_min;
997 1099 float fi_MAX;
998 1100 float fi;
999 1101 float deltaBelow;
1000 1102 float deltaAbove;
1001 1103 int binBelow;
1002 1104 int binAbove;
1003 1105 int closestBin;
1004 1106 unsigned int whichByte;
1005 1107 int selectedByte;
1006 1108 int bin;
1007 1109 int binToRemove[NB_BINS_TO_REMOVE];
1008 1110 int k;
1009 1111
1010 1112 closestBin = 0;
1011 1113 whichByte = 0;
1012 1114 bin = 0;
1013 1115
1014 1116 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1015 1117 {
1016 1118 binToRemove[k] = -1;
1017 1119 }
1018 1120
1019 1121 // compute the frequency range to filter [ rw_f - delta_f/2; rw_f + delta_f/2 ]
1020 1122 f_RW_min = rw_f - (filterPar.sy_lfr_sc_rw_delta_f / 2.);
1021 1123 f_RW_MAX = rw_f + (filterPar.sy_lfr_sc_rw_delta_f / 2.);
1022 1124
1023 1125 // compute the index of the frequency bin immediately below rw_f
1024 1126 binBelow = (int) ( floor( ((double) rw_f) / ((double) deltaFreq)) );
1025 1127 deltaBelow = rw_f - binBelow * deltaFreq;
1026 1128
1027 1129 // compute the index of the frequency bin immediately above rw_f
1028 1130 binAbove = (int) ( ceil( ((double) rw_f) / ((double) deltaFreq)) );
1029 1131 deltaAbove = binAbove * deltaFreq - rw_f;
1030 1132
1031 1133 // search the closest bin
1032 1134 if (deltaAbove > deltaBelow)
1033 1135 {
1034 1136 closestBin = binBelow;
1035 1137 }
1036 1138 else
1037 1139 {
1038 1140 closestBin = binAbove;
1039 1141 }
1040 1142
1041 1143 // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285]
1042 1144 fi = closestBin * deltaFreq;
1043 1145 fi_min = fi - (deltaFreq * FI_INTERVAL_COEFF);
1044 1146 fi_MAX = fi + (deltaFreq * FI_INTERVAL_COEFF);
1045 1147
1046 1148 //**************************************************************************************
1047 1149 // be careful here, one shall take into account that the bin 0 IS DROPPED in the spectra
1048 1150 // thus, the index 0 in a mask corresponds to the bin 1 of the spectrum
1049 1151 //**************************************************************************************
1050 1152
1051 1153 // 1. IF [ f_RW_min, f_RW_MAX] is included in [ fi_min; fi_MAX ]
1052 1154 // => remove f_(i), f_(i-1) and f_(i+1)
1053 1155 if ( ( f_RW_min > fi_min ) && ( f_RW_MAX < fi_MAX ) )
1054 1156 {
1055 1157 binToRemove[0] = (closestBin - 1) - 1;
1056 1158 binToRemove[1] = (closestBin) - 1;
1057 1159 binToRemove[2] = (closestBin + 1) - 1;
1058 1160 }
1059 1161 // 2. ELSE
1060 1162 // => remove the two f_(i) which are around f_RW
1061 1163 else
1062 1164 {
1063 1165 binToRemove[0] = (binBelow) - 1;
1064 1166 binToRemove[1] = (binAbove) - 1;
1065 1167 binToRemove[2] = (-1);
1066 1168 }
1067 1169
1068 1170 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1069 1171 {
1070 1172 bin = binToRemove[k];
1071 1173 if ( (bin >= BIN_MIN) && (bin <= BIN_MAX) )
1072 1174 {
1073 1175 if (flag == 1)
1074 1176 {
1075 1177 whichByte = (bin >> SHIFT_3_BITS); // division by 8
1076 1178 selectedByte = ( 1 << (bin - (whichByte * BITS_PER_BYTE)) );
1077 1179 fbins_mask[BYTES_PER_MASK - 1 - whichByte] =
1078 1180 fbins_mask[BYTES_PER_MASK - 1 - whichByte] & ((unsigned char) (~selectedByte)); // bytes are ordered MSB first in the packets
1079 1181 }
1080 1182 }
1081 1183 }
1082 1184 }
1083 1185
1084 1186 void build_sy_lfr_rw_mask( unsigned int channel )
1085 1187 {
1086 1188 unsigned char local_rw_fbins_mask[BYTES_PER_MASK];
1087 1189 unsigned char *maskPtr;
1088 1190 double deltaF;
1089 1191 unsigned k;
1090 1192
1091 1193 k = 0;
1092 1194
1093 1195 maskPtr = NULL;
1094 1196 deltaF = DELTAF_F2;
1095 1197
1096 1198 switch (channel)
1097 1199 {
1098 1200 case CHANNELF0:
1099 1201 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1100 1202 deltaF = DELTAF_F0;
1101 1203 break;
1102 1204 case CHANNELF1:
1103 1205 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1104 1206 deltaF = DELTAF_F1;
1105 1207 break;
1106 1208 case CHANNELF2:
1107 1209 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1108 1210 deltaF = DELTAF_F2;
1109 1211 break;
1110 1212 default:
1111 1213 break;
1112 1214 }
1113 1215
1114 1216 for (k = 0; k < BYTES_PER_MASK; k++)
1115 1217 {
1116 1218 local_rw_fbins_mask[k] = INT8_ALL_F;
1117 1219 }
1118 1220
1119 // RW1 F1
1120 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]
1121
1122 // RW1 F2
1123 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]
1221 // RW1
1222 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_7) >> SHIFT_7_BITS ); // [1000 0000]
1223 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_6) >> SHIFT_6_BITS ); // [0100 0000]
1224 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_5) >> SHIFT_5_BITS ); // [0010 0000]
1225 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_4) >> SHIFT_4_BITS ); // [0001 0000]
1124 1226
1125 // RW2 F1
1126 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]
1127
1128 // RW2 F2
1129 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]
1227 // RW2
1228 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_3) >> SHIFT_3_BITS ); // [0000 1000]
1229 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_2) >> SHIFT_2_BITS ); // [0000 0100]
1230 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_1) >> SHIFT_1_BITS ); // [0000 0010]
1231 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_0) ); // [0000 0001]
1130 1232
1131 // RW3 F1
1132 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]
1133
1134 // RW3 F2
1135 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]
1233 // RW3
1234 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_7) >> SHIFT_7_BITS ); // [1000 0000]
1235 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_6) >> SHIFT_6_BITS ); // [0100 0000]
1236 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_5) >> SHIFT_5_BITS ); // [0010 0000]
1237 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_4) >> SHIFT_4_BITS ); // [0001 0000]
1136 1238
1137 // RW4 F1
1138 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW4_F1) >> 1 ); // [0000 0010]
1139
1140 // RW4 F2
1141 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW4_F2) ); // [0000 0001]
1239 // RW4
1240 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_3) >> SHIFT_3_BITS ); // [0000 1000]
1241 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_2) >> SHIFT_2_BITS ); // [0000 0100]
1242 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_1) >> SHIFT_1_BITS ); // [0000 0010]
1243 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_0) ); // [0000 0001]
1142 1244
1143 1245 // update the value of the fbins related to reaction wheels frequency filtering
1144 1246 if (maskPtr != NULL)
1145 1247 {
1146 1248 for (k = 0; k < BYTES_PER_MASK; k++)
1147 1249 {
1148 1250 maskPtr[k] = local_rw_fbins_mask[k];
1149 1251 }
1150 1252 }
1151 1253 }
1152 1254
1153 1255 void build_sy_lfr_rw_masks( void )
1154 1256 {
1155 1257 build_sy_lfr_rw_mask( CHANNELF0 );
1156 1258 build_sy_lfr_rw_mask( CHANNELF1 );
1157 1259 build_sy_lfr_rw_mask( CHANNELF2 );
1158 1260 }
1159 1261
1160 1262 void merge_fbins_masks( void )
1161 1263 {
1162 1264 unsigned char k;
1163 1265
1164 1266 unsigned char *fbins_f0;
1165 1267 unsigned char *fbins_f1;
1166 1268 unsigned char *fbins_f2;
1167 1269 unsigned char *rw_mask_f0;
1168 1270 unsigned char *rw_mask_f1;
1169 1271 unsigned char *rw_mask_f2;
1170 1272
1171 1273 fbins_f0 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1172 1274 fbins_f1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
1173 1275 fbins_f2 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
1174 1276 rw_mask_f0 = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1175 1277 rw_mask_f1 = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1176 1278 rw_mask_f2 = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1177 1279
1178 1280 for( k=0; k < BYTES_PER_MASK; k++ )
1179 1281 {
1180 1282 fbins_masks.merged_fbins_mask_f0[k] = fbins_f0[k] & rw_mask_f0[k];
1181 1283 fbins_masks.merged_fbins_mask_f1[k] = fbins_f1[k] & rw_mask_f1[k];
1182 1284 fbins_masks.merged_fbins_mask_f2[k] = fbins_f2[k] & rw_mask_f2[k];
1183 1285 }
1184 1286 }
1185 1287
1186 1288 //***********
1187 1289 // FBINS MASK
1188 1290
1189 1291 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
1190 1292 {
1191 1293 int status;
1192 1294 unsigned int k;
1193 1295 unsigned char *fbins_mask_dump;
1194 1296 unsigned char *fbins_mask_TC;
1195 1297
1196 1298 status = LFR_SUCCESSFUL;
1197 1299
1198 1300 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1199 1301 fbins_mask_TC = TC->dataAndCRC;
1200 1302
1201 1303 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1202 1304 {
1203 1305 fbins_mask_dump[k] = fbins_mask_TC[k];
1204 1306 }
1205 1307
1206 1308 return status;
1207 1309 }
1208 1310
1209 1311 //***************************
1210 1312 // TC_LFR_LOAD_PAS_FILTER_PAR
1211 1313
1212 1314 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
1213 1315 {
1214 1316 int flag;
1215 1317 rtems_status_code status;
1216 1318
1217 1319 unsigned char sy_lfr_pas_filter_enabled;
1218 1320 unsigned char sy_lfr_pas_filter_modulus;
1219 1321 float sy_lfr_pas_filter_tbad;
1220 1322 unsigned char sy_lfr_pas_filter_offset;
1221 1323 float sy_lfr_pas_filter_shift;
1222 1324 float sy_lfr_sc_rw_delta_f;
1223 1325 char *parPtr;
1224 1326
1225 1327 flag = LFR_SUCCESSFUL;
1226 1328 sy_lfr_pas_filter_tbad = INIT_FLOAT;
1227 1329 sy_lfr_pas_filter_shift = INIT_FLOAT;
1228 1330 sy_lfr_sc_rw_delta_f = INIT_FLOAT;
1229 1331 parPtr = NULL;
1230 1332
1231 1333 //***************
1232 1334 // get parameters
1233 1335 sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & BIT_PAS_FILTER_ENABLED; // [0000 0001]
1234 1336 sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
1235 1337 copyFloatByChar(
1236 1338 (unsigned char*) &sy_lfr_pas_filter_tbad,
1237 1339 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD ]
1238 1340 );
1239 1341 sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
1240 1342 copyFloatByChar(
1241 1343 (unsigned char*) &sy_lfr_pas_filter_shift,
1242 1344 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT ]
1243 1345 );
1244 1346 copyFloatByChar(
1245 1347 (unsigned char*) &sy_lfr_sc_rw_delta_f,
1246 1348 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F ]
1247 1349 );
1248 1350
1249 1351 //******************
1250 1352 // CHECK CONSISTENCY
1251 1353
1252 1354 //**************************
1253 1355 // sy_lfr_pas_filter_enabled
1254 1356 // nothing to check, value is 0 or 1
1255 1357
1256 1358 //**************************
1257 1359 // sy_lfr_pas_filter_modulus
1258 1360 if ( (sy_lfr_pas_filter_modulus < MIN_PAS_FILTER_MODULUS) || (sy_lfr_pas_filter_modulus > MAX_PAS_FILTER_MODULUS) )
1259 1361 {
1260 1362 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1261 1363 flag = WRONG_APP_DATA;
1262 1364 }
1263 1365
1264 1366 //***********************
1265 1367 // sy_lfr_pas_filter_tbad
1266 1368 if ( (sy_lfr_pas_filter_tbad < MIN_PAS_FILTER_TBAD) || (sy_lfr_pas_filter_tbad > MAX_PAS_FILTER_TBAD) )
1267 1369 {
1268 1370 parPtr = (char*) &sy_lfr_pas_filter_tbad;
1269 1371 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1270 1372 flag = WRONG_APP_DATA;
1271 1373 }
1272 1374
1273 1375 //*************************
1274 1376 // sy_lfr_pas_filter_offset
1275 1377 if (flag == LFR_SUCCESSFUL)
1276 1378 {
1277 1379 if ( (sy_lfr_pas_filter_offset < MIN_PAS_FILTER_OFFSET) || (sy_lfr_pas_filter_offset > MAX_PAS_FILTER_OFFSET) )
1278 1380 {
1279 1381 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET + DATAFIELD_OFFSET, sy_lfr_pas_filter_offset );
1280 1382 flag = WRONG_APP_DATA;
1281 1383 }
1282 1384 }
1283 1385
1284 1386 //************************
1285 1387 // sy_lfr_pas_filter_shift
1286 1388 if (flag == LFR_SUCCESSFUL)
1287 1389 {
1288 1390 if ( (sy_lfr_pas_filter_shift < MIN_PAS_FILTER_SHIFT) || (sy_lfr_pas_filter_shift > MAX_PAS_FILTER_SHIFT) )
1289 1391 {
1290 1392 parPtr = (char*) &sy_lfr_pas_filter_shift;
1291 1393 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1292 1394 flag = WRONG_APP_DATA;
1293 1395 }
1294 1396 }
1295 1397
1296 1398 //*************************************
1297 1399 // check global coherency of the values
1298 1400 if (flag == LFR_SUCCESSFUL)
1299 1401 {
1300 1402 if ( (sy_lfr_pas_filter_tbad + sy_lfr_pas_filter_offset + sy_lfr_pas_filter_shift) > sy_lfr_pas_filter_modulus )
1301 1403 {
1302 1404 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1303 1405 flag = WRONG_APP_DATA;
1304 1406 }
1305 1407 }
1306 1408
1307 1409 //*********************
1308 1410 // sy_lfr_sc_rw_delta_f
1309 1411 // nothing to check, no default value in the ICD
1310 1412
1311 1413 return flag;
1312 1414 }
1313 1415
1314 1416 //**************
1315 1417 // KCOEFFICIENTS
1316 1418 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC,rtems_id queue_id )
1317 1419 {
1318 1420 unsigned int kcoeff;
1319 1421 unsigned short sy_lfr_kcoeff_frequency;
1320 1422 unsigned short bin;
1321 1423 unsigned short *freqPtr;
1322 1424 float *kcoeffPtr_norm;
1323 1425 float *kcoeffPtr_sbm;
1324 1426 int status;
1325 1427 unsigned char *kcoeffLoadPtr;
1326 1428 unsigned char *kcoeffNormPtr;
1327 1429 unsigned char *kcoeffSbmPtr_a;
1328 1430 unsigned char *kcoeffSbmPtr_b;
1329 1431
1330 1432 status = LFR_SUCCESSFUL;
1331 1433
1332 1434 kcoeffPtr_norm = NULL;
1333 1435 kcoeffPtr_sbm = NULL;
1334 1436 bin = 0;
1335 1437
1336 1438 freqPtr = (unsigned short *) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY];
1337 1439 sy_lfr_kcoeff_frequency = *freqPtr;
1338 1440
1339 1441 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
1340 1442 {
1341 1443 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
1342 1444 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + DATAFIELD_OFFSET + 1,
1343 1445 TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 1] ); // +1 to get the LSB instead of the MSB
1344 1446 status = LFR_DEFAULT;
1345 1447 }
1346 1448 else
1347 1449 {
1348 1450 if ( ( sy_lfr_kcoeff_frequency >= 0 )
1349 1451 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
1350 1452 {
1351 1453 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
1352 1454 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
1353 1455 bin = sy_lfr_kcoeff_frequency;
1354 1456 }
1355 1457 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
1356 1458 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
1357 1459 {
1358 1460 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
1359 1461 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
1360 1462 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
1361 1463 }
1362 1464 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
1363 1465 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
1364 1466 {
1365 1467 kcoeffPtr_norm = k_coeff_intercalib_f2;
1366 1468 kcoeffPtr_sbm = NULL;
1367 1469 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
1368 1470 }
1369 1471 }
1370 1472
1371 1473 if (kcoeffPtr_norm != NULL ) // update K coefficient for NORMAL data products
1372 1474 {
1373 1475 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1374 1476 {
1375 1477 // destination
1376 1478 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + kcoeff ];
1377 1479 // source
1378 1480 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1379 1481 // copy source to destination
1380 1482 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
1381 1483 }
1382 1484 }
1383 1485
1384 1486 if (kcoeffPtr_sbm != NULL ) // update K coefficient for SBM data products
1385 1487 {
1386 1488 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1387 1489 {
1388 1490 // destination
1389 1491 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ];
1390 1492 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ (((bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_KCOEFF_PER_NORM_KCOEFF) + 1 ];
1391 1493 // source
1392 1494 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1393 1495 // copy source to destination
1394 1496 copyFloatByChar( kcoeffSbmPtr_a, kcoeffLoadPtr );
1395 1497 copyFloatByChar( kcoeffSbmPtr_b, kcoeffLoadPtr );
1396 1498 }
1397 1499 }
1398 1500
1399 1501 // print_k_coeff();
1400 1502
1401 1503 return status;
1402 1504 }
1403 1505
1404 1506 void copyFloatByChar( unsigned char *destination, unsigned char *source )
1405 1507 {
1406 1508 destination[BYTE_0] = source[BYTE_0];
1407 1509 destination[BYTE_1] = source[BYTE_1];
1408 1510 destination[BYTE_2] = source[BYTE_2];
1409 1511 destination[BYTE_3] = source[BYTE_3];
1410 1512 }
1411 1513
1412 1514 void floatToChar( float value, unsigned char* ptr)
1413 1515 {
1414 1516 unsigned char* valuePtr;
1415 1517
1416 1518 valuePtr = (unsigned char*) &value;
1417 1519 ptr[BYTE_0] = valuePtr[BYTE_0];
1418 1520 ptr[BYTE_1] = valuePtr[BYTE_1];
1419 1521 ptr[BYTE_2] = valuePtr[BYTE_2];
1420 1522 ptr[BYTE_3] = valuePtr[BYTE_3];
1421 1523 }
1422 1524
1423 1525 //**********
1424 1526 // init dump
1425 1527
1426 1528 void init_parameter_dump( void )
1427 1529 {
1428 1530 /** This function initialize the parameter_dump_packet global variable with default values.
1429 1531 *
1430 1532 */
1431 1533
1432 1534 unsigned int k;
1433 1535
1434 1536 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
1435 1537 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
1436 1538 parameter_dump_packet.reserved = CCSDS_RESERVED;
1437 1539 parameter_dump_packet.userApplication = CCSDS_USER_APP;
1438 1540 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1439 1541 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1440 1542 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1441 1543 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1442 1544 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> SHIFT_1_BYTE);
1443 1545 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
1444 1546 // DATA FIELD HEADER
1445 1547 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1446 1548 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
1447 1549 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
1448 1550 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
1449 1551 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
1450 1552 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
1451 1553 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
1452 1554 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
1453 1555 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
1454 1556 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
1455 1557 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
1456 1558
1457 1559 //******************
1458 1560 // COMMON PARAMETERS
1459 1561 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
1460 1562 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
1461 1563
1462 1564 //******************
1463 1565 // NORMAL PARAMETERS
1464 1566 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> SHIFT_1_BYTE);
1465 1567 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1466 1568 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> SHIFT_1_BYTE);
1467 1569 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1468 1570 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> SHIFT_1_BYTE);
1469 1571 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1470 1572 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1471 1573 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1472 1574 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1473 1575
1474 1576 //*****************
1475 1577 // BURST PARAMETERS
1476 1578 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1477 1579 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1478 1580
1479 1581 //****************
1480 1582 // SBM1 PARAMETERS
1481 1583 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
1482 1584 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1483 1585
1484 1586 //****************
1485 1587 // SBM2 PARAMETERS
1486 1588 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1487 1589 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1488 1590
1489 1591 //************
1490 1592 // FBINS MASKS
1491 1593 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1492 1594 {
1493 1595 parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = INT8_ALL_F;
1494 1596 }
1495 1597
1496 1598 // PAS FILTER PARAMETERS
1497 1599 parameter_dump_packet.pa_rpw_spare8_2 = INIT_CHAR;
1498 1600 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = INIT_CHAR;
1499 1601 parameter_dump_packet.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
1500 1602 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_TBAD, parameter_dump_packet.sy_lfr_pas_filter_tbad );
1501 1603 parameter_dump_packet.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
1502 1604 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift );
1503 1605 floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f );
1504 1606
1607 // RW1_K
1608 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw1_k1);
1609 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw1_k2);
1610 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw1_k3);
1611 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw1_k4);
1612 // RW2_K
1613 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw2_k1);
1614 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw2_k2);
1615 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw2_k3);
1616 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw2_k4);
1617 // RW3_K
1618 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw3_k1);
1619 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw3_k2);
1620 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw3_k3);
1621 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw3_k4);
1622 // RW4_K
1623 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw4_k1);
1624 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw4_k2);
1625 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw4_k3);
1626 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw4_k4);
1627
1505 1628 // LFR_RW_MASK
1506 1629 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1507 1630 {
1508 1631 parameter_dump_packet.sy_lfr_rw_mask_f0_word1[k] = INT8_ALL_F;
1509 1632 }
1510 1633
1511 1634 // once the reaction wheels masks have been initialized, they have to be merged with the fbins masks
1512 1635 merge_fbins_masks();
1513 1636 }
1514 1637
1515 1638 void init_kcoefficients_dump( void )
1516 1639 {
1517 1640 init_kcoefficients_dump_packet( &kcoefficients_dump_1, PKTNR_1, KCOEFF_BLK_NR_PKT1 );
1518 1641 init_kcoefficients_dump_packet( &kcoefficients_dump_2, PKTNR_2, KCOEFF_BLK_NR_PKT2 );
1519 1642
1520 1643 kcoefficient_node_1.previous = NULL;
1521 1644 kcoefficient_node_1.next = NULL;
1522 1645 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1523 1646 kcoefficient_node_1.coarseTime = INIT_CHAR;
1524 1647 kcoefficient_node_1.fineTime = INIT_CHAR;
1525 1648 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1526 1649 kcoefficient_node_1.status = INIT_CHAR;
1527 1650
1528 1651 kcoefficient_node_2.previous = NULL;
1529 1652 kcoefficient_node_2.next = NULL;
1530 1653 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1531 1654 kcoefficient_node_2.coarseTime = INIT_CHAR;
1532 1655 kcoefficient_node_2.fineTime = INIT_CHAR;
1533 1656 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1534 1657 kcoefficient_node_2.status = INIT_CHAR;
1535 1658 }
1536 1659
1537 1660 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1538 1661 {
1539 1662 unsigned int k;
1540 1663 unsigned int packetLength;
1541 1664
1542 1665 packetLength =
1543 1666 ((blk_nr * KCOEFF_BLK_SIZE) + BYTE_POS_KCOEFFICIENTS_PARAMETES) - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1544 1667
1545 1668 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1546 1669 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1547 1670 kcoefficients_dump->reserved = CCSDS_RESERVED;
1548 1671 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1549 1672 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1550 1673 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1551 1674 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1552 1675 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1553 1676 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
1554 1677 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1555 1678 // DATA FIELD HEADER
1556 1679 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1557 1680 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1558 1681 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1559 1682 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1560 1683 kcoefficients_dump->time[BYTE_0] = INIT_CHAR;
1561 1684 kcoefficients_dump->time[BYTE_1] = INIT_CHAR;
1562 1685 kcoefficients_dump->time[BYTE_2] = INIT_CHAR;
1563 1686 kcoefficients_dump->time[BYTE_3] = INIT_CHAR;
1564 1687 kcoefficients_dump->time[BYTE_4] = INIT_CHAR;
1565 1688 kcoefficients_dump->time[BYTE_5] = INIT_CHAR;
1566 1689 kcoefficients_dump->sid = SID_K_DUMP;
1567 1690
1568 1691 kcoefficients_dump->pkt_cnt = KCOEFF_PKTCNT;
1569 1692 kcoefficients_dump->pkt_nr = PKTNR_1;
1570 1693 kcoefficients_dump->blk_nr = blk_nr;
1571 1694
1572 1695 //******************
1573 1696 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1574 1697 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1575 1698 for (k=0; k<(KCOEFF_BLK_NR_PKT1 * KCOEFF_BLK_SIZE); k++)
1576 1699 {
1577 1700 kcoefficients_dump->kcoeff_blks[k] = INIT_CHAR;
1578 1701 }
1579 1702 }
1580 1703
1581 1704 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id )
1582 1705 {
1583 1706 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
1584 1707 *
1585 1708 * @param packet_sequence_control points to the packet sequence control which will be incremented
1586 1709 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
1587 1710 *
1588 1711 * If the destination ID is not known, a dedicated counter is incremented.
1589 1712 *
1590 1713 */
1591 1714
1592 1715 unsigned short sequence_cnt;
1593 1716 unsigned short segmentation_grouping_flag;
1594 1717 unsigned short new_packet_sequence_control;
1595 1718 unsigned char i;
1596 1719
1597 1720 switch (destination_id)
1598 1721 {
1599 1722 case SID_TC_GROUND:
1600 1723 i = GROUND;
1601 1724 break;
1602 1725 case SID_TC_MISSION_TIMELINE:
1603 1726 i = MISSION_TIMELINE;
1604 1727 break;
1605 1728 case SID_TC_TC_SEQUENCES:
1606 1729 i = TC_SEQUENCES;
1607 1730 break;
1608 1731 case SID_TC_RECOVERY_ACTION_CMD:
1609 1732 i = RECOVERY_ACTION_CMD;
1610 1733 break;
1611 1734 case SID_TC_BACKUP_MISSION_TIMELINE:
1612 1735 i = BACKUP_MISSION_TIMELINE;
1613 1736 break;
1614 1737 case SID_TC_DIRECT_CMD:
1615 1738 i = DIRECT_CMD;
1616 1739 break;
1617 1740 case SID_TC_SPARE_GRD_SRC1:
1618 1741 i = SPARE_GRD_SRC1;
1619 1742 break;
1620 1743 case SID_TC_SPARE_GRD_SRC2:
1621 1744 i = SPARE_GRD_SRC2;
1622 1745 break;
1623 1746 case SID_TC_OBCP:
1624 1747 i = OBCP;
1625 1748 break;
1626 1749 case SID_TC_SYSTEM_CONTROL:
1627 1750 i = SYSTEM_CONTROL;
1628 1751 break;
1629 1752 case SID_TC_AOCS:
1630 1753 i = AOCS;
1631 1754 break;
1632 1755 case SID_TC_RPW_INTERNAL:
1633 1756 i = RPW_INTERNAL;
1634 1757 break;
1635 1758 default:
1636 1759 i = GROUND;
1637 1760 break;
1638 1761 }
1639 1762
1640 1763 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
1641 1764 sequence_cnt = sequenceCounters_TM_DUMP[ i ] & SEQ_CNT_MASK;
1642 1765
1643 1766 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
1644 1767
1645 1768 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> SHIFT_1_BYTE);
1646 1769 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1647 1770
1648 1771 // increment the sequence counter
1649 1772 if ( sequenceCounters_TM_DUMP[ i ] < SEQ_CNT_MAX )
1650 1773 {
1651 1774 sequenceCounters_TM_DUMP[ i ] = sequenceCounters_TM_DUMP[ i ] + 1;
1652 1775 }
1653 1776 else
1654 1777 {
1655 1778 sequenceCounters_TM_DUMP[ i ] = 0;
1656 1779 }
1657 1780 }
General Comments 0
You need to be logged in to leave comments. Login now