##// END OF EJS Templates
reaction wheels filtering implemented
paul -
r286:1a92544dda46 R3_plus draft
parent child
Show More
@@ -1,2 +1,2
1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
2 d867fbc1a62231ed43bdda3e91b3f45f45806c3d header/lfr_common_headers
2 dad8371a5549f3395f975fddc33098b05fd829f4 header/lfr_common_headers
@@ -30,6 +30,7 extern float cp_rpw_sc_rw3_f1;
30 extern float cp_rpw_sc_rw3_f2;
30 extern float cp_rpw_sc_rw3_f2;
31 extern float cp_rpw_sc_rw4_f1;
31 extern float cp_rpw_sc_rw4_f1;
32 extern float cp_rpw_sc_rw4_f2;
32 extern float cp_rpw_sc_rw4_f2;
33 extern float sy_lfr_sc_rw_delta_f;
33
34
34 // RTEMS TASKS
35 // RTEMS TASKS
35 rtems_task Init( rtems_task_argument argument);
36 rtems_task Init( rtems_task_argument argument);
@@ -59,7 +59,7 typedef struct {
59
59
60 // PDB >= 0.1.28, 0x80000f54
60 // PDB >= 0.1.28, 0x80000f54
61 typedef struct{
61 typedef struct{
62 int data_shaping; // 0x00 00 *** R1 R0 SP1 SP0 BW
62 int data_shaping; // 0x00 00 *** R2 R1 R0 SP1 SP0 BW
63 int run_burst_enable; // 0x04 01 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
63 int run_burst_enable; // 0x04 01 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
64 int addr_data_f0_0; // 0x08
64 int addr_data_f0_0; // 0x08
65 int addr_data_f0_1; // 0x0c
65 int addr_data_f0_1; // 0x0c
@@ -94,6 +94,9 extern Packet_TM_LFR_PARAMETER_DUMP_t pa
94 extern unsigned char rw_fbins_mask_f0[16];
94 extern unsigned char rw_fbins_mask_f0[16];
95 extern unsigned char rw_fbins_mask_f1[16];
95 extern unsigned char rw_fbins_mask_f1[16];
96 extern unsigned char rw_fbins_mask_f2[16];
96 extern unsigned char rw_fbins_mask_f2[16];
97 extern unsigned char merged_fbins_mask_f0[16];
98 extern unsigned char merged_fbins_mask_f1[16];
99 extern unsigned char merged_fbins_mask_f2[16];
97
100
98 // registers
101 // registers
99 extern time_management_regs_t *time_management_regs;
102 extern time_management_regs_t *time_management_regs;
@@ -20,9 +20,7 extern float k_coeff_intercalib_f0_sbm[
20 extern float k_coeff_intercalib_f1_norm[ ];
20 extern float k_coeff_intercalib_f1_norm[ ];
21 extern float k_coeff_intercalib_f1_sbm[ ];
21 extern float k_coeff_intercalib_f1_sbm[ ];
22 extern float k_coeff_intercalib_f2[ ];
22 extern float k_coeff_intercalib_f2[ ];
23 extern unsigned char rw_fbins_mask_f0[16];
23 extern fbins_masks_t fbins_masks;
24 extern unsigned char rw_fbins_mask_f1[16];
25 extern unsigned char rw_fbins_mask_f2[16];
26
24
27 int action_load_common_par( ccsdsTelecommandPacket_t *TC );
25 int action_load_common_par( ccsdsTelecommandPacket_t *TC );
28 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
26 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
@@ -61,8 +59,10 unsigned int check_update_info_hk_lfr_mo
61 unsigned int check_update_info_hk_tds_mode( unsigned char mode );
59 unsigned int check_update_info_hk_tds_mode( unsigned char mode );
62 unsigned int check_update_info_hk_thr_mode( unsigned char mode );
60 unsigned int check_update_info_hk_thr_mode( unsigned char mode );
63 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC );
61 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC );
64 void build_rw_fbins_mask( unsigned int channel );
62 void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag );
65 void build_rw_fbins_masks();
63 void build_sy_lfr_rw_mask( unsigned int channel );
64 void build_sy_lfr_rw_masks();
65 void merge_fbins_masks( void );
66
66
67 // FBINS_MASK
67 // FBINS_MASK
68 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC );
68 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC );
@@ -90,6 +90,6 float cp_rpw_sc_rw3_f1;
90 float cp_rpw_sc_rw3_f2;
90 float cp_rpw_sc_rw3_f2;
91 float cp_rpw_sc_rw4_f1;
91 float cp_rpw_sc_rw4_f1;
92 float cp_rpw_sc_rw4_f2;
92 float cp_rpw_sc_rw4_f2;
93 unsigned char rw_fbins_mask_f0[16];
93 float sy_lfr_sc_rw_delta_f;
94 unsigned char rw_fbins_mask_f1[16];
94
95 unsigned char rw_fbins_mask_f2[16];
95 fbins_masks_t fbins_masks;
@@ -169,6 +169,7 rtems_task Init( rtems_task_argument ign
169 cp_rpw_sc_rw3_f2 = 0.0;
169 cp_rpw_sc_rw3_f2 = 0.0;
170 cp_rpw_sc_rw4_f1 = 0.0;
170 cp_rpw_sc_rw4_f1 = 0.0;
171 cp_rpw_sc_rw4_f2 = 0.0;
171 cp_rpw_sc_rw4_f2 = 0.0;
172 sy_lfr_sc_rw_delta_f = 0.0;
172 update_last_valid_transition_date( DEFAULT_LAST_VALID_TRANSITION_DATE );
173 update_last_valid_transition_date( DEFAULT_LAST_VALID_TRANSITION_DATE );
173
174
174 // waveform picker initialization
175 // waveform picker initialization
@@ -684,13 +684,13 int getFBinMask( int index, unsigned cha
684 switch(channel)
684 switch(channel)
685 {
685 {
686 case 0:
686 case 0:
687 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
687 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0;
688 break;
688 break;
689 case 1:
689 case 1:
690 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
690 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1;
691 break;
691 break;
692 case 2:
692 case 2:
693 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
693 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2;
694 break;
694 break;
695 default:
695 default:
696 PRINTF("ERR *** in getFBinMask, wrong frequency channel")
696 PRINTF("ERR *** in getFBinMask, wrong frequency channel")
@@ -297,7 +297,7 int action_update_info(ccsdsTelecommandP
297 // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets)
297 // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets)
298 cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ];
298 cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ];
299 getReactionWheelsFrequencies( TC );
299 getReactionWheelsFrequencies( TC );
300 build_rw_fbins_masks();
300 build_sy_lfr_rw_masks();
301
301
302 result = status;
302 result = status;
303
303
@@ -342,6 +342,10 int action_load_filter_par(ccsdsTelecomm
342 parameter_dump_packet.sy_lfr_sc_rw_delta_f[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 1 ];
342 parameter_dump_packet.sy_lfr_sc_rw_delta_f[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 1 ];
343 parameter_dump_packet.sy_lfr_sc_rw_delta_f[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 2 ];
343 parameter_dump_packet.sy_lfr_sc_rw_delta_f[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 2 ];
344 parameter_dump_packet.sy_lfr_sc_rw_delta_f[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 3 ];
344 parameter_dump_packet.sy_lfr_sc_rw_delta_f[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 3 ];
345
346 // store the parameter sy_lfr_sc_rw_delta_f as a float
347 copyFloatByChar( (unsigned char*) &sy_lfr_sc_rw_delta_f,
348 (unsigned char*) &parameter_dump_packet.sy_lfr_sc_rw_delta_f[0] );
345 }
349 }
346
350
347 return flag;
351 return flag;
@@ -902,7 +906,6 void getReactionWheelsFrequencies( ccsds
902 */
906 */
903
907
904 unsigned char * bytePosPtr; // pointer to the beginning of the incoming TC packet
908 unsigned char * bytePosPtr; // pointer to the beginning of the incoming TC packet
905 unsigned char * floatPtr; // pointer to the Most Significant Byte of the considered float
906
909
907 bytePosPtr = (unsigned char *) &TC->packetID;
910 bytePosPtr = (unsigned char *) &TC->packetID;
908
911
@@ -939,54 +942,78 void getReactionWheelsFrequencies( ccsds
939 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] );
942 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] );
940 }
943 }
941
944
942 void setFBinMask( unsigned char *fbins_mask, float freq, unsigned char deltaFreq, unsigned char flag )
945 void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag )
943 {
946 {
944 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
947 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
945 *
948 *
946 * @param fbins_mask
949 * @param fbins_mask
947 * @param freq
950 * @param rw_f is the reaction wheel frequency to filter
948 * @param deltaFreq
951 * @param delta_f is the frequency step between the frequency bins, it depends on the frequency channel
949 * @param flag [true] filtering enabled [false] filtering disabled
952 * @param flag [true] filtering enabled [false] filtering disabled
950 *
953 *
951 * @return void
954 * @return void
952 *
955 *
953 */
956 */
954
957
955 unsigned int fBelow;
958 float fmin;
959 float fMAX;
960 int binBelow;
961 int binAbove;
962 unsigned int whichByte;
963 unsigned char selectedByte;
964 int bin;
965
966 whichByte = 0;
967 bin = 0;
956
968
957 // compute the index of the frequency immediately below the reaction wheel frequency
969 // compute the frequency range to filter [ rw_f - delta_f/2; rw_f + delta_f/2 ]
958 fBelow = (unsigned int) ( floor( ((double) cp_rpw_sc_rw1_f1) / ((double) deltaFreq)) );
970 fmin = rw_f - sy_lfr_sc_rw_delta_f / 2.;
971 fMAX = rw_f + sy_lfr_sc_rw_delta_f / 2.;
972
973 // compute the index of the frequency bin immediately below fmin
974 binBelow = (int) ( floor( ((double) fmin) / ((double) deltaFreq)) );
959
975
960 if (fBelow < 127) // if fbelow is greater than 127 or equal to 127, this means that the reaction wheel frequency is outside the frequency range
976 // compute the index of the frequency bin immediately above fMAX
977 binAbove = (int) ( ceil( ((double) fMAX) / ((double) deltaFreq)) );
978
979 for (bin = binBelow; bin <= binAbove; bin++)
980 {
981 if ( (bin >= 0) && (bin<=127) )
961 {
982 {
962 if (flag == 1)
983 if (flag == 1)
963 {
984 {
964 // rw_fbins_mask[k] = (1 << fBelow) | (1 << fAbove);
985 whichByte = bin >> 3; // division by 8
986 selectedByte = (unsigned char) ( 1 << (bin - (whichByte * 8)) );
987 fbins_mask[whichByte] = fbins_mask[whichByte] & (~selectedByte);
988 }
965 }
989 }
966 }
990 }
967 }
991 }
968
992
969 void build_rw_fbins_mask( unsigned int channel )
993 void build_sy_lfr_rw_mask( unsigned int channel )
970 {
994 {
971 unsigned char rw_fbins_mask[16];
995 unsigned char local_rw_fbins_mask[16];
972 unsigned char *maskPtr;
996 unsigned char *maskPtr;
973 double deltaF;
997 double deltaF;
974 unsigned k;
998 unsigned k;
975
999
976 k = 0;
1000 k = 0;
977
1001
1002 maskPtr = NULL;
1003 deltaF = 1.;
1004
978 switch (channel)
1005 switch (channel)
979 {
1006 {
980 case 0:
1007 case 0:
981 maskPtr = rw_fbins_mask_f0;
1008 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
982 deltaF = 96.;
1009 deltaF = 96.;
983 break;
1010 break;
984 case 1:
1011 case 1:
985 maskPtr = rw_fbins_mask_f1;
1012 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
986 deltaF = 16.;
1013 deltaF = 16.;
987 break;
1014 break;
988 case 2:
1015 case 2:
989 maskPtr = rw_fbins_mask_f2;
1016 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
990 deltaF = 1.;
1017 deltaF = 1.;
991 break;
1018 break;
992 default:
1019 default:
@@ -995,39 +1022,76 void build_rw_fbins_mask( unsigned int c
995
1022
996 for (k = 0; k < 16; k++)
1023 for (k = 0; k < 16; k++)
997 {
1024 {
998 rw_fbins_mask[k] = 0x00;
1025 local_rw_fbins_mask[k] = 0xff;
999 }
1026 }
1000
1027
1001 // RW1 F1
1028 // RW1 F1
1002 setFBinMask( rw_fbins_mask, fBelow );
1029 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x80) >> 7 ); // [1000 0000]
1003
1030
1004 // RW1 F2
1031 // RW1 F2
1032 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x40) >> 6 ); // [0100 0000]
1005
1033
1006 // RW2 F1
1034 // RW2 F1
1035 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x20) >> 5 ); // [0010 0000]
1007
1036
1008 // RW2 F2
1037 // RW2 F2
1038 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x10) >> 4 ); // [0001 0000]
1009
1039
1010 // RW3 F1
1040 // RW3 F1
1041 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x08) >> 3 ); // [0000 1000]
1011
1042
1012 // RW3 F2
1043 // RW3 F2
1044 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw_f_flags & 0x04) >> 2 ); // [0000 0100]
1013
1045
1014 // RW4 F1
1046 // RW4 F1
1047 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x02) >> 1 ); // [0000 0010]
1015
1048
1016 // RW4 F2
1049 // RW4 F2
1017
1050 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw_f_flags & 0x01) ); // [0000 0001]
1018
1051
1019 // update the value of the fbins related to reaction wheels frequency filtering
1052 // update the value of the fbins related to reaction wheels frequency filtering
1053 if (maskPtr != NULL)
1054 {
1020 for (k = 0; k < 16; k++)
1055 for (k = 0; k < 16; k++)
1021 {
1056 {
1022 maskPtr[k] = rw_fbins_mask[k];
1057 maskPtr[k] = local_rw_fbins_mask[k];
1058 }
1023 }
1059 }
1024 }
1060 }
1025
1061
1026 void build_rw_fbins_masks()
1062 void build_sy_lfr_rw_masks( void )
1063 {
1064 build_sy_lfr_rw_mask( 0 );
1065 build_sy_lfr_rw_mask( 1 );
1066 build_sy_lfr_rw_mask( 2 );
1067
1068 merge_fbins_masks();
1069 }
1070
1071 void merge_fbins_masks( void )
1027 {
1072 {
1028 build_rw_fbins_mask( 0 );
1073 unsigned char k;
1029 build_rw_fbins_mask( 1 );
1074
1030 build_rw_fbins_mask( 2 );
1075 unsigned char *fbins_f0;
1076 unsigned char *fbins_f1;
1077 unsigned char *fbins_f2;
1078 unsigned char *rw_mask_f0;
1079 unsigned char *rw_mask_f1;
1080 unsigned char *rw_mask_f2;
1081
1082 fbins_f0 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1083 fbins_f1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
1084 fbins_f2 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
1085 rw_mask_f0 = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1086 rw_mask_f1 = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1087 rw_mask_f2 = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1088
1089 for( k=0; k < 16; k++ )
1090 {
1091 fbins_masks.merged_fbins_mask_f0[k] = fbins_f0[k] & rw_mask_f0[k];
1092 fbins_masks.merged_fbins_mask_f1[k] = fbins_f1[k] & rw_mask_f1[k];
1093 fbins_masks.merged_fbins_mask_f2[k] = fbins_f2[k] & rw_mask_f2[k];
1094 }
1031 }
1095 }
1032
1096
1033 //***********
1097 //***********
@@ -1049,12 +1113,6 int set_sy_lfr_fbins( ccsdsTelecommandPa
1049 {
1113 {
1050 fbins_mask_dump[k] = fbins_mask_TC[k];
1114 fbins_mask_dump[k] = fbins_mask_TC[k];
1051 }
1115 }
1052 for (k=0; k < NB_FBINS_MASKS; k++)
1053 {
1054 unsigned char *auxPtr;
1055 auxPtr = &parameter_dump_packet.sy_lfr_fbins_f0_word1[k*NB_BYTES_PER_FBINS_MASK];
1056 }
1057
1058
1116
1059 return status;
1117 return status;
1060 }
1118 }
@@ -1086,17 +1144,17 int check_sy_lfr_filter_parameters( ccsd
1086 sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & 0x01; // [0000 0001]
1144 sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & 0x01; // [0000 0001]
1087 sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
1145 sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
1088 copyFloatByChar(
1146 copyFloatByChar(
1089 (char*) &sy_lfr_pas_filter_tbad,
1147 (unsigned char*) &sy_lfr_pas_filter_tbad,
1090 (char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD ]
1148 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD ]
1091 );
1149 );
1092 sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
1150 sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
1093 copyFloatByChar(
1151 copyFloatByChar(
1094 (char*) &sy_lfr_pas_filter_shift,
1152 (unsigned char*) &sy_lfr_pas_filter_shift,
1095 (char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT ]
1153 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT ]
1096 );
1154 );
1097 copyFloatByChar(
1155 copyFloatByChar(
1098 (char*) &sy_lfr_sc_rw_delta_f,
1156 (unsigned char*) &sy_lfr_sc_rw_delta_f,
1099 (char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F ]
1157 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F ]
1100 );
1158 );
1101
1159
1102 //******************
1160 //******************
General Comments 0
You need to be logged in to leave comments. Login now