@@ -1,2 +1,2 | |||
|
1 | 1 | 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters |
|
2 |
d |
|
|
2 | dad8371a5549f3395f975fddc33098b05fd829f4 header/lfr_common_headers |
@@ -30,6 +30,7 extern float cp_rpw_sc_rw3_f1; | |||
|
30 | 30 | extern float cp_rpw_sc_rw3_f2; |
|
31 | 31 | extern float cp_rpw_sc_rw4_f1; |
|
32 | 32 | extern float cp_rpw_sc_rw4_f2; |
|
33 | extern float sy_lfr_sc_rw_delta_f; | |
|
33 | 34 | |
|
34 | 35 | // RTEMS TASKS |
|
35 | 36 | rtems_task Init( rtems_task_argument argument); |
@@ -59,7 +59,7 typedef struct { | |||
|
59 | 59 | |
|
60 | 60 | // PDB >= 0.1.28, 0x80000f54 |
|
61 | 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 | 63 | int run_burst_enable; // 0x04 01 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ] |
|
64 | 64 | int addr_data_f0_0; // 0x08 |
|
65 | 65 | int addr_data_f0_1; // 0x0c |
@@ -94,6 +94,9 extern Packet_TM_LFR_PARAMETER_DUMP_t pa | |||
|
94 | 94 | extern unsigned char rw_fbins_mask_f0[16]; |
|
95 | 95 | extern unsigned char rw_fbins_mask_f1[16]; |
|
96 | 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 | 101 | // registers |
|
99 | 102 | extern time_management_regs_t *time_management_regs; |
@@ -20,9 +20,7 extern float k_coeff_intercalib_f0_sbm[ | |||
|
20 | 20 | extern float k_coeff_intercalib_f1_norm[ ]; |
|
21 | 21 | extern float k_coeff_intercalib_f1_sbm[ ]; |
|
22 | 22 | extern float k_coeff_intercalib_f2[ ]; |
|
23 | extern unsigned char rw_fbins_mask_f0[16]; | |
|
24 | extern unsigned char rw_fbins_mask_f1[16]; | |
|
25 | extern unsigned char rw_fbins_mask_f2[16]; | |
|
23 | extern fbins_masks_t fbins_masks; | |
|
26 | 24 | |
|
27 | 25 | int action_load_common_par( ccsdsTelecommandPacket_t *TC ); |
|
28 | 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 | 59 | unsigned int check_update_info_hk_tds_mode( unsigned char mode ); |
|
62 | 60 | unsigned int check_update_info_hk_thr_mode( unsigned char mode ); |
|
63 | 61 | void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC ); |
|
64 | void build_rw_fbins_mask( unsigned int channel ); | |
|
65 | void build_rw_fbins_masks(); | |
|
62 | void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag ); | |
|
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 | 67 | // FBINS_MASK |
|
68 | 68 | int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC ); |
@@ -90,6 +90,6 float cp_rpw_sc_rw3_f1; | |||
|
90 | 90 | float cp_rpw_sc_rw3_f2; |
|
91 | 91 | float cp_rpw_sc_rw4_f1; |
|
92 | 92 | float cp_rpw_sc_rw4_f2; |
|
93 | unsigned char rw_fbins_mask_f0[16]; | |
|
94 | unsigned char rw_fbins_mask_f1[16]; | |
|
95 | unsigned char rw_fbins_mask_f2[16]; | |
|
93 | float sy_lfr_sc_rw_delta_f; | |
|
94 | ||
|
95 | fbins_masks_t fbins_masks; |
@@ -169,6 +169,7 rtems_task Init( rtems_task_argument ign | |||
|
169 | 169 | cp_rpw_sc_rw3_f2 = 0.0; |
|
170 | 170 | cp_rpw_sc_rw4_f1 = 0.0; |
|
171 | 171 | cp_rpw_sc_rw4_f2 = 0.0; |
|
172 | sy_lfr_sc_rw_delta_f = 0.0; | |
|
172 | 173 | update_last_valid_transition_date( DEFAULT_LAST_VALID_TRANSITION_DATE ); |
|
173 | 174 | |
|
174 | 175 | // waveform picker initialization |
@@ -684,13 +684,13 int getFBinMask( int index, unsigned cha | |||
|
684 | 684 | switch(channel) |
|
685 | 685 | { |
|
686 | 686 | case 0: |
|
687 |
sy_lfr_fbins_fx_word1 = |
|
|
687 | sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0; | |
|
688 | 688 | break; |
|
689 | 689 | case 1: |
|
690 |
sy_lfr_fbins_fx_word1 = |
|
|
690 | sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1; | |
|
691 | 691 | break; |
|
692 | 692 | case 2: |
|
693 |
sy_lfr_fbins_fx_word1 = |
|
|
693 | sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2; | |
|
694 | 694 | break; |
|
695 | 695 | default: |
|
696 | 696 | PRINTF("ERR *** in getFBinMask, wrong frequency channel") |
@@ -297,7 +297,7 int action_update_info(ccsdsTelecommandP | |||
|
297 | 297 | // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets) |
|
298 | 298 | cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ]; |
|
299 | 299 | getReactionWheelsFrequencies( TC ); |
|
300 |
build_rw |
|
|
300 | build_sy_lfr_rw_masks(); | |
|
301 | 301 | |
|
302 | 302 | result = status; |
|
303 | 303 |
@@ -342,6 +342,10 int action_load_filter_par(ccsdsTelecomm | |||
|
342 | 342 | parameter_dump_packet.sy_lfr_sc_rw_delta_f[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 1 ]; |
|
343 | 343 | parameter_dump_packet.sy_lfr_sc_rw_delta_f[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 2 ]; |
|
344 | 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*) ¶meter_dump_packet.sy_lfr_sc_rw_delta_f[0] ); | |
|
345 | 349 | } |
|
346 | 350 | |
|
347 | 351 | return flag; |
@@ -902,7 +906,6 void getReactionWheelsFrequencies( ccsds | |||
|
902 | 906 | */ |
|
903 | 907 | |
|
904 | 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 | 910 | bytePosPtr = (unsigned char *) &TC->packetID; |
|
908 | 911 | |
@@ -939,54 +942,78 void getReactionWheelsFrequencies( ccsds | |||
|
939 | 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 |
|
|
945 | void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag ) | |
|
943 | 946 | { |
|
944 | 947 | /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received. |
|
945 | 948 | * |
|
946 | 949 | * @param fbins_mask |
|
947 | * @param freq | |
|
948 | * @param deltaFreq | |
|
950 | * @param rw_f is the reaction wheel frequency to filter | |
|
951 | * @param delta_f is the frequency step between the frequency bins, it depends on the frequency channel | |
|
949 | 952 | * @param flag [true] filtering enabled [false] filtering disabled |
|
950 | 953 | * |
|
951 | 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 | |
|
958 | fBelow = (unsigned int) ( floor( ((double) cp_rpw_sc_rw1_f1) / ((double) deltaFreq)) ); | |
|
969 | // compute the frequency range to filter [ rw_f - delta_f/2; rw_f + delta_f/2 ] | |
|
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 | 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 |
|
|
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 | 996 | unsigned char *maskPtr; |
|
973 | 997 | double deltaF; |
|
974 | 998 | unsigned k; |
|
975 | 999 | |
|
976 | 1000 | k = 0; |
|
977 | 1001 | |
|
1002 | maskPtr = NULL; | |
|
1003 | deltaF = 1.; | |
|
1004 | ||
|
978 | 1005 | switch (channel) |
|
979 | 1006 | { |
|
980 | 1007 | case 0: |
|
981 | maskPtr = rw_fbins_mask_f0; | |
|
1008 | maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f0_word1; | |
|
982 | 1009 | deltaF = 96.; |
|
983 | 1010 | break; |
|
984 | 1011 | case 1: |
|
985 | maskPtr = rw_fbins_mask_f1; | |
|
1012 | maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f1_word1; | |
|
986 | 1013 | deltaF = 16.; |
|
987 | 1014 | break; |
|
988 | 1015 | case 2: |
|
989 | maskPtr = rw_fbins_mask_f2; | |
|
1016 | maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f2_word1; | |
|
990 | 1017 | deltaF = 1.; |
|
991 | 1018 | break; |
|
992 | 1019 | default: |
@@ -995,39 +1022,76 void build_rw_fbins_mask( unsigned int c | |||
|
995 | 1022 | |
|
996 | 1023 | for (k = 0; k < 16; k++) |
|
997 | 1024 | { |
|
998 |
rw_fbins_mask[k] = 0x |
|
|
1025 | local_rw_fbins_mask[k] = 0xff; | |
|
999 | 1026 | } |
|
1000 | 1027 | |
|
1001 | 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 | 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 | 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 | 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 | 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 | 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 | 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 | 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 | 1052 | // update the value of the fbins related to reaction wheels frequency filtering |
|
1053 | if (maskPtr != NULL) | |
|
1054 | { | |
|
1020 | 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 |
|
|
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 ); | |
|
1029 | build_rw_fbins_mask( 1 ); | |
|
1030 | build_rw_fbins_mask( 2 ); | |
|
1073 | unsigned char k; | |
|
1074 | ||
|
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 | 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 = ¶meter_dump_packet.sy_lfr_fbins_f0_word1[k*NB_BYTES_PER_FBINS_MASK]; | |
|
1056 | } | |
|
1057 | ||
|
1058 | 1116 | |
|
1059 | 1117 | return status; |
|
1060 | 1118 | } |
@@ -1086,17 +1144,17 int check_sy_lfr_filter_parameters( ccsd | |||
|
1086 | 1144 | sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & 0x01; // [0000 0001] |
|
1087 | 1145 | sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ]; |
|
1088 | 1146 | copyFloatByChar( |
|
1089 | (char*) &sy_lfr_pas_filter_tbad, | |
|
1090 | (char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD ] | |
|
1147 | (unsigned char*) &sy_lfr_pas_filter_tbad, | |
|
1148 | (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD ] | |
|
1091 | 1149 | ); |
|
1092 | 1150 | sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ]; |
|
1093 | 1151 | copyFloatByChar( |
|
1094 | (char*) &sy_lfr_pas_filter_shift, | |
|
1095 | (char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT ] | |
|
1152 | (unsigned char*) &sy_lfr_pas_filter_shift, | |
|
1153 | (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT ] | |
|
1096 | 1154 | ); |
|
1097 | 1155 | copyFloatByChar( |
|
1098 | (char*) &sy_lfr_sc_rw_delta_f, | |
|
1099 | (char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F ] | |
|
1156 | (unsigned char*) &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