latest BAE code 10 dec

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of RAJANGAM_REVIEW_BAE_CODE by Team Fox

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers TCTM.cpp Source File

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 }