latest BAE code 10 dec
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of RAJANGAM_REVIEW_BAE_CODE by
TCTM.cpp
00001 00002 #include "mbed.h" 00003 #include "rtos.h" 00004 #include "TCTM.h" 00005 #include "crc.h" 00006 #include "EPS.h" 00007 #include "ACS.h" 00008 #include "BCN.h" 00009 #include "pin_config.h" 00010 #include "FreescaleIAP.h" 00011 #include "inttypes.h" 00012 #include "iostream" 00013 #include "stdint.h" 00014 #include "cassert" 00015 #include"math.h" 00016 00017 //Serial pc(USBTX,USBRX); 00018 00019 Serial gpc(USBTX, USBRX); 00020 00021 00022 //**********************************STATUS_PARAMETERS***************************************************** 00023 uint8_t BCN_TX_SW_ENABLE=0x00; 00024 00025 //***********************************FOR STANDBY TIMER**************************************************** 00026 extern void BAE_STANDBY_TIMER_RESET(); 00027 00028 uint8_t telemetry[134]; 00029 00030 //*****************PARA****************************** 00031 00032 //ACS 00033 extern float db[3]; 00034 extern float data[6]; 00035 extern float b_old[3]; // Unit: Tesla 00036 extern float moment[3]; 00037 extern uint8_t ACS_STATE; 00038 extern uint8_t ACS_STATUS; 00039 extern uint8_t flag_firsttime; 00040 extern uint8_t ACS_DETUMBLING_ALGO_TYPE; 00041 extern uint8_t ACS_INIT_STATUS; 00042 extern uint8_t ACS_DATA_ACQ_STATUS; 00043 extern uint8_t ACS_MAIN_STATUS; 00044 extern uint8_t ACS_MAG_TIME_DELAY; 00045 extern uint8_t ACS_DEMAG_TIME_DELAY; 00046 extern uint8_t ACS_Z_FIXED_MOMENT; 00047 extern uint8_t ACS_TR_X_PWM; 00048 extern uint8_t ACS_TR_Y_PWM; 00049 extern uint8_t ACS_TR_Z_PWM; 00050 extern uint8_t B_SCZ_ANGLE; 00051 extern uint8_t alarmmode; 00052 extern uint8_t controlmode_mms; 00053 extern uint8_t invjm_mms[9]; 00054 extern uint8_t jm_mms[9]; 00055 extern uint8_t bb_mms[3]; 00056 extern uint8_t singularity_flag_mms; 00057 extern uint8_t ACS_TR_Z_SW_STATUS; 00058 extern uint8_t ACS_TR_XY_SW_STATUS; 00059 00060 extern uint8_t ATS1_EVENT_STATUS_RGTR; 00061 extern uint8_t ATS1_SENTRAL_STATUS_RGTR; 00062 extern uint8_t ATS1_ERROR_RGTR; 00063 extern uint8_t ATS2_EVENT_STATUS_RGTR; 00064 extern uint8_t ATS2_SENTRAL_STATUS_RGTR; 00065 extern uint8_t ATS2_ERROR_RGTR; 00066 //change 00067 extern uint16_t ACS_MM_X_COMSN; 00068 extern uint16_t ACS_MM_Y_COMSN; 00069 extern uint16_t ACS_MG_X_COMSN; 00070 extern uint16_t ACS_MG_Y_COMSN; 00071 extern uint16_t ACS_MM_Z_COMSN; 00072 extern uint16_t ACS_MG_Z_COMSN; 00073 //acs func 00074 extern void F_ACS(); 00075 extern int SENSOR_INIT(); 00076 extern int FCTN_ACS_INIT(); 00077 extern int SENSOR_DATA_ACQ(); 00078 extern int FCTN_ATS_DATA_ACQ(); 00079 extern void FCTN_ACS_GENPWM_MAIN(float Moment[3]); 00080 extern void FCTN_ACS_CNTRLALGO(float*,float*,int); 00081 00082 00083 //main 00084 extern Timer BAE_uptime; 00085 extern Timer I2C_last; 00086 extern uint8_t BAE_RESET_COUNTER; 00087 extern void RETURN_UPTIME(float time, uint8_t *day,uint8_t *hour,uint8_t *min); 00088 00089 extern uint8_t ACS_ATS_STATUS; 00090 extern uint16_t ACS_MAIN_COUNTER;//change\apply 00091 extern uint8_t HTR_CYCLE_COUNTER; 00092 extern RtosTimer *HTR_CYCLE; 00093 extern uint8_t HTR_CYCLE_COUNTS; //Count of heater cycles 00094 extern uint8_t HTR_CYCLE_START_DLY; //EPS_HTR_DLY_TIMER timer duration in minutes 00095 extern uint8_t HTR_ON_DURATION; //EPS_HTR_OFF timer duration in minutes 00096 extern uint16_t HTR_CYCLE_PERIOD; 00097 00098 extern DigitalOut ACS_TR_XY_ENABLE; 00099 extern DigitalOut ACS_TR_Z_ENABLE; 00100 extern DigitalIn ACS_TR_XY_OC_FAULT; 00101 extern DigitalIn ACS_TR_Z_OC_FAULT; 00102 extern DigitalIn ACS_TR_XY_FAULT; 00103 extern DigitalIn ACS_TR_Z_FAULT; 00104 extern DigitalIn ACS_ATS1_OC_FAULT; 00105 extern DigitalIn ACS_ATS2_OC_FAULT; 00106 00107 extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch 00108 extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch 00109 00110 extern DigitalOut DRV_Z_EN; 00111 extern DigitalOut DRV_XY_EN; 00112 //extern DigitalOut TRXY_SW; //TR XY Switch if any TR_SW error arises then it is same as TR_SW_EN 00113 //extern DigitalOut TRZ_SW; //TR Z Switch 00114 00115 extern DigitalOut phase_TR_x; 00116 extern DigitalOut phase_TR_y; 00117 extern DigitalOut phase_TR_z; 00118 00119 00120 //CDMS 00121 extern DigitalOut CDMS_RESET; // CDMS RESET 00122 extern uint8_t CDMS_SW_STATUS; 00123 extern DigitalIn CDMS_OC_FAULT; 00124 00125 00126 //BCN 00127 extern DigitalOut BCN_SW; //Beacon switch 00128 extern uint8_t BCN_TX_STATUS; 00129 extern uint8_t BCN_FEN; 00130 extern uint8_t BCN_SPND_TX; 00131 extern uint8_t BCN_TX_MAIN_STATUS; 00132 extern uint8_t BCN_TX_SW_STATUS; 00133 extern uint8_t BCN_LONG_MSG_TYPE; 00134 extern uint8_t BCN_INIT_STATUS; 00135 extern uint8_t BCN_FAIL_COUNT; 00136 extern uint16_t BCN_TX_MAIN_COUNTER; 00137 extern DigitalIn BCN_TX_OC_FAULT; 00138 extern uint8_t BCN_TMP; 00139 extern void F_BCN(); 00140 extern void FCTN_BCN_TX_MAIN(); 00141 extern uint8_t SHORT_HK_data[15]; 00142 extern void FCTN_BCN_SPND_TX(); 00143 extern void FCTN_BCN_INIT(); 00144 00145 extern uint8_t readreg(uint8_t reg); 00146 extern void writereg(uint8_t reg,uint8_t val); 00147 00148 00149 //BAE 00150 extern uint8_t BAE_STANDBY; 00151 extern uint8_t BAE_INIT_STATUS; 00152 extern uint8_t BAE_RESET_COUNTER; 00153 extern uint8_t BAE_MNG_I2C_STATUS; 00154 /*given a default value as of now shuld read value from flash and increment it write it back very time it starts(bae)*/ 00155 00156 extern uint32_t BAE_STATUS;//extern uint32_t BAE_STATUS; 00157 extern BAE_HK_quant quant_data; 00158 extern BAE_HK_actual actual_data; 00159 extern BAE_HK_min_max bae_HK_minmax; 00160 //extern DigitalOut TRXY_SW; 00161 //extern DigitalOut TRZ_SW_EN; //same as TRZ_SW 00162 extern uint32_t BAE_ENABLE; 00163 extern uint16_t BAE_I2C_COUNTER; 00164 extern uint8_t LONG_HK_data[2][134]; 00165 //extern uint8_t BCN_FAIL_COUNT; 00166 00167 00168 //EPS 00169 extern bool firstCount; 00170 extern uint8_t EPS_BTRY_HTR_AUTO; 00171 extern uint8_t EPS_BATT_TEMP_LOW; 00172 extern uint8_t EPS_BATT_TEMP_HIGH; 00173 extern uint8_t EPS_BATT_TEMP_DEFAULT; 00174 extern DigitalOut BTRY_HTR_ENABLE; 00175 extern uint8_t EPS_SOC_LEVEL_12; 00176 extern uint8_t EPS_SOC_LEVEL_23; 00177 extern uint8_t EPS_INIT_STATUS; 00178 extern uint8_t EPS_BATTERY_GAUGE_STATUS ; 00179 extern uint8_t EPS_MAIN_STATUS; 00180 extern uint8_t EPS_BTRY_TMP_STATUS ; 00181 extern uint8_t EPS_STATUS ; 00182 extern uint8_t EPS_BAT_TEMP_LOW; 00183 extern uint8_t EPS_BAT_TEMP_HIGH; 00184 extern uint8_t EPS_BAT_TEMP_DEFAULT; 00185 extern uint16_t EPS_MAIN_COUNTER; 00186 extern uint8_t EPS_BTRY_HTR; 00187 00188 extern DigitalOut SelectLineb3; // MSB of Select Lines 00189 extern DigitalOut SelectLineb2; 00190 extern DigitalOut SelectLineb1; 00191 extern DigitalOut SelectLineb0; 00192 extern DigitalIn EPS_CHARGER_FAULT; 00193 extern DigitalIn EPS_CHARGER_STATUS; 00194 extern DigitalIn EPS_BATTERY_GAUGE_ALERT; 00195 00196 extern void F_EPS(); 00197 extern AnalogIn CurrentInput; 00198 00199 //--------------------check this refer prashant 00200 00201 extern PwmOut PWM1; //x //Functions used to generate PWM signal 00202 extern PwmOut PWM2; //y 00203 extern PwmOut PWM3; //z //PWM output comes from pins p6 00204 00205 //included after home 00206 //extern void FCTN_ACS_GENPWM_MAIN(); 00207 00208 uint16_t crc_hk_data()//gencrc16_for_me() 00209 { 00210 uint16_t crc = CRC::crc16_gen(&LONG_HK_data[1][0],132);//BAE_chardata i.e char data type usesd earlier BAE_HK_data 00211 return crc; 00212 } 00213 00214 uint8_t crc8_short() 00215 { 00216 uint8_t crc = CRC::crc8_gen(SHORT_HK_data,14); 00217 return crc; 00218 } 00219 00220 float uint16_to_float(float min,float max,uint16_t scale) 00221 { 00222 float div=max-min; 00223 div=(div/(65535.0)); 00224 return ((div*(float)scale)+ min); 00225 } 00226 00227 uint16_t float_to_uint16(float min,float max,float val) //takes care of -ve num as its scale with min and max as reference 00228 { 00229 if(val>max) 00230 {return 0xffff; 00231 } 00232 if(val<min) 00233 {return 0x0000; 00234 } 00235 float div=max-min; 00236 div=(65535.0/div); 00237 val=((val-min)*div); 00238 //gpc.printf("\n\r the scale is %x",(uint16_t)val); 00239 return (uint16_t)val; 00240 } 00241 00242 void gen_I_TM() 00243 { 00244 telemetry[0] = 0xB0; 00245 for(int i=1;i<11;i++) 00246 telemetry[i] = 0x00; 00247 uint16_t crc = CRC::crc16_gen(telemetry,11);//BAE_chardata i.e char data type usesd earlier 00248 telemetry[11] = (uint8_t)(crc >> 8); 00249 telemetry[12] = (uint8_t)crc ; 00250 for(int i=13;i<135;i++) 00251 telemetry[i] = 0x00; 00252 } 00253 void FCTN__UINT (uint8_t input[2], float* output) 00254 { 00255 00256 *output = (float) input[1]; 00257 *output = *output/100.0; //input[0] integer part 00258 *output = *output + (float) input[0]; //input[1] decimal part correct to two decimal places 00259 } 00260 00261 float angle(float x,float y,float z) 00262 { 00263 float mag_total=sqrt(x*x + y*y + z*z); 00264 float cos_z = z/mag_total; 00265 float theta_z = acosf(cos_z); 00266 00267 return theta_z; 00268 //gpc.printf("/n cos_zz= %f /t theta_z= %f /n",cos_z,theta_z); 00269 } 00270 00271 //uint8_t tm1[134]; 00272 00273 void FCTN_BAE_TM_TC (uint8_t* tc) 00274 { 00275 //tm1[0] = 1; 00276 //calculating crc 00277 for(int i=0;i<134;i++) 00278 { 00279 telemetry[i]=tc[i]; 00280 } 00281 00282 00283 /*changed*/ 00284 uint8_t* tm; // without it some identifier error 00285 uint16_t crc16=CRC::crc16_gen(telemetry,9); //implementing crc 00286 gpc.printf("\n\r the crc is %x",crc16); 00287 // for(int qw=0;qw<11;qw++); 00288 // gpc.printf("\n\r the telecommand %d \t %x",qw,tc[qw]); 00289 // pc.printf("\n the tc provided is %s \n" , tc"); 00290 if( ( ((uint8_t)((crc16 & 0xFF00)>>8)==tc[9]) && ((uint8_t)(crc16 & 0x00FF)==tc[10]) )== 0 ) 00291 { 00292 telemetry[0]=0xB0; 00293 telemetry[1]=0x00;//tc psc defined for this case 00294 telemetry[2]=0x01;//ack code for this case 00295 for(int i=3;i<11;i++) 00296 { 00297 telemetry[i]=0x00; 00298 } 00299 //add crc 00300 crc16 = CRC::crc16_gen(telemetry,11); 00301 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00302 telemetry[12] = (uint8_t)(crc16&0x00FF); 00303 gpc.printf("\n\rincorrect crc"); 00304 for(int i=13;i<134;i++) 00305 { 00306 telemetry[i]=0x00; 00307 } 00308 } 00309 else 00310 { 00311 //check apid 00312 uint8_t apid_check=((tc[1]&0xC0)>>6); 00313 if(apid_check!=0x01) 00314 { 00315 telemetry[0]=0xB0; 00316 telemetry[1]=tc[0];//tc psc defined for this case 00317 telemetry[2]=0x02;//ack code for this case 00318 for(int i=3;i<11;i++) 00319 { 00320 telemetry[i]=0x00; 00321 } 00322 crc16 = CRC::crc16_gen(telemetry,11); 00323 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00324 telemetry[12] = (uint8_t)(crc16&0x00FF); 00325 gpc.printf("\n\rillegal TC packet APID don't match"); 00326 for(int i=13;i<134;i++) 00327 { 00328 telemetry[i]=0x00; 00329 } 00330 } 00331 else 00332 {// all the possible cases of fms and mms 00333 uint8_t service_type=(tc[2]&0xF0); 00334 //check for fms first 00335 switch(service_type) 00336 { 00337 case 0x60: 00338 { 00339 gpc.printf("Memory Management Service\r\n"); 00340 uint8_t service_subtype=(tc[2]&0x0F); 00341 switch(service_subtype) 00342 { 00343 case 0x02: 00344 { 00345 gpc.printf("\n\rRead from RAM"); 00346 uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4]; 00347 switch(MID) 00348 { 00349 case 0x0000://changed from 0001 00350 { 00351 gpc.printf("\n\rRead from MID 0000 \n"); 00352 gpc.printf("\n\rReading flash parameters"); 00353 00354 /*taking some varible till we find some thing more useful*/ 00355 //uint8_t ref_val=0x01; 00356 telemetry[0] = 0x30; 00357 telemetry[1] = tc[0]; 00358 telemetry[2] = ACK_CODE; 00359 /*random or with bcn_tx_sw_enable assuming it is 1 bit in length 00360 how to get that we dont know, now we just assume it to be so*/ 00361 telemetry[3] = 0x00; 00362 telemetry[4] = ACS_ATS_STATUS; 00363 telemetry[5] = ACS_TR_XY_SW_STATUS; 00364 telemetry[5] = (telemetry[5]<<2)| ACS_TR_Z_SW_STATUS; 00365 telemetry[5] = (telemetry[5]<<4) | ACS_STATE; 00366 telemetry[6] = ACS_DETUMBLING_ALGO_TYPE; 00367 telemetry[6] = (telemetry[6]<<2) | BCN_TX_SW_STATUS; 00368 telemetry[6] = (telemetry[6]<<1) | BCN_SPND_TX; 00369 telemetry[6] = (telemetry[6]<<1) | BCN_FEN; 00370 telemetry[6] = (telemetry[6]<<1) | BCN_LONG_MSG_TYPE; 00371 telemetry[6] = (telemetry[6]<<1) | EPS_BTRY_HTR_AUTO;//EPS_BATTERY_HEATER_ENABLE 00372 telemetry[6] = (telemetry[6]<<1); 00373 //now one spares in telemetry[6] 00374 telemetry[7] = BAE_RESET_COUNTER; 00375 telemetry[8] = EPS_SOC_LEVEL_12; 00376 telemetry[9] = EPS_SOC_LEVEL_23; 00377 telemetry[10] = ACS_MAG_TIME_DELAY; 00378 telemetry[11] = ACS_DEMAG_TIME_DELAY; 00379 telemetry[12] = EPS_BAT_TEMP_LOW; 00380 telemetry[13] = EPS_BAT_TEMP_HIGH; 00381 telemetry[14] = EPS_BAT_TEMP_DEFAULT; 00382 00383 telemetry[15] = ACS_MM_X_COMSN >> 8; 00384 telemetry[16] = ACS_MM_X_COMSN; 00385 printf("\n\r the mm x commission %d",ACS_MM_X_COMSN); 00386 00387 telemetry[17] = ACS_MM_Y_COMSN >> 8; 00388 telemetry[18] = ACS_MM_Y_COMSN; 00389 00390 telemetry[19] = ACS_MG_X_COMSN >> 8; 00391 telemetry[20] = ACS_MG_X_COMSN; 00392 00393 telemetry[21] = ACS_MG_Y_COMSN >> 8; 00394 telemetry[22] = ACS_MG_Y_COMSN; 00395 00396 telemetry[23] = ACS_MM_Z_COMSN >> 8; 00397 telemetry[24] = ACS_MM_Z_COMSN; 00398 00399 telemetry[25] = ACS_MG_Z_COMSN >> 8; 00400 telemetry[26] = ACS_MG_Z_COMSN; 00401 00402 telemetry[27] = ACS_Z_FIXED_MOMENT >> 8; 00403 telemetry[28] = ACS_Z_FIXED_MOMENT; 00404 printf("\n\r the z_fixed_moment %d",ACS_Z_FIXED_MOMENT); 00405 00406 00407 //BAE RAM PARAMETER 00408 telemetry[29] = BAE_INIT_STATUS; 00409 telemetry[29] = (telemetry[29]<<1) | BAE_MNG_I2C_STATUS;//changed 00410 telemetry[29] = (telemetry[29]<<1) | BCN_INIT_STATUS; 00411 telemetry[29] = (telemetry[29]<<1) | BCN_TX_MAIN_STATUS; 00412 telemetry[29] = (telemetry[29]<<3) | BCN_TX_STATUS; 00413 telemetry[29] = (telemetry[29]<<1) | ACS_INIT_STATUS; 00414 00415 telemetry[30] = ACS_DATA_ACQ_STATUS; 00416 telemetry[30] = (telemetry[30]<<1) | ACS_MAIN_STATUS; 00417 telemetry[30] = (telemetry[30]<<4) | ACS_STATUS; 00418 telemetry[30] = (telemetry[30]<<1) | EPS_INIT_STATUS; 00419 00420 telemetry[31] = EPS_BATTERY_GAUGE_STATUS; 00421 telemetry[31] = (telemetry[31]<<1) | EPS_MAIN_STATUS; 00422 telemetry[31] = (telemetry[31]<<1) | EPS_BTRY_TMP_STATUS; 00423 telemetry[31] = (telemetry[31]<<3) | EPS_STATUS; 00424 telemetry[31] = (telemetry[31]<<2) | CDMS_SW_STATUS; 00425 // telemetry[30] = (telemetry[30]<<1) | EPS_BTRY_HTR_STATUS;//new to : implement 00426 00427 telemetry[32] = EPS_BTRY_HTR; //new to : implement 00428 //spare 4 00429 telemetry[32] = (telemetry[32]<<7) | BAE_STANDBY; 00430 // 6 next telemetries value to be given by registers 00431 telemetry[33] = ATS1_EVENT_STATUS_RGTR; 00432 telemetry[34] = ATS1_SENTRAL_STATUS_RGTR; 00433 telemetry[35] = ATS1_ERROR_RGTR; 00434 telemetry[36] = ATS2_EVENT_STATUS_RGTR; 00435 telemetry[37] = ATS2_SENTRAL_STATUS_RGTR; 00436 telemetry[38] = ATS2_ERROR_RGTR; 00437 00438 telemetry[39] = BCN_FAIL_COUNT; 00439 telemetry[40] = actual_data.power_mode; 00440 telemetry[41] = HTR_CYCLE_COUNTER;//new to : implement 00441 00442 // GS changed order has to be updated in the telemetry sheet or jithin should change the order 00443 telemetry[43] = BAE_I2C_COUNTER; 00444 telemetry[42] = BAE_I2C_COUNTER>>8; 00445 telemetry[45] = ACS_MAIN_COUNTER; 00446 telemetry[44] = ACS_MAIN_COUNTER>>8; 00447 telemetry[47] = BCN_TX_MAIN_COUNTER; 00448 telemetry[46] = BCN_TX_MAIN_COUNTER>>8; 00449 telemetry[49] = EPS_MAIN_COUNTER; 00450 telemetry[48] = EPS_MAIN_COUNTER>>8; 00451 00452 uint8_t days,hours,mins; 00453 RETURN_UPTIME(BAE_uptime.read(),&days,&hours,&mins); 00454 printf("\n\r the values bae of days,hour,min %d, %d, %d",days,hours,mins); 00455 00456 telemetry[50] = days; 00457 RETURN_UPTIME(I2C_last.read(),&days,&hours,&mins); 00458 telemetry[50] = (telemetry[50]<<3) | (hours>>2); 00459 telemetry[51] = hours; 00460 telemetry[51] = (telemetry[51]<<6) | mins; 00461 00462 printf("\n\r the values of bae uptime %f",BAE_uptime.read()); 00463 printf("\n\r the values i2c of days,hour,min %d, %d, %d",days,hours,mins); 00464 00465 00466 //sending in uint can be converted back to int by direct conversion for +values 00467 //make sure to convert baack to int for getting negative values 00468 //algo for that done 00469 00470 uint16_t scaled; 00471 00472 scaled = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]); 00473 telemetry[52] = (scaled>>8); 00474 telemetry[53] = scaled; 00475 scaled = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]); 00476 telemetry[54] = (scaled>>8); 00477 telemetry[55] = scaled; 00478 scaled = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]); 00479 telemetry[56] = (scaled>>8); 00480 telemetry[57] = scaled; 00481 scaled = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[0]); 00482 telemetry[58] = (scaled>>8); 00483 telemetry[59] = scaled; 00484 scaled = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[1]); 00485 telemetry[60] = (scaled>>8); 00486 telemetry[61] = scaled; 00487 scaled = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[2]); 00488 telemetry[62] = (scaled>>8); 00489 telemetry[63] = scaled; 00490 00491 telemetry[64] = (~BCN_TX_OC_FAULT); 00492 telemetry[64] = (telemetry[64]<<1) | ACS_TR_XY_ENABLE; 00493 telemetry[64] = (telemetry[64]<<1) | ACS_TR_Z_ENABLE; 00494 telemetry[64] = (telemetry[64]<<1) | (~ACS_TR_XY_OC_FAULT); 00495 telemetry[64] = (telemetry[64]<<1) | (~ACS_TR_Z_OC_FAULT); 00496 telemetry[64] = (telemetry[64]<<1) | (~ACS_TR_XY_FAULT); 00497 telemetry[64] = (telemetry[64]<<1) | (~EPS_CHARGER_FAULT); 00498 telemetry[64] = (telemetry[64]<<1) | EPS_CHARGER_STATUS; 00499 00500 telemetry[65] = EPS_BATTERY_GAUGE_ALERT; 00501 telemetry[65] = (telemetry[65]<<1) | (~CDMS_OC_FAULT); 00502 telemetry[65] = (telemetry[65]<<1) | (~ACS_ATS1_OC_FAULT); 00503 telemetry[65] = (telemetry[65]<<1) | (~ACS_ATS2_OC_FAULT); 00504 telemetry[65] = (telemetry[65]<<1) | (~ACS_TR_Z_FAULT); 00505 telemetry[65] = (telemetry[65]<<3); 00506 //3 spare 00507 00508 telemetry[66] = ACS_TR_X_PWM; 00509 telemetry[67] = ACS_TR_Y_PWM; 00510 telemetry[68] = ACS_TR_Z_PWM; 00511 //spare byte 00512 //assigned it to counter HTR_CYCLE_COUNTER 00513 00514 //assign it b_scz_angle 00515 telemetry[69] = B_SCZ_ANGLE>>4; 00516 telemetry[69] = (telemetry[69]<<1) | alarmmode; 00517 telemetry[69] = (telemetry[69]<<1) | controlmode_mms; 00518 telemetry[69] = (telemetry[69]<<1) | singularity_flag_mms; 00519 telemetry[69] = (telemetry[69]<<1); 00520 //1 bit spare 00521 00522 for(int i=0;i<9;i++) 00523 { 00524 telemetry[70+i] = invjm_mms[i]; 00525 telemetry[81+i] = jm_mms[i]; 00526 } 00527 00528 for(int i=1;i<3;i++) 00529 telemetry[79+i] = bb_mms[i]; 00530 00531 for(int i=0;i<16;i++) 00532 { 00533 telemetry[90+i] = (uint8_t)(quant_data.voltage_quant[i]);//changed (uint8_t)(quant_data.voltage_quant[i]<<3)+(quant_data.voltage_quant[i]<<1); /*multiplied by 10 */ 00534 //printf("\n\r %d \t %d",i,telemetry[90+i]); 00535 telemetry[106+i] = quant_data.current_quant[i]; 00536 /*check the working*/ 00537 } 00538 00539 telemetry[122] = quant_data.Batt_voltage_quant;// changed (quant_data.Batt_voltage_quant<<3)+(quant_data.Batt_voltage_quant<<1); 00540 telemetry[123] = quant_data.BAE_temp_quant; 00541 telemetry[124] = (uint8_t)(actual_data.Batt_gauge_actual[1]); 00542 telemetry[125] = quant_data.Batt_temp_quant[0]; 00543 telemetry[126] = quant_data.Batt_temp_quant[1]; 00544 00545 telemetry[127] = BCN_TMP; 00546 00547 for (int i=128; i<132;i++) 00548 { 00549 telemetry[i] = 0x00; 00550 } 00551 crc16 = CRC::crc16_gen(telemetry,132); 00552 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8); 00553 telemetry[133] = (uint8_t)(crc16&0x00FF); 00554 00555 break; 00556 } 00557 case 0x0001: 00558 { 00559 gpc.printf("\r\nHK data min max"); 00560 telemetry[0] = 0x30; // changed0x06 shifted by 1 bit 0x03 00561 telemetry[1] = tc[0]; 00562 telemetry[2] = ACK_CODE; 00563 telemetry[3] = 0x00; 00564 00565 for(int i;i<16;i++) 00566 { 00567 telemetry[i+4] = (uint8_t)bae_HK_minmax.voltage_max[i];//(bae_HK_minmax.voltage_max[i]<<3)+(bae_HK_minmax.voltage_max[i]<<2); 00568 telemetry[i+20] = bae_HK_minmax.current_max[i]; 00569 } 00570 00571 telemetry[36] = (uint8_t)bae_HK_minmax.Batt_voltage_max;//(bae_HK_minmax.Batt_voltage_max<<3)+(bae_HK_minmax.Batt_voltage_max<<2); 00572 telemetry[37] = bae_HK_minmax.BAE_temp_max; 00573 00574 telemetry[38] = bae_HK_minmax.Batt_SOC_max; 00575 00576 telemetry[39] = bae_HK_minmax.Batt_temp_max[0]; 00577 telemetry[40] = bae_HK_minmax.Batt_temp_max[1]; 00578 00579 /*BCN temp there*/ 00580 telemetry[41] = bae_HK_minmax.BCN_TEMP_max; 00581 00582 for(int i=0; i<3; i++) 00583 { 00584 telemetry[42+(2*i)] = (bae_HK_minmax.bit_data_acs_mm_max[i]>>8); 00585 telemetry[43+(2*i)] = bae_HK_minmax.bit_data_acs_mm_max[i]; 00586 00587 telemetry[48+(2*i)] = (bae_HK_minmax.bit_data_acs_mg_max[i]>>8); 00588 telemetry[49+(2*i)] = bae_HK_minmax.bit_data_acs_mg_max[i]; 00589 } 00590 00591 /*min data*/ 00592 00593 for(int i;i<16;i++) 00594 { telemetry[i+54] = (uint8_t)bae_HK_minmax.voltage_min[i];//(bae_HK_minmax.voltage_min[i]<<3)+(bae_HK_minmax.voltage_min[i]<<1); 00595 //for(int i;i<16;i++) 00596 telemetry[i+70] = bae_HK_minmax.current_min[i]; 00597 } 00598 00599 telemetry[86] = (uint8_t)bae_HK_minmax.Batt_voltage_min;//(bae_HK_minmax.Batt_voltage_min<<3)+(bae_HK_minmax.Batt_voltage_min<<1); 00600 telemetry[87] = bae_HK_minmax.BAE_temp_min; 00601 00602 /*battery soc*/ 00603 telemetry[88] = bae_HK_minmax.Batt_SOC_min; 00604 00605 telemetry[89] = bae_HK_minmax.Batt_temp_min[0]; 00606 telemetry[90] = bae_HK_minmax.Batt_temp_min[1]; 00607 //huhu// 00608 00609 /*BCN temp named as BCN_TS_BUFFER there*/ 00610 telemetry[91] = bae_HK_minmax.BCN_TEMP_min; 00611 00612 for(int i=0; i<3; i++) 00613 { 00614 telemetry[92+(2*i)] = (bae_HK_minmax.bit_data_acs_mm_min[i]>>8); 00615 telemetry[93+(2*i)] = bae_HK_minmax.bit_data_acs_mm_min[i]; 00616 00617 telemetry[98+(2*i)] = (bae_HK_minmax.bit_data_acs_mg_min[i]>>8); 00618 telemetry[99+(2*i)] = bae_HK_minmax.bit_data_acs_mg_min[i]; 00619 } 00620 telemetry[104] = (~(BCN_TX_OC_FAULT)); 00621 telemetry[104] = (telemetry[104]<<1) | (~(ACS_TR_XY_OC_FAULT)); 00622 telemetry[104] = (telemetry[104]<<1) | (~(ACS_TR_Z_OC_FAULT)); 00623 telemetry[104] = (telemetry[104]<<1) | (~(ACS_TR_XY_FAULT)); 00624 //EPS CHARGER 00625 telemetry[104] = (telemetry[104]<<1) | (~(EPS_CHARGER_FAULT));//eps_charger; 00626 telemetry[104] = (telemetry[104]<<1) | (~(CDMS_OC_FAULT)); 00627 telemetry[104] = (telemetry[104]<<1) | (~(ACS_ATS1_OC_FAULT)); 00628 telemetry[104] = (telemetry[104]<<1) | (~(ACS_ATS2_OC_FAULT)); 00629 00630 telemetry[105] = (ACS_TR_Z_FAULT<<7); 00631 //spare 23 bits 00632 telemetry[106] = 0x00; 00633 telemetry[107] = 0x00; 00634 00635 for (int i=108; i<132;i++) 00636 { 00637 telemetry[i] = 0x00; 00638 } 00639 crc16 = CRC::crc16_gen(telemetry,132); 00640 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8); 00641 telemetry[133] = (uint8_t)(crc16&0x00FF); 00642 break; 00643 } 00644 default://invalid MID 00645 { 00646 //ACK_L234_telemetry 00647 telemetry[0]=0xB0; // check if this needs to be changed 00648 telemetry[1]=tc[0]; 00649 telemetry[2]=0x02;//for this case 00650 for(uint8_t i=3;i<11;i++) 00651 { 00652 telemetry[i]=0x00; 00653 } 00654 crc16 = CRC::crc16_gen(telemetry,11); 00655 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00656 telemetry[12] = (uint8_t)(crc16&0x00FF); 00657 for(uint8_t i=13;i<134;i++) 00658 { 00659 telemetry[i]=0x00; 00660 } 00661 break; 00662 } 00663 } 00664 break; 00665 } 00666 case 0x05: 00667 { 00668 gpc.printf("\n\rDATA for mms 0x05 flash"); 00669 /*changed*/ 00670 gpc.printf("\n\rWrite on flash\n"); 00671 uint32_t FLASH_DATA;//256 bits 00672 00673 uint8_t VALID_MID;//to determine wether mid is valid or not otherwise to much repetition of code 1 meaning valid 00674 00675 uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4]; 00676 switch(MID ) 00677 { 00678 case 0x1100: 00679 { 00680 //FLASH_DATA[0] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[8]); 00681 //FCTN_BAE_WR_FLASH(0,FLASH_DATA[0]); 00682 BCN_LONG_MSG_TYPE = tc[8]; 00683 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0); 00684 FLASH_DATA = (FLASH_DATA & 0xFFFFFBFF) | (10<<(uint32_t)tc[8]);//see if uint8 to uint32 conversion works 00685 FCTN_BAE_WR_FLASH(0,FLASH_DATA); 00686 VALID_MID=1; 00687 break; 00688 } 00689 case 0x0101: 00690 { 00691 //FLASH_DATA[1] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00692 ACS_DETUMBLING_ALGO_TYPE = (tc[8] & 0x01); 00693 //=============================================== 00694 ACS_STATE = 4; 00695 //tc[8] = 1001_ 00696 //ACS_STATE = (tc[8]>>1) & 0x0F; 00697 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0); 00698 FLASH_DATA = (FLASH_DATA & 0xFFF07FFF) | (15<<(uint32_t)tc[8]); 00699 FCTN_BAE_WR_FLASH(0,FLASH_DATA); 00700 VALID_MID=1; 00701 break; 00702 } 00703 00704 case 0x0102: 00705 { 00706 //FLASH_DATA[2] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00707 //EPS_BATTERY_HEATER_ENABLE = tc[8]; 00708 EPS_BTRY_HTR_AUTO = tc[8]; 00709 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0); 00710 FLASH_DATA = (FLASH_DATA & 0xFFFFFDFF) | (9<<(uint32_t)tc[8]); 00711 FCTN_BAE_WR_FLASH(0,FLASH_DATA); 00712 VALID_MID=1; 00713 break; 00714 } 00715 00716 case 0x0103: 00717 { 00718 //FLASH_DATA[3] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00719 ACS_MAG_TIME_DELAY = tc[7]; 00720 ACS_DEMAG_TIME_DELAY = tc[8]; 00721 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(1); 00722 FLASH_DATA = (FLASH_DATA & 0xFFFF0000) | (8<<(uint32_t)tc[7]) | ((uint32_t)tc[8]); 00723 FCTN_BAE_WR_FLASH(1,FLASH_DATA); 00724 VALID_MID=1; 00725 break; 00726 } 00727 case 0x0104: 00728 { 00729 //FLASH_DATA[4] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00730 ACS_Z_FIXED_MOMENT = ((uint16_t)tc[7]<<8) | (uint16_t)tc[8]; 00731 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(6); 00732 FLASH_DATA = (FLASH_DATA & 0x0000FFFF) | ((uint32_t)ACS_Z_FIXED_MOMENT<<16); 00733 FCTN_BAE_WR_FLASH(6,FLASH_DATA); 00734 VALID_MID=1; 00735 break; 00736 } 00737 case 0x0105: 00738 { 00739 //FLASH_DATA[4] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00740 EPS_BAT_TEMP_DEFAULT=(uint8_t)tc[6]; 00741 EPS_BAT_TEMP_LOW=(uint8_t)tc[7]; 00742 EPS_BAT_TEMP_HIGH=(uint8_t)tc[8]; 00743 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(2); 00744 FLASH_DATA = (FLASH_DATA & 0x000000FF) | ((uint32_t)EPS_BAT_TEMP_DEFAULT<<24) | ((uint32_t)EPS_BAT_TEMP_LOW<<16) | ((uint32_t)EPS_BAT_TEMP_LOW<<8); 00745 FCTN_BAE_WR_FLASH(6,FLASH_DATA); 00746 VALID_MID=1; 00747 break; 00748 } 00749 case 0x0106: 00750 { 00751 //FLASH_DATA[6] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00752 ACS_MM_Z_COMSN = ((uint16_t)tc[5]<<8) | (uint16_t)tc[6]; 00753 ACS_MG_Z_COMSN = ((uint16_t)tc[7]<<8) | (uint16_t)tc[8]; 00754 FLASH_DATA = ((uint32_t)ACS_MM_Z_COMSN<<16) | (uint32_t)ACS_MG_Z_COMSN; 00755 FCTN_BAE_WR_FLASH(5,FLASH_DATA); 00756 VALID_MID=1; 00757 break; 00758 } 00759 case 0x0107: 00760 { 00761 //FLASH_DATA[5] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00762 EPS_SOC_LEVEL_12 = tc[7]; 00763 EPS_SOC_LEVEL_23 = tc[8]; 00764 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(1); 00765 FLASH_DATA = (FLASH_DATA & 0x0000FFFF) | ((uint32_t)tc[7]<<24) | ((uint32_t)tc[8]<<16); 00766 FCTN_BAE_WR_FLASH(1,FLASH_DATA); 00767 VALID_MID=1; 00768 break; 00769 } 00770 case 0x0108: 00771 { 00772 //FLASH_DATA[6] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00773 ACS_MM_X_COMSN = ((uint16_t)tc[5]<<8) | (uint16_t)tc[6]; 00774 ACS_MM_Y_COMSN = ((uint16_t)tc[7]<<8) | (uint16_t)tc[8]; 00775 FLASH_DATA = ((uint32_t)ACS_MM_X_COMSN<<16) | (uint32_t)ACS_MM_Y_COMSN; 00776 FCTN_BAE_WR_FLASH(3,FLASH_DATA); 00777 VALID_MID=1; 00778 break; 00779 } 00780 case 0x0109: 00781 { 00782 //FLASH_DATA[7] = (((uint32_t)tc[5] << 24) | ((uint32_t)tc[6] << 16) | ((uint32_t)tc[7] << 8) | (uint32_t)tc[6]); 00783 ACS_MG_X_COMSN = ((uint16_t)tc[5]<<8) | (uint16_t)tc[6]; 00784 ACS_MG_Y_COMSN = ((uint16_t)tc[7]<<8) | (uint16_t)tc[8]; 00785 FLASH_DATA = ((uint32_t)ACS_MG_X_COMSN<<16) | (uint32_t)ACS_MG_Y_COMSN; 00786 FCTN_BAE_WR_FLASH(4,FLASH_DATA); 00787 VALID_MID=1; 00788 break; 00789 } 00790 00791 default: 00792 { 00793 gpc.printf("Invalid MMS case 0x05 invalid MID\r\n"); 00794 VALID_MID=0; 00795 //ACK_L234_telemetry 00796 break; 00797 00798 } 00799 } 00800 00801 if(VALID_MID==1)//valid MID 00802 { 00803 telemetry[0]=0xB0;//or 0x60? check 00804 telemetry[1]=tc[0]; 00805 telemetry[2]=0xA0;// when valid 00806 } 00807 else if(VALID_MID==0)//invalid MID 00808 { 00809 telemetry[0]=0xB0; 00810 telemetry[1]=tc[0]; 00811 telemetry[2]=0x02;//for this case 00812 } 00813 00814 for(uint8_t i=3;i<11;i++) 00815 { 00816 telemetry[i]=0x00; 00817 } 00818 crc16 = CRC::crc16_gen(telemetry,11); 00819 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00820 telemetry[12] = (uint8_t)(crc16&0x00FF); 00821 for(uint8_t i=13;i<134;i++) 00822 { 00823 telemetry[i]=0x00; 00824 } 00825 00826 gpc.printf("\n\rWritten on Flash"); 00827 break; 00828 } 00829 default://when invalid service subtype 00830 { 00831 gpc.printf("\n\r MMS invalid Service Subtype"); 00832 //ACK_L234_telemetry 00833 telemetry[0]=0xB0; 00834 telemetry[1]=tc[0]; 00835 telemetry[2]=0x02;//for this case 00836 for(uint8_t i=3;i<11;i++) 00837 { 00838 telemetry[i]=0x00; 00839 } 00840 crc16 = CRC::crc16_gen(telemetry,11); 00841 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 00842 telemetry[12] = (uint8_t)(crc16&0x00FF); 00843 for(uint8_t i=13;i<134;i++) 00844 { 00845 telemetry[i]=0x00; 00846 } 00847 break; 00848 } 00849 } 00850 break; 00851 } 00852 case 0x80: 00853 { 00854 gpc.printf("Function Management Service\r\n");//changed 00855 uint8_t service_subtype=(tc[2]&0x0F); 00856 switch(service_subtype) 00857 { 00858 case 0x01: 00859 { 00860 gpc.printf("\n\rFMS Activated"); 00861 uint8_t fid=tc[3];//changed from pid to fid 00862 switch(fid) 00863 { 00864 case 0xE0: 00865 { 00866 //float mag_data_comm[3]={uint16_to_float(-1000,1000,ACS_MM_X_COMSN),uint16_to_float(-1000,1000,ACS_MM_Y_COMSN),uint16_to_float(-1000,1000,ACS_MM_Z_COMSN)}; 00867 //float gyro_data_comm[3]={uint16_to_float(-5000,5000,ACS_MG_X_COMSN),uint16_to_float(-5000,5000,ACS_MG_Y_COMSN),uint16_to_float(-5000,5000,ACS_MG_Z_COMSN)}; 00868 float mag_data_comm[3] = {ACS_MM_X_COMSN,ACS_MM_Y_COMSN,ACS_MM_Z_COMSN}; 00869 float gyro_data_comm[3] = {ACS_MG_X_COMSN,ACS_MG_Y_COMSN,ACS_MG_Z_COMSN}; 00870 float moment_comm[3]; 00871 gpc.printf("ACS_COMSN SOFTWARE\r\n"); 00872 //ACK_L234_telemetry 00873 ACS_STATE = tc[4]; 00874 if(ACS_STATE == 2) // Nominal mode 00875 { 00876 gpc.printf("\n\r Nominal mode \n"); 00877 FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x01,0x00,ACS_DETUMBLING_ALGO_TYPE); 00878 gpc.printf("\n\r Moment values returned by control algo \n"); 00879 for(int i=0; i<3; i++) 00880 { 00881 gpc.printf("%f\t",moment_comm[i]); 00882 } 00883 00884 } 00885 else if(ACS_STATE == 3) // Auto Control 00886 { 00887 gpc.printf("\n\r Auto control mode \n"); 00888 FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x00,0x00,ACS_DETUMBLING_ALGO_TYPE); 00889 gpc.printf("\n\r Moment values returned by control algo \n"); 00890 for(int i=0; i<3; i++) 00891 { 00892 gpc.printf("%f\t",moment_comm[i]); 00893 } 00894 } 00895 else if(ACS_STATE == 4) // Detumbling 00896 { 00897 FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x00,0x01,ACS_DETUMBLING_ALGO_TYPE); 00898 gpc.printf("\n\r Moment values returned by control algo \n"); 00899 for(int i=0; i<3; i++) 00900 { 00901 gpc.printf("%f\t",moment_comm[i]); 00902 } 00903 } 00904 else 00905 { 00906 ACS_STATUS = 7; 00907 } 00908 00909 // Control algo commissioning 00910 uint16_t moment_ret; 00911 telemetry[0] = 0x78; 00912 telemetry[1] = tc[0]; 00913 telemetry[2] = ACK_CODE; 00914 telemetry[3] = 0x00; 00915 telemetry[4] = ACS_STATUS; 00916 moment_ret = float_to_uint16(-2.2,2.2,moment_comm[0]); 00917 telemetry[5] = (uint8_t)(moment_ret>>8); 00918 telemetry[6] = (uint8_t)moment_ret; 00919 moment_ret = float_to_uint16(-2.2,2.2,moment_comm[0]); 00920 telemetry[7] = (uint8_t)(moment_ret>>8); 00921 telemetry[8] = (uint8_t)moment_ret; 00922 moment_ret = float_to_uint16(-2.2,2.2,moment_comm[2]); 00923 telemetry[9] = (uint8_t)(moment_ret>>8); 00924 telemetry[10] = (uint8_t)moment_ret; 00925 00926 //FCTN_CONVERT_FLOAT(moment_comm[0],&telemetry[4]); //telemetry[4] - telemetry[7] 00927 //FCTN_CONVERT_FLOAT(moment_comm[1],&telemetry[8]); //telemetry[8] - telemetry[11] 00928 //FCTN_CONVERT_FLOAT(moment_comm[2],&telemetry[12]); //telemetry[12] - telemetry[15] 00929 // to include commission TR as well 00930 for(uint8_t i=11;i<132;i++) 00931 { 00932 telemetry[i]=0x00; 00933 } 00934 crc16 = CRC::crc16_gen(telemetry,132); 00935 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8); 00936 telemetry[133] = (uint8_t)(crc16&0x00FF); 00937 break; 00938 } 00939 case 0xE1: 00940 { 00941 gpc.printf("HARDWARE_COMSN\r\n"); 00942 //ACK_L234_telemetry 00943 uint8_t SENSOR_NO; 00944 00945 int init1=0; 00946 int init2=0; 00947 int data1=0; 00948 int data2=0; 00949 00950 uint16_t assign_val; 00951 00952 float PWM_tc[3]; 00953 PWM_tc[0]=(float(tc[4]))/256; 00954 PWM_tc[1]=(float(tc[5]))/256; 00955 PWM_tc[2]=(float(tc[6]))/256; 00956 00957 DRV_Z_EN = 1; 00958 DRV_XY_EN = 1; 00959 telemetry[0]=0x78; 00960 telemetry[1]=tc[0]; 00961 telemetry[2]=ACK_CODE; 00962 telemetry[3] = 0x00; 00963 00964 SENSOR_NO = 0; 00965 00966 PWM1 = 0; 00967 PWM2 = 0; 00968 PWM3 = 0; 00969 00970 wait_ms(ACS_DEMAG_TIME_DELAY); 00971 ATS2_SW_ENABLE = 1; 00972 wait_ms(5); 00973 ATS1_SW_ENABLE = 1; 00974 wait_ms(5); 00975 //will it lead to causing delay in i2c interrupt 00976 init1 = SENSOR_INIT(); 00977 if( init1 == 1) 00978 { 00979 data1 = SENSOR_DATA_ACQ(); 00980 } 00981 ATS1_SW_ENABLE = 0; 00982 wait_ms(5); 00983 ATS2_SW_ENABLE = 0; 00984 wait_ms(5); 00985 00986 if(data1 == 0) 00987 { 00988 ATS2_SW_ENABLE = 0; 00989 wait_ms(5); 00990 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; 00991 } 00992 else if(data1==1) 00993 { 00994 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; 00995 } 00996 else if(data1==2) 00997 { 00998 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20; 00999 } 01000 else if(data1==3) 01001 { 01002 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x30; 01003 } 01004 01005 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]); 01006 telemetry[5] = (assign_val>>8); 01007 telemetry[6] = assign_val; 01008 01009 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]); 01010 telemetry[7] = (assign_val>>8); 01011 telemetry[8] = assign_val; 01012 01013 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]); 01014 telemetry[9] = (assign_val>>8); 01015 telemetry[10] = assign_val; 01016 01017 assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[0]); 01018 telemetry[11] = (assign_val>>8); 01019 telemetry[12] = assign_val; 01020 01021 assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[1]); 01022 telemetry[13] = (assign_val>>8); 01023 telemetry[14] = assign_val; 01024 01025 assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[2]); 01026 telemetry[15] = (assign_val>>8); 01027 telemetry[16] = assign_val; 01028 01029 init2 = SENSOR_INIT(); 01030 if( init2 == 1) 01031 { 01032 data2 = SENSOR_DATA_ACQ(); 01033 } 01034 //uint8_t ats_data=1; 01035 01036 if(data2 == 0) 01037 { 01038 ATS2_SW_ENABLE = 1; 01039 wait_ms(5); 01040 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; 01041 SENSOR_NO = 0; 01042 if(data1 == 2) 01043 { 01044 ATS1_SW_ENABLE = 1; 01045 wait_ms(5); 01046 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; 01047 SENSOR_NO = 1; 01048 } 01049 else if(data1 == 3) 01050 { 01051 ATS1_SW_ENABLE = 1; 01052 wait_ms(5); 01053 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07; 01054 SENSOR_NO = 1; 01055 } 01056 else if(data1 == 1) 01057 { 01058 ATS1_SW_ENABLE = 1; 01059 wait_ms(5); 01060 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; 01061 SENSOR_NO = 0; 01062 01063 } 01064 01065 } 01066 else if(data2==1) 01067 { 01068 if(data1 == 2) 01069 { 01070 ATS2_SW_ENABLE = 1; 01071 wait_ms(5); 01072 ATS1_SW_ENABLE = 1; 01073 wait_ms(5); 01074 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; 01075 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; 01076 SENSOR_NO = 1; 01077 } 01078 else if(data1 == 3) 01079 { 01080 ATS2_SW_ENABLE = 1; 01081 wait_ms(5); 01082 ATS1_SW_ENABLE = 1; 01083 wait_ms(5); 01084 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; 01085 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70; 01086 SENSOR_NO = 1; 01087 } 01088 else 01089 { 01090 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; 01091 SENSOR_NO = 0; 01092 //ats_data = 0; 01093 } 01094 } 01095 01096 else if(data2==2) 01097 { 01098 if(data1 == 3) 01099 { 01100 ATS2_SW_ENABLE = 1; 01101 wait_ms(5); 01102 ATS1_SW_ENABLE = 1; 01103 wait_ms(5); 01104 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02; 01105 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70; 01106 SENSOR_NO = 1; 01107 } 01108 else 01109 { 01110 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; 01111 SENSOR_NO = 2; 01112 } 01113 } 01114 else if(data2==3) 01115 { 01116 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07; 01117 SENSOR_NO = 2; 01118 } 01119 01120 SENSOR_NO = 1; 01121 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]); 01122 telemetry[17] = (assign_val>>8); 01123 telemetry[18] = assign_val; 01124 01125 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]); 01126 telemetry[19] = (assign_val>>8); 01127 telemetry[20] = assign_val; 01128 01129 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]); 01130 telemetry[21] = (assign_val>>8); 01131 telemetry[22] = assign_val; 01132 01133 assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[0]); 01134 telemetry[23] = (assign_val>>8); 01135 telemetry[24] = assign_val; 01136 01137 assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[1]); 01138 telemetry[25] = (assign_val>>8); 01139 telemetry[26] = assign_val; 01140 01141 assign_val = float_to_uint16(-5000,5000,actual_data.AngularSpeed_actual[2]); 01142 telemetry[27] = (assign_val>>8); 01143 telemetry[28] = assign_val; 01144 01145 telemetry[4] = ACS_ATS_STATUS; 01146 01147 01148 SelectLineb3 =0; 01149 SelectLineb2 =1; 01150 SelectLineb1 =0; 01151 SelectLineb0 =1; 01152 int resistance; 01153 PWM1 = PWM_tc[0]; 01154 PWM2 = 0; 01155 PWM3 = 0; 01156 01157 wait_ms(ACS_DEMAG_TIME_DELAY); 01158 if(SENSOR_NO != 0) 01159 SENSOR_DATA_ACQ(); 01160 actual_data.current_actual[5]=CurrentInput.read(); 01161 actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens); 01162 01163 resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]); 01164 if(actual_data.current_actual[5]>1.47) 01165 { 01166 actual_data.current_actual[5]=3694/log(24.032242*resistance); 01167 } 01168 else 01169 { 01170 actual_data.current_actual[5]=3365.4/log(7.60573*resistance); 01171 } 01172 01173 //to be edited final tele 01174 //uint16_t assign_val; 01175 assign_val = float_to_uint16(-100,100,actual_data.current_actual[5]);//assuming max min values for current to be diss 01176 telemetry[29] = (assign_val>>8); 01177 telemetry[30] = assign_val; 01178 01179 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]); 01180 telemetry[31] = (assign_val>>8); 01181 telemetry[32] = assign_val; 01182 01183 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]); 01184 telemetry[33] = (assign_val>>8); 01185 telemetry[34] = assign_val; 01186 01187 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]); 01188 telemetry[35] = (assign_val>>8); 01189 telemetry[36] = assign_val; 01190 01191 PWM1 = 0; 01192 PWM2 = PWM_tc[1]; 01193 PWM3 = 0; 01194 01195 wait_ms(ACS_DEMAG_TIME_DELAY); 01196 01197 if(SENSOR_NO != 0) 01198 SENSOR_DATA_ACQ(); 01199 actual_data.current_actual[5]=CurrentInput.read(); 01200 actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens); 01201 01202 01203 resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]); 01204 if(actual_data.current_actual[5]>1.47) 01205 { 01206 actual_data.current_actual[5]=3694/log(24.032242*resistance); 01207 } 01208 else 01209 { 01210 actual_data.current_actual[5]=3365.4/log(7.60573*resistance); 01211 } 01212 01213 assign_val = float_to_uint16(-100,100,actual_data.current_actual[5]);//assuming max min values for current to be diss 01214 telemetry[37] = (assign_val>>8); 01215 telemetry[38] = assign_val; 01216 01217 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]); 01218 telemetry[39] = (assign_val>>8); 01219 telemetry[40] = assign_val; 01220 01221 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]); 01222 telemetry[41] = (assign_val>>8); 01223 telemetry[42] = assign_val; 01224 01225 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]); 01226 telemetry[43] = (assign_val>>8); 01227 telemetry[44] = assign_val; 01228 01229 PWM1 = 0; 01230 PWM2 = 0; 01231 PWM3 = PWM_tc[2]; 01232 01233 wait_ms(ACS_DEMAG_TIME_DELAY); 01234 01235 if(SENSOR_NO != 0) 01236 SENSOR_DATA_ACQ(); 01237 actual_data.current_actual[5]=CurrentInput.read(); 01238 actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens); 01239 01240 resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]); 01241 if(actual_data.current_actual[5]>1.47) 01242 { 01243 actual_data.current_actual[5]=3694/log(24.032242*resistance); 01244 } 01245 else 01246 { 01247 actual_data.current_actual[5]=3365.4/log(7.60573*resistance); 01248 } 01249 assign_val = float_to_uint16(-100,100,actual_data.current_actual[5]);//assuming max min values for current to be diss 01250 telemetry[45] = (assign_val>>8); 01251 telemetry[46] = assign_val; 01252 01253 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]); 01254 telemetry[47] = (assign_val>>8); 01255 telemetry[48] = assign_val; 01256 01257 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]); 01258 telemetry[49] = (assign_val>>8); 01259 telemetry[50] = assign_val; 01260 01261 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]); 01262 telemetry[51] = (assign_val>>8); 01263 telemetry[52] = assign_val; 01264 01265 PWM1 = 0; 01266 PWM2 = 0; 01267 PWM3 = 0; 01268 01269 wait_ms(ACS_DEMAG_TIME_DELAY); 01270 01271 if(SENSOR_NO != 0) 01272 SENSOR_DATA_ACQ(); 01273 actual_data.current_actual[5]=CurrentInput.read(); 01274 actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens); 01275 01276 resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]); 01277 if(actual_data.current_actual[5]>1.47) 01278 { 01279 actual_data.current_actual[5]=3694/log(24.032242*resistance); 01280 } 01281 else 01282 { 01283 actual_data.current_actual[5]=3365.4/log(7.60573*resistance); 01284 } 01285 01286 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[0]); 01287 telemetry[53] = (assign_val>>8); 01288 telemetry[54] = assign_val; 01289 01290 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[1]); 01291 telemetry[55] = (assign_val>>8); 01292 telemetry[56] = assign_val; 01293 01294 assign_val = float_to_uint16(-1000,1000,actual_data.Bvalue_actual[2]); 01295 telemetry[57] = (assign_val>>8); 01296 telemetry[58] = assign_val; 01297 01298 // for(int i=0; i<12; i++) 01299 // FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&telemetry[16 + (i*4)]); 01300 01301 // FCTN_ATS_DATA_ACQ(); //get data 01302 01303 // to include commission TR as well 01304 01305 telemetry[59] = SENSOR_NO; 01306 01307 for(uint8_t i=60;i<132;i++) 01308 { 01309 telemetry[i]=0x00; 01310 } 01311 01312 crc16 = CRC::crc16_gen(telemetry,132); 01313 telemetry[132] = (uint8_t)((crc16&0xFF00)>>8); 01314 telemetry[133] = (uint8_t)(crc16&0x00FF); 01315 01316 if(data1 == 0) 01317 { 01318 ATS1_SW_ENABLE = 0; 01319 //wait_ms(5); 01320 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; 01321 } 01322 else if(data1==1) 01323 { 01324 ATS1_SW_ENABLE = 1; 01325 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x40; 01326 } 01327 else if(data1==2) 01328 { 01329 ATS1_SW_ENABLE = 1; 01330 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; 01331 } 01332 else if(data1==3) 01333 { 01334 ATS1_SW_ENABLE = 1; 01335 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70; 01336 } 01337 printf("Exited HW\r\n"); 01338 break; 01339 } 01340 case 0xE2: 01341 { //BCN_SW active high - samp 01342 uint8_t BCN_SPND_STATE; 01343 BCN_SPND_STATE=tc[4]; 01344 if(BCN_SPND_STATE==0x00)// to switch on bcn 01345 { 01346 BCN_SPND_TX=0; 01347 if( BCN_TX_SW_STATUS == 3 ) 01348 { 01349 BCN_SW = 1; 01350 BCN_TX_SW_STATUS = 1; 01351 } 01352 //stop BCN_STANDBY_TIMER.stop();//create 01353 telemetry[2]=0xA0; 01354 } 01355 else if(BCN_SPND_STATE==0x01) 01356 { 01357 FCTN_BCN_SPND_TX();//direct implementation of function 01358 /*{ 01359 //printf("BCN_SPND\n\r"); 01360 BCN_SPND_TX = 1; 01361 if( BCN_TX_MAIN_STATUS == 1 && BCN_TX_SW_STATUS == 1 ) 01362 { 01363 writereg(RF22_REG_07_OPERATING_MODE1,0x00); //standby mode 01364 if( readreg(RF22_REG_07_OPERATING_MODE1) & 0x08 == 0x08 ) 01365 { 01366 BCN_SW = 0; 01367 BCN_TX_SW_STATUS = 3; 01368 uint32_t FLASH_DATA; 01369 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0); //updated in flash 01370 FLASH_DATA = (FLASH_DATA | 0x00006000); 01371 FCTN_BAE_WR_FLASH(0,FLASH_DATA); 01372 } 01373 }*/ 01374 //} 01375 //BCN_SW =1; 01376 //stop BCN_STANDBY_TIMER.start();//create 01377 if(BCN_TX_MAIN_STATUS==0) 01378 { 01379 telemetry[2]=0xA0; 01380 } 01381 else if(BCN_TX_MAIN_STATUS==1) 01382 { 01383 telemetry[2]=0xA0; //changed 01384 } 01385 } 01386 else 01387 { 01388 telemetry[2]=0x02; 01389 } 01390 //ACK_L234_telemetry 01391 telemetry[0]=0xB0; 01392 telemetry[1]=tc[0]; 01393 //ack_code taken care above 01394 for(uint8_t i=3;i<11;i++) 01395 { 01396 telemetry[i]=0x00; 01397 } 01398 crc16 = CRC::crc16_gen(telemetry,11); 01399 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01400 telemetry[12] = (uint8_t)(crc16&0x00FF); 01401 for(uint8_t i=13;i<134;i++) 01402 { 01403 telemetry[i]=0x00; 01404 } 01405 break; 01406 } 01407 case 0xE3: 01408 { 01409 gpc.printf("\n\n\rE3 batter heater\n"); 01410 01411 if(EPS_BTRY_HTR_AUTO != 0) 01412 telemetry[2]=0x87; 01413 else 01414 { 01415 HTR_CYCLE_COUNTS = tc[4]; 01416 if(HTR_CYCLE_COUNTS==0x00) 01417 { 01418 BTRY_HTR_ENABLE = 0; 01419 HTR_CYCLE->stop(); 01420 //clear EPS_BTRY_HTR is it 01421 EPS_BTRY_HTR_AUTO = 0; 01422 gpc.printf("\n\n\rheater OFF\n"); 01423 01424 } 01425 else 01426 { 01427 gpc.printf("\n\n\rheater ON\n"); 01428 01429 if(HTR_CYCLE_COUNTS != 0xFF) 01430 { 01431 HTR_CYCLE_COUNTER = 0; 01432 } 01433 //uint8_t HTR_CYCLE_START_DLY = tc[5]; 01434 HTR_CYCLE_START_DLY = tc[5]; 01435 HTR_ON_DURATION = tc[6]; 01436 01437 //make it uint16_t 01438 HTR_CYCLE_PERIOD = (tc[7]<<8) | tc[8]; 01439 //start BTRY_HTR_DLY_TIMER; 01440 } 01441 telemetry[2]=0xA0; 01442 } 01443 //ACK_L234_telemetry 01444 telemetry[0]=0xB0; 01445 telemetry[1]=tc[0]; 01446 //ACK code taken care of 01447 for(uint8_t i=3;i<11;i++) 01448 { 01449 telemetry[i]=0x00; 01450 } 01451 crc16 = CRC::crc16_gen(telemetry,11); 01452 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01453 telemetry[12] = (uint8_t)(crc16&0x00FF); 01454 for(uint8_t i=13;i<134;i++) 01455 { 01456 telemetry[i]=0x00; 01457 } 01458 01459 break; 01460 } 01461 case 0x01: 01462 { if(BAE_STANDBY==0x00) 01463 { 01464 gpc.printf("\n\n\rRun P_EPS_INIT\n"); 01465 FCTN_EPS_INIT(); 01466 telemetry[2]=ACK_CODE; 01467 } 01468 else 01469 { 01470 gpc.printf("\n\runable to Run P_EPS_INIT as BAE_STATUS not 000 ");; 01471 telemetry[2]=0x87; 01472 } 01473 01474 //ACK_L234_telemetry 01475 telemetry[0]=0xB0; 01476 telemetry[1]=tc[0]; 01477 //ACK code taken care of 01478 for(uint8_t i=3;i<11;i++) 01479 { 01480 telemetry[i]=0x00; 01481 } 01482 crc16 = CRC::crc16_gen(telemetry,11); 01483 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01484 telemetry[12] = (uint8_t)(crc16&0x00FF); 01485 for(uint8_t i=13;i<134;i++) 01486 { 01487 telemetry[i]=0x00; 01488 } 01489 01490 break; 01491 } 01492 case 0x02: 01493 { 01494 if(BAE_STANDBY==0x00) 01495 { 01496 gpc.printf("\n\n\rRun P_EPS_MAIN\n"); 01497 F_EPS(); 01498 telemetry[2]=ACK_CODE; 01499 } 01500 else 01501 { 01502 gpc.printf("\n\runable to Run P_EPS_MAIN as BAE_STATUS not 000 ");; 01503 telemetry[2]=0x87; 01504 } 01505 01506 //ACK_L234_telemetry 01507 telemetry[0]=0xB0; 01508 telemetry[1]=tc[0]; 01509 //ACK code taken care of 01510 for(uint8_t i=3;i<11;i++) 01511 { 01512 telemetry[i]=0x00; 01513 } 01514 crc16 = CRC::crc16_gen(telemetry,11); 01515 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01516 telemetry[12] = (uint8_t)(crc16&0x00FF); 01517 for(uint8_t i=13;i<134;i++) 01518 { 01519 telemetry[i]=0x00; 01520 } 01521 break; 01522 } 01523 case 0x03: 01524 { 01525 if(BAE_STANDBY==0x00) 01526 { 01527 gpc.printf("\n\n\rRun P_ACS_INIT\n"); 01528 FCTN_ACS_INIT(); 01529 telemetry[2]=ACK_CODE; 01530 } 01531 else 01532 { 01533 gpc.printf("\n\runable to Run P_ACS_INIT as BAE_STATUS not 000 ");; 01534 telemetry[2]=0x87; 01535 } 01536 01537 //ACK_L234_telemetry 01538 telemetry[0]=0xB0; 01539 telemetry[1]=tc[0]; 01540 //ACK CODE TAKEN CARE OF 01541 for(uint8_t i=3;i<11;i++) 01542 { 01543 telemetry[i]=0x00; 01544 } 01545 crc16 = CRC::crc16_gen(telemetry,11); 01546 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01547 telemetry[12] = (uint8_t)(crc16&0x00FF); 01548 for(uint8_t i=13;i<134;i++) 01549 { 01550 telemetry[i]=0x00; 01551 } 01552 break; 01553 } 01554 case 0x05: 01555 { 01556 if(BAE_STANDBY==0x00) 01557 { 01558 gpc.printf("\n\n\rRun P_ACS_MAIN\n"); 01559 F_ACS(); 01560 telemetry[2]=ACK_CODE; 01561 } 01562 else 01563 { 01564 gpc.printf("\n\runable to Run P_ACS_MAIN as BAE_STATUS not 000 ");; 01565 telemetry[2]=0x87; 01566 } 01567 01568 //ACK_L234_TM 01569 telemetry[0]=0xB0; 01570 telemetry[1]=tc[0]; 01571 //ACK CODE TAKEN CARE OF 01572 for(uint8_t i=3;i<11;i++) 01573 { 01574 telemetry[i]=0x00; 01575 } 01576 01577 crc16 = CRC::crc16_gen(telemetry,11); 01578 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01579 telemetry[12] = (uint8_t)(crc16&0x00FF); 01580 for(uint8_t i=13;i<134;i++) 01581 { 01582 telemetry[i]=0x00; 01583 } 01584 break; 01585 } 01586 case 0x06: 01587 { 01588 if(BAE_STANDBY==0x00) 01589 { 01590 gpc.printf("\n\n\rRun P_BCN_INIT\n"); 01591 // F_BCN(); why was this written? 01592 01593 FCTN_BCN_INIT(); // does this makes sense?? 01594 telemetry[2]=ACK_CODE; 01595 } 01596 else 01597 { 01598 gpc.printf("\n\runable to Run P_BCN_INIT as BAE_STATUS not 000 ");; 01599 telemetry[2]=0x87; 01600 } 01601 01602 //ACK_L234_TM 01603 telemetry[0]=0xB0; 01604 telemetry[1]=tc[0]; 01605 //ACK CODE TAKEN CARE OF 01606 for(uint8_t i=3;i<11;i++) 01607 { 01608 telemetry[i]=0x00; 01609 } 01610 crc16 = CRC::crc16_gen(telemetry,11); 01611 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01612 telemetry[12] = (uint8_t)(crc16&0x00FF); 01613 for(uint8_t i=13;i<134;i++) 01614 { 01615 telemetry[i]=0x00; 01616 } 01617 break; 01618 } 01619 case 0x07: 01620 { 01621 if(BAE_STANDBY==0x00) 01622 { 01623 gpc.printf("\n\n\rRun P_BCN_TX_MAIN\n"); 01624 FCTN_BCN_TX_MAIN();//correct function check once 01625 telemetry[2]=ACK_CODE; 01626 } 01627 else 01628 { 01629 gpc.printf("\n\runable to Run P_BCN_TX_MAIN as BAE_STATUS not 000 ");; 01630 telemetry[2]=0x87; 01631 } 01632 01633 //ACK_L234_TM 01634 telemetry[0]=0xB0; 01635 telemetry[1]=tc[0]; 01636 //ACK CODE TAKEN CARE OF 01637 for(uint8_t i=3;i<11;i++) 01638 { 01639 telemetry[i]=0x00; 01640 } 01641 crc16 = CRC::crc16_gen(telemetry,11); 01642 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01643 telemetry[12] = (uint8_t)(crc16&0x00FF); 01644 for(uint8_t i=13;i<134;i++) 01645 { 01646 telemetry[i]=0x00; 01647 } 01648 break; 01649 } 01650 case 0x11: 01651 { 01652 gpc.printf("\n\n\rSW_ON_ACS_ATS1_SW_ENABLE\n"); 01653 //ACK_L234_TM 01654 telemetry[0]=0xB0; 01655 telemetry[1]=tc[0]; 01656 //____________________________________________************************************ 01657 /* 01658 ATS PIN OR STATUS YET TO BE DECIDED. DECIDED THAT IT IS PIN TC CAN SWITCH ON/OFF THE SENSOR 01659 */ 01660 ATS2_SW_ENABLE = 0; // making sure we switch off the other 01661 //ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF3) | 0x0C ; 01662 ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF0) | 0x0C ; 01663 ATS1_SW_ENABLE = 1; // what should the signal be for unabling 0 or 1 01664 //ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x3F); 01665 ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x0F);// state now 00|0x40; //changed device status 01666 telemetry[2]=ACK_CODE; 01667 01668 /*update in flash here*/ 01669 uint32_t FLASH_DATA; 01670 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0); 01671 FLASH_DATA = (FLASH_DATA & 0x3FFFFFFF); 01672 FCTN_BAE_WR_FLASH(0,FLASH_DATA); 01673 01674 01675 for(uint8_t i=3;i<11;i++) 01676 { 01677 telemetry[i]=0x00; 01678 } 01679 crc16 = CRC::crc16_gen(telemetry,11); 01680 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01681 telemetry[12] = (uint8_t)(crc16&0x00FF); 01682 for(uint8_t i=13;i<134;i++) 01683 { 01684 telemetry[i]=0x00; 01685 } 01686 break; 01687 } 01688 case 0x12: 01689 { 01690 gpc.printf("\n\rSW_ON_ACS_ATS2_SW_ENABLE"); 01691 //ACK_L234_TM 01692 telemetry[0]=0xB0; 01693 telemetry[1]=tc[0]; 01694 ATS1_SW_ENABLE = 0; //make sure u switch off the other 01695 ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x0F) | 0xC0 ; 01696 ATS2_SW_ENABLE = 1; 01697 ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF0);//|0x04; changed here 01698 01699 /*update in flash here*/ 01700 uint32_t FLASH_DATA; 01701 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0); 01702 FLASH_DATA = (FLASH_DATA & 0xF3FFFFFF); 01703 FCTN_BAE_WR_FLASH(0,FLASH_DATA); 01704 01705 01706 telemetry[2]=ACK_CODE; 01707 for(uint8_t i=3;i<11;i++) 01708 { 01709 telemetry[i]=0x00; 01710 } 01711 crc16 = CRC::crc16_gen(telemetry,11); 01712 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01713 telemetry[12] = (uint8_t)(crc16&0x00FF); 01714 for(uint8_t i=13;i<134;i++) 01715 { 01716 telemetry[i]=0x00; 01717 } 01718 break; 01719 } 01720 case 0x13: 01721 { 01722 gpc.printf("\n\n\rSW_ON_ACS_TR_XY_ENABLE\n"); 01723 //ACK_L234_TM 01724 telemetry[0]=0xB0; 01725 telemetry[1]=tc[0]; 01726 DRV_XY_EN = 1;//1 SWITCH enable here 01727 ACS_TR_XY_ENABLE = 1; 01728 ACS_TR_XY_SW_STATUS=0x01; 01729 01730 /*update in flash here*/ 01731 uint32_t FLASH_DATA; 01732 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0); 01733 FLASH_DATA = ((FLASH_DATA & 0xFF3FFFFF) | 0x00400000 ) ; 01734 FCTN_BAE_WR_FLASH(0,FLASH_DATA); 01735 01736 01737 telemetry[2]=ACK_CODE; 01738 for(uint8_t i=3;i<11;i++) 01739 { 01740 telemetry[i]=0x00; 01741 } 01742 crc16 = CRC::crc16_gen(telemetry,11); 01743 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01744 telemetry[12] = (uint8_t)(crc16&0x00FF); 01745 for(uint8_t i=13;i<134;i++) 01746 { 01747 telemetry[i]=0x00; 01748 } 01749 break; 01750 } 01751 case 0x14: 01752 { 01753 gpc.printf("\n\n\rSW_ON_ACS_TR_Z_ENABLE\n"); 01754 //ACK_L234_TM 01755 telemetry[0]=0xB0; 01756 telemetry[1]=tc[0]; 01757 DRV_Z_EN = 1; 01758 ACS_TR_Z_ENABLE = 1; 01759 ACS_TR_Z_SW_STATUS=0x01; 01760 01761 /*update in flash here*/ 01762 uint32_t FLASH_DATA; 01763 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0); 01764 FLASH_DATA = ((FLASH_DATA & 0xFFCFFFFF) | 0x00100000 ) ; 01765 FCTN_BAE_WR_FLASH(0,FLASH_DATA); 01766 01767 01768 telemetry[2]=ACK_CODE; 01769 for(uint8_t i=3;i<11;i++) 01770 { 01771 telemetry[i]=0x00; 01772 } 01773 crc16 = CRC::crc16_gen(telemetry,11); 01774 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01775 telemetry[12] = (uint8_t)(crc16&0x00FF); 01776 for(uint8_t i=13;i<134;i++) 01777 { 01778 telemetry[i]=0x00; 01779 } 01780 break; 01781 } 01782 case 0x15: 01783 { 01784 gpc.printf("\n\n\rSW_ON_BCN_TX_SW_ENABLE\n"); 01785 //ACK_L234_TM 01786 telemetry[0]=0xB0; 01787 telemetry[1]=tc[0]; 01788 BCN_SW = 1;//here 1 is switch enable 5th dec 01789 BCN_TX_SW_STATUS=0x01;//BCN_TX_SW_ENABLE CHANGED TO BCN_TX_SW_STATUS 01790 01791 /*update in flash here*/ 01792 uint32_t FLASH_DATA; 01793 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0); 01794 FLASH_DATA = ((FLASH_DATA & 0xFFFF9FFF) | 0x00002000 ) ; 01795 FCTN_BAE_WR_FLASH(0,FLASH_DATA); 01796 01797 01798 telemetry[2]=ACK_CODE; 01799 for(uint8_t i=3;i<11;i++) 01800 { 01801 telemetry[i]=0x00; 01802 } 01803 crc16 = CRC::crc16_gen(telemetry,11); 01804 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01805 telemetry[12] = (uint8_t)(crc16&0x00FF); 01806 for(uint8_t i=13;i<134;i++) 01807 { 01808 telemetry[i]=0x00; 01809 } 01810 break; 01811 } 01812 case 0x21: 01813 { 01814 gpc.printf("\n\n\rSW_OFF_ACS_ATS1_SW_ENABLE\n"); 01815 //ACK_L234_TM 01816 telemetry[0]=0xB0; 01817 telemetry[1]=tc[0]; 01818 ATS1_SW_ENABLE = 0; 01819 ACS_ATS_STATUS = (ACS_ATS_STATUS & 0x3F) | 0xC0 ; 01820 telemetry[2]=ACK_CODE; 01821 01822 /*update in flaash here*/ 01823 uint32_t FLASH_DATA; 01824 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0); 01825 FLASH_DATA = (FLASH_DATA | 0xC0000000 ) ; 01826 FCTN_BAE_WR_FLASH(0,FLASH_DATA); 01827 01828 01829 for(uint8_t i=3;i<11;i++) 01830 { 01831 telemetry[i]=0x00; 01832 } 01833 crc16 = CRC::crc16_gen(telemetry,11); 01834 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01835 telemetry[12] = (uint8_t)(crc16&0x00FF); 01836 for(uint8_t i=13;i<134;i++) 01837 { 01838 telemetry[i]=0x00; 01839 } 01840 break; 01841 } 01842 case 0x22: 01843 { 01844 gpc.printf("\n\rSW_OFF_ACS_ATS2_SW_ENABLE"); 01845 //ACK_L234_TM 01846 telemetry[0]=0xB0; 01847 telemetry[1]=tc[0]; 01848 ATS2_SW_ENABLE = 0; 01849 01850 uint32_t FLASH_DATA; 01851 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0); 01852 FLASH_DATA = (FLASH_DATA | 0x0C000000); 01853 FCTN_BAE_WR_FLASH(0,FLASH_DATA); 01854 01855 ACS_ATS_STATUS = (ACS_ATS_STATUS & 0xF3) | 0x0C ; 01856 telemetry[2]=ACK_CODE; 01857 for(uint8_t i=3;i<11;i++) 01858 { 01859 telemetry[i]=0x00; 01860 } 01861 crc16 = CRC::crc16_gen(telemetry,11); 01862 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01863 telemetry[12] = (uint8_t)(crc16&0x00FF); 01864 for(uint8_t i=13;i<134;i++) 01865 { 01866 telemetry[i]=0x00; 01867 } 01868 break; 01869 } 01870 case 0x23: 01871 { 01872 gpc.printf("\n\n\rSW_OFF_ACS_TR_XY_ENABLE\n"); 01873 //ACK_L234_TM 01874 telemetry[0]=0xB0; 01875 telemetry[1]=tc[0]; 01876 ACS_TR_XY_ENABLE = 0; 01877 ACS_TR_XY_SW_STATUS=0x03; 01878 01879 /*update in flash here*/ 01880 uint32_t FLASH_DATA; 01881 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0); 01882 FLASH_DATA = (FLASH_DATA | 0x00C00000 ) ; 01883 FCTN_BAE_WR_FLASH(0,FLASH_DATA); 01884 01885 01886 telemetry[2]=ACK_CODE; 01887 for(uint8_t i=3;i<11;i++) 01888 { 01889 telemetry[i]=0x00; 01890 } 01891 crc16 = CRC::crc16_gen(telemetry,11); 01892 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01893 telemetry[12] = (uint8_t)(crc16&0x00FF); 01894 for(uint8_t i=13;i<134;i++) 01895 { 01896 telemetry[i]=0x00; 01897 } 01898 break; 01899 } 01900 case 0x24: 01901 { 01902 gpc.printf("\n\n\rSW_OFF_ACS_TR_Z_ENABLE\n"); 01903 //ACK_L234_TM 01904 telemetry[0]=0xB0; 01905 telemetry[1]=tc[0]; 01906 ACS_TR_Z_ENABLE = 0; 01907 ACS_TR_Z_SW_STATUS=0x03; 01908 01909 /*update in flash here*/ 01910 uint32_t FLASH_DATA; 01911 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0); 01912 FLASH_DATA = (FLASH_DATA | 0x00300000 ) ; 01913 FCTN_BAE_WR_FLASH(0,FLASH_DATA); 01914 01915 01916 telemetry[2]=ACK_CODE; 01917 for(uint8_t i=3;i<11;i++) 01918 { 01919 telemetry[i]=0x00; 01920 } 01921 crc16 = CRC::crc16_gen(telemetry,11); 01922 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01923 telemetry[12] = (uint8_t)(crc16&0x00FF); 01924 for(uint8_t i=13;i<134;i++) 01925 { 01926 telemetry[i]=0x00; 01927 } 01928 break; 01929 } 01930 case 0x25: 01931 { 01932 gpc.printf("\n\n\rSW_OFF_BCN_TX_SW_ENABLE\n"); 01933 //ACK_L234_TM 01934 telemetry[0]=0xB0; 01935 telemetry[1]=tc[0]; 01936 BCN_SW = 0;// same convention active high 5th dec 01937 BCN_TX_SW_ENABLE=0x03; 01938 01939 /*update in the flash*/ 01940 uint32_t FLASH_DATA; 01941 FLASH_DATA = FCTN_BAE_RD_FLASH_ENTITY(0); 01942 FLASH_DATA = (FLASH_DATA | 0x00006000 ) ; 01943 FCTN_BAE_WR_FLASH(0,FLASH_DATA); 01944 01945 01946 telemetry[2]=ACK_CODE; 01947 for(uint8_t i=3;i<11;i++) 01948 { 01949 telemetry[i]=0x00; 01950 } 01951 crc16 = CRC::crc16_gen(telemetry,11); 01952 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01953 telemetry[12] = (uint8_t)(crc16&0x00FF); 01954 for(uint8_t i=13;i<134;i++) 01955 { 01956 telemetry[i]=0x00; 01957 } 01958 break; 01959 } 01960 case 0x31: 01961 { 01962 gpc.printf("\n\n\rACS_ATS1_SW_RESET\n"); 01963 //ACK_L234_TM 01964 telemetry[0]=0xB0; 01965 telemetry[1]=tc[0]; 01966 ATS2_SW_ENABLE = 1;//as ats switched off 01967 ATS1_SW_ENABLE = 0; 01968 wait_ms(5); 01969 ATS1_SW_ENABLE = 1; 01970 telemetry[2]=ACK_CODE; 01971 for(uint8_t i=3;i<11;i++) 01972 { 01973 telemetry[i]=0x00; 01974 } 01975 crc16 = CRC::crc16_gen(telemetry,11); 01976 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 01977 telemetry[12] = (uint8_t)(crc16&0x00FF); 01978 for(uint8_t i=13;i<134;i++) 01979 { 01980 telemetry[i]=0x00; 01981 } 01982 break; 01983 } 01984 case 0x32: 01985 { 01986 gpc.printf("\n\n\rACS_ATS2_SW_RESET\n"); 01987 //ACK_L234_TM 01988 telemetry[0]=0xB0; 01989 telemetry[1]=tc[0]; 01990 ATS1_SW_ENABLE = 0;//as ats1 switched off 01991 ATS2_SW_ENABLE = 1; 01992 wait_ms(5); 01993 ATS2_SW_ENABLE = 0; 01994 telemetry[2]=ACK_CODE; 01995 for(uint8_t i=3;i<11;i++) 01996 { 01997 telemetry[i]=0x00; 01998 } 01999 crc16 = CRC::crc16_gen(telemetry,11); 02000 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 02001 telemetry[12] = (uint8_t)(crc16&0x00FF); 02002 for(uint8_t i=13;i<134;i++) 02003 { 02004 telemetry[i]=0x00; 02005 } 02006 break; 02007 } 02008 case 0x33: 02009 { 02010 gpc.printf("\n\n\rACS_TR_XY_SW_RESET\n"); 02011 //ACK_L234_TM 02012 telemetry[0]=0xB0; 02013 telemetry[1]=tc[0]; 02014 ACS_TR_XY_ENABLE= 0; 02015 wait_ms(5); 02016 ACS_TR_XY_ENABLE= 1; 02017 telemetry[2]=ACK_CODE; 02018 for(uint8_t i=3;i<11;i++) 02019 { 02020 telemetry[i]=0x00; 02021 } 02022 crc16 = CRC::crc16_gen(telemetry,11); 02023 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 02024 telemetry[12] = (uint8_t)(crc16&0x00FF); 02025 for(uint8_t i=13;i<134;i++) 02026 { 02027 telemetry[i]=0x00; 02028 } 02029 break; 02030 } 02031 case 0x34: 02032 { 02033 gpc.printf("\n\n\rACS_TR_Z_SW_RESET\n"); 02034 //ACK_L234_TM 02035 telemetry[0]=0xB0; 02036 telemetry[1]=tc[0]; 02037 ACS_TR_Z_ENABLE= 0; 02038 wait_ms(5); 02039 ACS_TR_Z_ENABLE= 1; 02040 telemetry[2]=ACK_CODE; 02041 for(uint8_t i=3;i<11;i++) 02042 { 02043 telemetry[i]=0x00; 02044 } 02045 crc16 = CRC::crc16_gen(telemetry,11); 02046 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 02047 telemetry[12] = (uint8_t)(crc16&0x00FF); 02048 for(uint8_t i=13;i<134;i++) 02049 { 02050 telemetry[i]=0x00; 02051 } 02052 break; 02053 } 02054 case 0x35: 02055 { 02056 gpc.printf("\n\n\rBCN_TX_SW_RESET\n"); 02057 //ACK_L234_TM 02058 telemetry[0]=0xB0; 02059 telemetry[1]=tc[0]; 02060 BCN_SW = 0; //active high 02061 wait_ms(5); 02062 BCN_SW = 1; 02063 telemetry[2]=ACK_CODE; 02064 for(uint8_t i=3;i<11;i++) 02065 { 02066 telemetry[i]=0x00; 02067 } 02068 crc16 = CRC::crc16_gen(telemetry,11); 02069 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 02070 telemetry[12] = (uint8_t)(crc16&0x00FF); 02071 for(uint8_t i=13;i<134;i++) 02072 { 02073 telemetry[i]=0x00; 02074 } 02075 break; 02076 } 02077 case 0x36: 02078 { 02079 gpc.printf("\n\n\rBAE_INTERNAL_RESET TO be done \n"); 02080 NVIC_SystemReset(); 02081 break; 02082 } 02083 case 0x37: 02084 { 02085 gpc.printf("\n\n\rCDMS_RESET\n"); 02086 //ACK_L234_TM 02087 telemetry[0]=0xB0; 02088 telemetry[1]=tc[0]; 02089 CDMS_RESET = 0; 02090 wait_ms(5); 02091 CDMS_RESET = 1; 02092 telemetry[2]=ACK_CODE; 02093 for(uint8_t i=3;i<11;i++) 02094 { 02095 telemetry[i]=0x00; 02096 } 02097 crc16 = CRC::crc16_gen(telemetry,11); 02098 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 02099 telemetry[12] = (uint8_t)(crc16&0x00FF); 02100 for(uint8_t i=13;i<134;i++) 02101 { 02102 telemetry[i]=0x00; 02103 } 02104 break; 02105 } 02106 case 0x38://not in IF present in SBC 02107 { 02108 gpc.printf("\n\rCDMS_SW_RESET pin yet to be decided"); 02109 //ACK_L234_TM 02110 telemetry[0]=0xB0; 02111 telemetry[1]=tc[0]; 02112 /* 02113 PIN to be DECIDED***************************************************************8 02114 */ 02115 telemetry[2]=ACK_CODE; 02116 for(uint8_t i=3;i<11;i++) 02117 { 02118 telemetry[i]=0x00; 02119 } 02120 crc16 = CRC::crc16_gen(telemetry,11); 02121 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 02122 telemetry[12] = (uint8_t)(crc16&0x00FF); 02123 for(uint8_t i=13;i<134;i++) 02124 { 02125 telemetry[i]=0x00; 02126 } 02127 break; 02128 } 02129 case 0x40://condition changed by AK47 now 00 means off in TC 02130 { 02131 printf("\n\n\r EN_PRCS\n"); 02132 uint8_t STANDBY_DATA_TC; 02133 BAE_STANDBY=0x07; 02134 STANDBY_DATA_TC=tc[4]; 02135 if(STANDBY_DATA_TC==0x00) 02136 { 02137 BAE_STANDBY &=0x03; 02138 //$ L BAE_STANDBY_TIMER_RESET(); 02139 //BAE_STANDBY_TIMER();//to be created to make all sensor on after 20 minutes 02140 } 02141 STANDBY_DATA_TC=tc[5]; 02142 if(STANDBY_DATA_TC==0x00) 02143 { 02144 BAE_STANDBY &=0x05; 02145 //$ L BAE_STANDBY_TIMER_RESET(); 02146 //BAE_STANDBY_TIMER();//to be created to make all sensor on after 20 minutes 02147 } 02148 STANDBY_DATA_TC=tc[6]; 02149 if(STANDBY_DATA_TC==0x00) 02150 { 02151 BAE_STANDBY &=0x06; 02152 //$ L BAE_STANDBY_TIMER_RESET(); 02153 //BAE_STANDBY_TIMER();//to be created to make all sensor on after 20 minutes 02154 } 02155 telemetry[0]=0xB0; 02156 telemetry[1]=tc[0]; 02157 telemetry[2]=0xC0;//ack_code for this case 02158 for(uint8_t i=3;i<11;i++) 02159 { 02160 telemetry[i]=0x00; 02161 } 02162 crc16 = CRC::crc16_gen(telemetry,11); 02163 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 02164 telemetry[12] = (uint8_t)(crc16&0x00FF); 02165 for(uint8_t i=13;i<134;i++) 02166 { 02167 telemetry[i]=0x00; 02168 } 02169 break; 02170 } 02171 case 0x41: 02172 { 02173 gpc.printf("\n\n\rexecutng BAE reset HK counter\n"); 02174 firstCount=true; 02175 void minMaxHkData(); 02176 02177 //ACK_L234_TM 02178 telemetry[0]=0xB0; 02179 telemetry[1]=tc[0]; 02180 telemetry[2]=0x02; 02181 for(uint8_t i=3;i<11;i++) 02182 { 02183 telemetry[i]=0x00; 02184 } 02185 crc16 = CRC::crc16_gen(telemetry,11); 02186 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 02187 telemetry[12] = (uint8_t)(crc16&0x00FF); 02188 for(uint8_t i=13;i<134;i++) 02189 { 02190 telemetry[i]=0x00; 02191 } 02192 break; 02193 } 02194 default: 02195 { 02196 gpc.printf("\n\rInvalid TC for FMS no matching FID"); 02197 //ACK_L234_TM 02198 telemetry[0]=0xB0; 02199 telemetry[1]=tc[0]; 02200 telemetry[2]=0x02; 02201 for(uint8_t i=3;i<11;i++) 02202 { 02203 telemetry[i]=0x00; 02204 } 02205 crc16 = CRC::crc16_gen(telemetry,11); 02206 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 02207 telemetry[12] = (uint8_t)(crc16&0x00FF); 02208 for(uint8_t i=13;i<134;i++) 02209 { 02210 telemetry[i]=0x00; 02211 } 02212 break; 02213 } 02214 } 02215 break; 02216 } 02217 default: 02218 { 02219 gpc.printf("\n\rInvalid TC, FMS service subtype mismacth"); 02220 //ACK_L234_TM 02221 telemetry[0]=0xB0; 02222 telemetry[1]=tc[0]; 02223 telemetry[2]=0x02; 02224 for(uint8_t i=3;i<11;i++) 02225 { 02226 telemetry[i]=0x00; 02227 } 02228 crc16 = CRC::crc16_gen(telemetry,11); 02229 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 02230 telemetry[12] = (uint8_t)(crc16&0x00FF); 02231 for(uint8_t i=13;i<134;i++) 02232 { 02233 telemetry[i]=0x00; 02234 } 02235 break; 02236 } 02237 } 02238 break; 02239 } 02240 default: 02241 { 02242 gpc.printf("\n\rInvalid TC neither FMS nor MMS"); 02243 //ACK_L234_TM 02244 telemetry[0]=0xB0; 02245 telemetry[1]=tc[0]; 02246 telemetry[2]=0x02; 02247 for(uint8_t i=3;i<11;i++) 02248 { 02249 telemetry[i]=0x00; 02250 } 02251 crc16 = CRC::crc16_gen(telemetry,11); 02252 telemetry[11] = (uint8_t)((crc16&0xFF00)>>8); 02253 telemetry[12] = (uint8_t)(crc16&0x00FF); 02254 for(uint8_t i=13;i<134;i++) 02255 { 02256 telemetry[i]=0x00; 02257 } 02258 break; 02259 } 02260 } 02261 } 02262 } 02263 } 02264 02265 02266 02267 int strt_add = flash_size() - (2*SECTOR_SIZE); 02268 uint32_t flasharray[8]; //256+(3*1024) 02269 /*corrected*/ 02270 int *nativeflash = (int*)strt_add; 02271 02272 /*Writing to the Flash*/ 02273 void FCTN_BAE_WR_FLASH(uint16_t j,uint32_t fdata) //j-position to write address ; fdata - flash data to be written 02274 { 02275 for(int i=0;i<8;i++) 02276 { 02277 flasharray[i]=nativeflash[i]; 02278 } 02279 flasharray[j]=fdata; 02280 erase_sector(strt_add); 02281 program_flash(strt_add, (char*)flasharray,32); 02282 } 02283 02284 /*End*/ 02285 02286 /*Reading from Flash*/ 02287 /*return choice parameter included so that i f we want the whole 32 packet data to be sent back we can do so*/ 02288 uint32_t FCTN_BAE_RD_FLASH_ENTITY(uint16_t entity) 02289 { 02290 for(int i=0;i<8;i++) 02291 { 02292 flasharray[i]=nativeflash[i]; 02293 } 02294 return flasharray[entity]; 02295 } 02296 02297 uint32_t* FCTN_BAE_RD_FLASH() 02298 { 02299 for(int i=0;i<8;i++) 02300 { 02301 flasharray[i]=nativeflash[i]; 02302 } 02303 return flasharray; 02304 } 02305 02306 /*End*/ 02307 02308 // Convert float to 4 uint8_t 02309 02310 02311 void FCTN_CONVERT_FLOAT(float input, uint8_t output[4]) 02312 { 02313 assert(sizeof(float) == sizeof(uint32_t)); 02314 uint32_t* temp = reinterpret_cast<uint32_t*>(&input); 02315 02316 //float* output1 = reinterpret_cast<float*>(temp); 02317 02318 //gpc.printf("\n\r %f ", input); 02319 //std::cout << "\n\r uint32"<<*temp << std::endl; 02320 02321 output[0] =(uint8_t )(((*temp)>>24)&0xFF); 02322 output[1] =(uint8_t ) (((*temp)>>16)&0xFF); 02323 output[2] =(uint8_t ) (((*temp)>>8)&0xFF); 02324 output[3] =(uint8_t ) ((*temp) & 0xFF); // verify the logic 02325 02326 // gpc.printf("\n\rthe values generated are\n"); 02327 /*gpc.printf("\n\r%x\n",output[0]); 02328 gpc.printf("\n\r%x\n",output[1]); 02329 gpc.printf("\n\r%x\n",output[2]); 02330 gpc.printf("\n\r%x\n",output[3]); 02331 to check the values generated 02332 */ 02333 //gpc.printf("\n\r inside %d %d %d %d", output[3],output[2],output[1],output[0]); 02334 //std:: cout << "\n\r uint8 inside " << output[3] << '\t' << output[2] << '\t' << output[1] << '\t' << output[0] <<std::endl; 02335 }
Generated on Tue Jul 19 2022 10:57:30 by
1.7.2
