Team Fox / Mbed 2 deprecated BAE_QM_MAR9

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of workinQM_5thJan_azad by Team Fox

EPS.cpp

Committer:
lakshya
Date:
2016-07-22
Revision:
51:661dc022613a
Parent:
50:6001287f3045
Child:
52:daa685b0e390

File content as of revision 51:661dc022613a:

#include "EPS.h"
#include "pin_config.h"
#include "iostream"

Serial eps_pc(USBTX,USBRX);

Timer timer_alertFlags;
Timer timer_soc;
Timer timer_FCTN_BATTERYGAUGE_INIT;

//FOR APPEDING HK DATA===================================

extern uint16_t crc_hk_data();

//acs
extern uint8_t ACS_DETUMBLING_ALGO_TYPE;
extern uint8_t ACS_STATE;
extern uint8_t ACS_MAG_TIME_DELAY;
extern uint8_t ACS_DEMAG_TIME_DELAY;
extern uint8_t ACS_Z_FIXED_MOMENT;
extern uint16_t ACS_MM_X_COMSN;
extern uint16_t ACS_MM_Y_COMSN;
extern uint16_t ACS_MG_X_COMSN;
extern uint16_t ACS_MG_Y_COMSN;
extern uint16_t ACS_MM_Z_COMSN;
extern uint16_t ACS_MG_Z_COMSN;
extern uint8_t ACS_MAIN_STATUS;
extern uint8_t ACS_STATUS;
extern uint8_t ATS1_EVENT_STATUS_RGTR;
extern uint8_t ATS1_SENTRAL_STATUS_RGTR;
extern uint8_t ATS1_ERROR_RGTR;
extern uint8_t ATS2_EVENT_STATUS_RGTR;
extern uint8_t ATS2_SENTRAL_STATUS_RGTR;
extern uint8_t ATS2_ERROR_RGTR;
extern uint8_t ACS_DATA_ACQ_STATUS;
extern uint8_t ACS_TR_X_PWM;
extern uint8_t ACS_TR_Y_PWM;
extern uint8_t ACS_TR_Z_PWM;
extern uint8_t alarmmode;
extern uint8_t controlmode_mms;
extern uint8_t invjm_mms[9];
extern uint8_t jm_mms[9];
extern uint8_t bb_mms[3];
extern uint8_t singularity_flag_mms;
extern uint8_t B_SCZ_ANGLE;


//bcn
extern uint8_t BCN_SPND_TX;
extern uint8_t BCN_FEN;
extern uint8_t BCN_LONG_MSG_TYPE;
extern uint8_t BCN_TX_MAIN_STATUS; 
extern uint8_t BCN_TX_STATUS;
extern uint8_t BCN_INIT_STATUS;
extern uint8_t BCN_FAIL_COUNT;
extern uint16_t BCN_TX_MAIN_COUNTER;
extern uint8_t BCN_TX_MAIN_STATUS;

//bae
extern Timer BAE_uptime;
extern Timer I2C_last;
extern uint8_t BAE_RESET_COUNTER;
extern uint8_t BAE_INIT_STATUS;
extern uint8_t BAE_STANDBY;
extern uint16_t BAE_I2C_COUNTER;
extern void RETURN_UPTIME(float time, uint8_t *day,uint8_t *hour,uint8_t *min);

//eps
extern uint8_t ACS_INIT_STATUS;
extern uint16_t EPS_MAIN_COUNTER;
extern uint8_t EPS_BTRY_TMP_STATUS;


//main
extern uint8_t HTR_CYCLE_COUNTER;
extern uint16_t ACS_MAIN_COUNTER;
extern uint8_t float_to_uint8(float min,float max,float val);

//=======================================================


/***********************************************global variable declaration***************************************************************/
extern uint32_t BAE_STATUS;
extern uint32_t BAE_ENABLE;
extern uint8_t LONG_HK_data[2][134];
//extern uint8_t BAE_HK_data[134];
//extern char BAE_chardata[74]; 

//implement them assign them default values  
uint8_t EPS_BATT_TEMP_LOW;
uint8_t EPS_BATT_TEMP_HIGH;
uint8_t EPS_BATT_TEMP_DEFAULT;
uint8_t EPS_SOC_LEVEL_12 = 70;
uint8_t EPS_SOC_LEVEL_23 = 90;
uint8_t EPS_BAT_TEMP_LOW;
uint8_t EPS_BAT_TEMP_HIGH;
uint8_t EPS_BAT_TEMP_DEFAULT;
DigitalIn EPS_CHARGER_FAULT(PIN42);
DigitalIn EPS_CHARGER_STATUS(PIN31);
DigitalIn EPS_BATTERY_GAUGE_ALERT(PIN73);

//m_I2C.frequency(10000)
extern int eps_btg_read_flag;  // flag to check I2C ack on reading from BTG
extern int eps_btg_writ_flag;  // flag to check I2C ack on writing from BTG

const char RCOMP0= 0x97;// don't know what it is now 
BAE_HK_actual actual_data;
BAE_HK_quant quant_data;
BAE_HK_min_max bae_HK_minmax;
BAE_HK_arch arch_data;

/************************battery temperature var*********************************/
//instead just return the approprate value.. test it

//......................................Peripheral declarations.........................................................//
Serial pc_eps(USBTX,USBRX);

I2C m_I2C(PIN85,PIN84);

//any default values or states
//====================================
//DigitalInOut TRXY(TRXY_DR_EN);            //active high
//DigitalInOut TRZ(TRZ_DR_EN);              //active high
DigitalOut EN3V3A(ENBL3V3A);
//====================================

DigitalOut BTRY_HTR_ENABLE(BATT_HEAT);// earlier BTRY_HTR_ENABLE
//DigitalIn BTRY_HT_OUTPUT(BATT_HEAT_OUTPUT);
//AnalogIn Vbatt_ang(VBATT);
AnalogIn Batt_voltage(PIN20);   //Battery voltage

SPI spi_bt(PIN99,PIN100,PIN98); //MOSI,MISO,SLK // battery temp something 3
DigitalOut ssn1(PIN19); //Slave select1 // low line master talks
DigitalOut ssn2(PIN21);//Slave select2
//DigitalOut PS(PTB0);
//DigitalOut HS(PTB1);

AnalogIn CurrentInput(PIN54); // Input from Current Multiplexer //PIN54
AnalogIn VoltageInput(PIN53); // Input from Voltage Multiplexer //PIN53
AnalogIn BAE_temp_sensor(PIN55); //Input from BAE temp sensor

/*mux for reading value one by one*/
DigitalOut SelectLinea3 (PIN46); // MSB of Select Lines
DigitalOut SelectLinea2 (PIN45);
DigitalOut SelectLinea1 (PIN44);
DigitalOut SelectLinea0 (PIN43); // LSB of Select Lines

DigitalOut SelectLineb3 (PIN56); // MSB of Select Lines
DigitalOut SelectLineb2 (PIN57);
DigitalOut SelectLineb1 (PIN58);
DigitalOut SelectLineb0 (PIN59); // LSB of Select Lines

//*********************************************************flags********************************************************//
extern uint8_t EPS_INIT_STATUS ;
extern uint8_t EPS_BATTERY_GAUGE_STATUS ;
extern uint8_t EPS_MAIN_STATUS;
extern uint8_t EPS_BTRY_TMP_STATUS ;
extern uint8_t EPS_STATUS ;

extern uint8_t EPS_BTRY_HTR ;

//eps cdms fault
extern uint8_t CDMS_SW_STATUS;
extern DigitalIn CDMS_OC_FAULT;
extern bool CDMS_SW_ENABLE;
extern int CDMS_FAULT_COUNTER;
extern uint8_t EPS_BTRY_HTR_AUTO;

//eps hw faults

/********* EXTERN ACS VAR ********************/
extern uint8_t ACS_ATS_STATUS;
extern uint8_t ACS_TR_Z_SW_STATUS;
extern DigitalOut ACS_TR_Z_ENABLE;
extern DigitalIn ACS_TR_Z_OC_FAULT;
extern DigitalIn ACS_TR_Z_FAULT;            //Driver IC fault
extern int ACS_TR_Z_FAULT_COUNTER;

extern uint8_t ACS_TR_XY_SW_STATUS;
extern DigitalOut ACS_TR_XY_ENABLE;
extern DigitalIn ACS_TR_XY_OC_FAULT;
extern DigitalIn ACS_TR_XY_FAULT;            //Driver IC fault
extern int ACS_TR_XY_FAULT_COUNTER;

//extern uint8_t ACS_ATS1_SW_STATUS;
extern DigitalOut ATS1_SW_ENABLE;
extern DigitalIn ACS_ATS1_OC_FAULT;
extern int ACS_ATS1_FAULT_COUNTER;

//extern uint8_t ACS_ATS2_SW_STATUS;

extern DigitalOut ATS2_SW_ENABLE;
extern DigitalIn ACS_ATS2_OC_FAULT;
extern int ACS_ATS2_FAULT_COUNTER;

/********* EXTERN BCN VAR ********************/
extern uint8_t BCN_TX_SW_STATUS;
extern bool BCN_TX_ENABLE;
extern DigitalIn BCN_TX_OC_FAULT;
extern int BCN_TX_FAULT_COUNTER;
extern uint8_t BCN_TMP;

//........................................... FUCTIONS.................................................//

void FCTN_EPS_INIT()
{
////    eps_pc.printf("\n\r eps init \n");
    EPS_INIT_STATUS = 1 ;             //set EPS_INIT_STATUS flag
    // FLAG();
    FCTN_BATTERYGAUGE_INIT();
    EN3V3A = 1;                             //enable dc dc converter A  
    
    timer_alertFlags.reset();
    timer_alertFlags.start();
    char value=alertFlags(); // initialization part of battery gauge
    timer_alertFlags.stop();
    
    unsigned short value_u= (short int )value;
    //value_u &=0x0001;                     
    if(value_u & 0x0001 == 0x0001)                       // battery gauge not initialised
    {
        //actual_data.power_mode = 1;
        actual_data.power_mode = 0;
        EPS_BATTERY_GAUGE_STATUS = 0;               //clear EPS_BATTERY_GAUGE_STATUS
        eps_pc.printf(" init BTG fail - %d\n\r", value_u);
    }
    else
    {
        timer_soc.reset();
        timer_soc.start();
        actual_data.Batt_gauge_actual[1] = soc();
        timer_soc.stop();
        
        actual_data.Batt_voltage_actual = Batt_voltage.read()*3.3; //1 corresponds to 3.3 scaling factor
        FCTN_EPS_POWERMODE(actual_data.Batt_gauge_actual[1]);
        EPS_BATTERY_GAUGE_STATUS = 1;               //set EPS_BATTERY_GAUGE_STATUS
        eps_pc.printf("init BTG success - %d\n\r", value_u);
    }
    
    //if( read(REG_VERSION) == 
    eps_pc.printf("REG_VERSION = %d\r\n",read(REG_VERSION));
   
    FCTN_BATTTEMP_INIT();
    //EPS_BATTERY_GAUGE_STATUS = 1;
    
    EPS_INIT_STATUS = 0 ;             //clear EPS_INIT_STATUS flag

}

void FCTN_EPS_HANDLE_CDMS_FAULT()
{   //Variable names from MMS structure, if not, marked as //Temp name
    if(CDMS_SW_STATUS == 0b11)          //powered on and oc fault
        if(!CDMS_OC_FAULT)   
           CDMS_SW_STATUS = 0b01;       //powered on and working
    else
    {
        if(CDMS_SW_STATUS == 0b10)      //powered off and oc fault        
            CDMS_SW_ENABLE = 1;         //Temp name
        if(CDMS_OC_FAULT == 0)
        {
            CDMS_FAULT_COUNTER = 0;     //Temp name
            CDMS_SW_STATUS = 0b01;      //powered on and working
        }
        else
        {
            CDMS_FAULT_COUNTER++;
            if(CDMS_FAULT_COUNTER == 3)
                CDMS_SW_STATUS = 0b11;  //powered on and oc fault
            else
            {
                CDMS_SW_ENABLE = 0;         //power OFF switch
                CDMS_SW_STATUS = 0b10;      //powered off and oc fault   
            }
        }
    }     
}

void FCTN_EPS_HANDLE_HW_FAULTS()
{   //Variable names from MMS structure, if not, marked as //Temp name
    
    //--------ACS_TR_Z--------//
    if(ACS_TR_Z_SW_STATUS != 0b11)          //!disabled
    {
        if(ACS_TR_Z_SW_STATUS == 0b10)      //oc fault and powered off
            ACS_TR_Z_ENABLE = 1;
        if(ACS_TR_Z_OC_FAULT || ACS_TR_Z_FAULT)
        {
            ACS_TR_Z_ENABLE = 0;
            ACS_TR_Z_FAULT_COUNTER++;       //Temp name
            if(ACS_TR_Z_FAULT_COUNTER == 3)
                ACS_TR_Z_SW_STATUS = 0b11;  //disabled
                //update in flash as default state at bootup
            else ACS_TR_Z_SW_STATUS = 0b10; //oc fault and powered off
        }
        else
        {
            ACS_TR_Z_SW_STATUS = 0b01;      //powered on and working;
            //update in flash also
            ACS_TR_Z_FAULT_COUNTER = 0;
        }
    }
    
    //--------ACS_TR_XY--------//
    //Same as ACS_TR_Z, just replace Z with XY
        if(ACS_TR_XY_SW_STATUS != 0b11)          //!disabled
    {
        if(ACS_TR_XY_SW_STATUS == 0b10)      //oc fault and powered off
            ACS_TR_XY_ENABLE = 1;
        if(ACS_TR_XY_OC_FAULT || ACS_TR_XY_FAULT)
        {
            ACS_TR_XY_ENABLE = 0;
            ACS_TR_XY_FAULT_COUNTER++;       //Temp name
            if(ACS_TR_XY_FAULT_COUNTER == 3)
                ACS_TR_XY_SW_STATUS = 0b11;  //disabled
                //update in flash as default state at bootup
            else ACS_TR_XY_SW_STATUS = 0b10; //oc fault and powered off
        }
        else
        {
            ACS_TR_XY_SW_STATUS = 0b01;      //powered on and working;
            //update in flash also
            ACS_TR_XY_FAULT_COUNTER = 0;
        }
    }    

    //--------ACS_ATS1--------//
    //Same as ACS_ATS2
    //if(ACS_ATS1_SW_STATUS & 0b1100 != 0b1100)          //!disabled 
    if(ACS_ATS_STATUS&0xC0!=0xC0)
    {
        //if(ACS_ATS1_SW_STATUS & 0b1100 == 0b1000)      //oc fault and powered off
        if(ACS_ATS_STATUS&0xC0!=0x80)
            ATS1_SW_ENABLE = 0;                       //Temp name
        if(ACS_ATS1_OC_FAULT)
        {
            ATS1_SW_ENABLE = 1;
            ACS_ATS1_FAULT_COUNTER++;                  //Temp name
            if(ACS_ATS1_FAULT_COUNTER == 3)
                //ACS_ATS1_SW_STATUS = ACS_ATS1_SW_STATUS | 0b1100;  //disabled
                ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
                //update in flash as default state at bootup
            else 
            {
                //ACS_ATS1_SW_STATUS = ACS_ATS1_SW_STATUS | 0b1000; //setting to 10xx
                ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x80;
                //ACS_ATS1_SW_STATUS = ACS_ATS1_SW_STATUS & 0b1011; //oc fault and powered off
            }
        }
        else
        {
            //if(ACS_ATS1_SW_STATUS & 0b1100 == 0b1000)             //Device oc fault?
            if(ACS_ATS_STATUS&0xC0==0x80)
                ATS1_SW_ENABLE = 1;
            //ACS_ATS1_SW_STATUS = ACS_ATS1_SW_STATUS & 0b0011;     //working but powered off
            ACS_ATS_STATUS = ACS_ATS_STATUS&0x3F;
            //Update in flash also
            ACS_ATS1_FAULT_COUNTER = 0;
        }
    }
    
    //--------ACS_ATS2--------//
    //if(ACS_ATS2_SW_STATUS & 0b1100 != 0b1100)          //!disabled
    if(ACS_ATS_STATUS&0x0C!=0x0C)
    {
        //if(ACS_ATS2_SW_STATUS & 0b1100 == 0b1000)      //oc fault and powered off
        if(ACS_ATS_STATUS&0x0C!=0x08)
            ATS2_SW_ENABLE = 0;                       //Temp name
        if(ACS_ATS2_OC_FAULT)
        {
            ATS2_SW_ENABLE = 1;
            ACS_ATS2_FAULT_COUNTER++;                  //Temp name
            if(ACS_ATS2_FAULT_COUNTER == 3)
                //ACS_ATS2_SW_STATUS = ACS_ATS2_SW_STATUS | 0b1100;  //disabled
                ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
                //update in flash as default state at bootup
            else 
            {
                //ACS_ATS2_SW_STATUS = ACS_ATS2_SW_STATUS | 0b1000; //setting to 10xx
                //ACS_ATS2_SW_STATUS = ACS_ATS2_SW_STATUS & 0b1011; //oc fault and powered off
                ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x08;
            }
        }
        else
        {
            //if(ACS_ATS2_SW_STATUS & 0b1100 == 0b1000)             //Device oc fault?
            if(ACS_ATS_STATUS&0x0C==0x08)
                ATS2_SW_ENABLE = 1;
            //ACS_ATS2_SW_STATUS = ACS_ATS2_SW_STATUS & 0b0011;     //working but powered off
            ACS_ATS_STATUS = ACS_ATS_STATUS&0xF3;
            //Update in flash also
            ACS_ATS2_FAULT_COUNTER = 0;
        }
    }
    
    //--------BCN_TX----------//
    /* Commented out to test BCN TC
    if(BCN_TX_SW_STATUS != 0b11)          //!disabled
    {
        if(BCN_TX_SW_STATUS == 0b10)      //oc fault and powered off
            BCN_TX_ENABLE = 1;            //Temp name
        if(BCN_TX_OC_FAULT)
        {
            BCN_TX_ENABLE = 0;
            BCN_TX_FAULT_COUNTER++;       //Temp name
            if(BCN_TX_FAULT_COUNTER == 3)
                BCN_TX_SW_STATUS = 0b11;  //disabled
                //update in flash as default state at bootup
            else BCN_TX_SW_STATUS = 0b10; //oc fault and powered off
                
        }
        else
        {
            BCN_TX_SW_STATUS = 0b01;      //powered on and working;
            //update in flash also
            BCN_TX_FAULT_COUNTER = 0;
        }
    }
    */
}

//----------------------------------------------------Power algo code--------------------------------------------------------------------//
/*update the power modes*/

void FCTN_EPS_POWERMODE(float soc)              //dummy algo
{
    if(soc >= EPS_SOC_LEVEL_23*100 )
        actual_data.power_mode = 3;
    else if(soc >= EPS_SOC_LEVEL_12*100 )
       actual_data.power_mode = 2;
    else actual_data.power_mode = 1;
}

//...................................................HK...........................................//
/*reading values*/

void FCTN_HK_MAIN()
{
    int Iteration=0;

    SelectLinea0=0;
    SelectLinea1=0;
    SelectLinea2=0;
    SelectLinea3=0;

    SelectLineb0=0;
    SelectLineb1=0;
    SelectLineb2=0;
    SelectLineb3=0;
    
      //collecting data
    for(Iteration=0; Iteration<16; Iteration++)
    {
            actual_data.voltage_actual[Iteration]=VoltageInput.read();
            wait_ms(100); //remove
            actual_data.current_actual[Iteration]=CurrentInput.read();
           
            SelectLinea0=!(SelectLinea0);
            if(Iteration%2==1)
                SelectLinea1=!(SelectLinea1);
            if(Iteration%4==3)
                SelectLinea2=!(SelectLinea2);
            if(Iteration%8==7)
                SelectLinea3=!(SelectLinea3);
                int s0,s1,s2,s3;
            s0=SelectLineb0=SelectLinea0;
            s1=SelectLineb1=SelectLinea2;
            s2=SelectLineb2=SelectLinea2;
            s3=SelectLineb3=SelectLinea3;
////            printf("\n\r  %d %d %d %d", s0,s1,s2,s3);

    }
      for(Iteration=0; Iteration<16; Iteration++)
    {
        if(Iteration==14)
            actual_data.voltage_actual[Iteration]= (-90.7*3.3*actual_data.voltage_actual[Iteration])+190.1543;
        else
            actual_data.voltage_actual[Iteration]= actual_data.voltage_actual[Iteration]*3.3*5.63;
    }
        
    for(Iteration=0;Iteration<12;Iteration++){
        if(Iteration<8)
            actual_data.current_actual[Iteration]= actual_data.current_actual[Iteration]*3.3/(50*rsens);
        else
            actual_data.current_actual[Iteration]=actual_data.current_actual[Iteration]*3.3;
            int resistance;       
             
            resistance=24000*actual_data.current_actual[Iteration]/(3.3-actual_data.current_actual[Iteration]);
            if(actual_data.current_actual[Iteration]>1.47) 
            {
                actual_data.current_actual[Iteration]=3694/log(24.032242*resistance);
            }
            else{
                
                actual_data.current_actual[Iteration]=3365.4/log(7.60573*resistance);
            }
    }
    //actual_data.BAE_temp_actual=(-90.7*3.3*actual_data.BAE_temp_actual)+190.1543;
    actual_data.BAE_temp_actual=(-90.7*3.3*BAE_temp_sensor.read())+190.1543;
    
    actual_data.Batt_voltage_actual=Batt_voltage.read()*3.3*5.63;

    //quantizing data //changing the algo 
    for(Iteration=0; Iteration<16; Iteration++){

        if(Iteration==14)
            quant_data.voltage_quant[Iteration] = float_to_uint8(0.0,5,actual_data.voltage_actual[Iteration]);//quantiz(tstart,tstep,actual_data.voltage_actual[Iteration]);
        else
            quant_data.voltage_quant[Iteration] = float_to_uint8(0.0,5,actual_data.voltage_actual[Iteration]);//quantiz(vstart,vstep,actual_data.voltage_actual[Iteration]);
        
    }
    for(Iteration=0;Iteration<12;Iteration++){
        if(Iteration<8)
            quant_data.current_quant[Iteration] = float_to_uint8(0.0,5,actual_data.current_actual[Iteration]);//quantiz(cstart,cstep,actual_data.current_actual[Iteration]);
        else
            quant_data.current_quant[Iteration] = float_to_uint8(0.0,5,actual_data.current_actual[Iteration]);//quantiz(tstart_thermistor,tstep_thermistor,actual_data.current_actual[Iteration]);
     }       
    for(Iteration=0;Iteration<2;Iteration++){
        
        quant_data.Batt_temp_quant[Iteration] = float_to_uint8(0.0,5,actual_data.Batt_temp_actual[Iteration]);//quantiz(tstart,tstep,actual_data.Batt_temp_actual[Iteration]);
    }
    
    //to be changed
    quant_data.Batt_gauge_quant[0] = quantiz(vcell_start,vcell_step,actual_data.Batt_gauge_actual[0]);
    quant_data.Batt_gauge_quant[1]=quantiz(soc_start,soc_step,actual_data.Batt_gauge_actual[1]);
    quant_data.Batt_gauge_quant[2]=quantiz(crate_start,crate_step,actual_data.Batt_gauge_actual[2]);
    quant_data.Batt_gauge_alerts=actual_data.Batt_gauge_actual[3];
    
    quant_data.BAE_temp_quant=quantiz(tstart,tstep,actual_data.BAE_temp_actual);
    
////    for(Iteration=0;Iteration<3;Iteration++){
////        quant_data.AngularSpeed_quant[Iteration]=actual_data.AngularSpeed_actual[Iteration];
////        printf("\n\r w value %f",quant_data.AngularSpeed_quant[Iteration]);
////    }
    
////    for(Iteration=0;Iteration<3;Iteration++){
////        quant_data.Bvalue_quant[Iteration]=actual_data.Bvalue_actual[Iteration];
////        printf("\n\r b value %f",quant_data.Bvalue_quant[Iteration]);
////    }
    
    quant_data.Batt_voltage_quant=quantiz(vstart,vstep,actual_data.Batt_voltage_actual);
    
    
    arch_data.Batt_1_temp=quant_data.Batt_temp_quant[0];
    arch_data.Batt_2_temp=quant_data.Batt_temp_quant[1];
    arch_data.EPS_PCB_temp=quant_data.voltage_quant[14];
    arch_data.Batt_SOC=quant_data.Batt_gauge_quant[1];
    arch_data.power_mode=actual_data.power_mode;
    arch_data.faultPoll_status=actual_data.faultPoll_status;
    arch_data.faultIr_status=actual_data.faultIr_status;
    arch_data.Batt_voltage=quant_data.Batt_voltage_quant;    
////    printf("\n\r in hk");
    
}

void FCTN_APPEND_HKDATA()
{
    LONG_HK_data[1][0] = 0x28;
    LONG_HK_data[1][1] = 0x00;
    LONG_HK_data[1][2] = 0x00;
    LONG_HK_data[1][3] = 0x00;
    LONG_HK_data[1][4] = ACS_ATS_STATUS;
    LONG_HK_data[1][5] = ACS_TR_XY_SW_STATUS;
    LONG_HK_data[1][5] = (LONG_HK_data[1][5]<<2)| ACS_TR_Z_SW_STATUS;
    LONG_HK_data[1][5] = (LONG_HK_data[1][5]<<4) | ACS_STATE;
    
    LONG_HK_data[1][6] = ACS_DETUMBLING_ALGO_TYPE;
    LONG_HK_data[1][6] = (LONG_HK_data[1][6]<<2) | BCN_TX_SW_STATUS;
    LONG_HK_data[1][6] = (LONG_HK_data[1][6]<<1) | BCN_SPND_TX;
    LONG_HK_data[1][6] = (LONG_HK_data[1][6]<<1) | BCN_FEN;
    LONG_HK_data[1][6] = (LONG_HK_data[1][6]<<1) | BCN_LONG_MSG_TYPE;
    LONG_HK_data[1][6] = (LONG_HK_data[1][6]<<1) | EPS_BTRY_HTR_AUTO;//EPS_BATTERY_HEATER_ENABLE
    LONG_HK_data[1][6] = (LONG_HK_data[1][6]<<1);
    //now one spares in BAE_HK_data[5]
    LONG_HK_data[1][7] = BAE_RESET_COUNTER;
    LONG_HK_data[1][8] = EPS_SOC_LEVEL_12; 
    LONG_HK_data[1][9] = EPS_SOC_LEVEL_23;
    LONG_HK_data[1][10] = ACS_MAG_TIME_DELAY;
    LONG_HK_data[1][11] = ACS_DEMAG_TIME_DELAY;
    LONG_HK_data[1][12] = EPS_BAT_TEMP_LOW;
    LONG_HK_data[1][13] = EPS_BAT_TEMP_HIGH;
    LONG_HK_data[1][14] = EPS_BAT_TEMP_DEFAULT;
    LONG_HK_data[1][15] = ACS_MM_X_COMSN >> 8;
    LONG_HK_data[1][16] = ACS_MM_X_COMSN;
    
    LONG_HK_data[1][17] = ACS_MM_Y_COMSN >> 8;
    LONG_HK_data[1][18] = ACS_MM_Y_COMSN;
                                                                
    LONG_HK_data[1][19] = ACS_MG_X_COMSN >> 8;
    LONG_HK_data[1][20] = ACS_MG_X_COMSN;
                                                                
    LONG_HK_data[1][21] = ACS_MG_Y_COMSN >> 8;
    LONG_HK_data[1][22] = ACS_MG_Y_COMSN;
                                                            
    LONG_HK_data[1][23] = ACS_MM_Z_COMSN >> 8;
    LONG_HK_data[1][24] = ACS_MM_Z_COMSN;
                                                                
    LONG_HK_data[1][25] = ACS_MG_Z_COMSN >> 8;
    LONG_HK_data[1][26] = ACS_MG_Z_COMSN;
                                                                
    LONG_HK_data[1][27] = ACS_Z_FIXED_MOMENT >> 8;
    LONG_HK_data[1][28] = ACS_Z_FIXED_MOMENT; 
                                                             
    //BAE RAM PARAMETER
    LONG_HK_data[1][29] = BAE_INIT_STATUS;
    LONG_HK_data[1][29] = (LONG_HK_data[1][29]<<1) | 0;//change it================================
    LONG_HK_data[1][29] = (LONG_HK_data[1][29]<<1) | BCN_INIT_STATUS; 
    LONG_HK_data[1][29] = (LONG_HK_data[1][29]<<1) | BCN_TX_MAIN_STATUS;
    LONG_HK_data[1][29] = (LONG_HK_data[1][29]<<3) | BCN_TX_STATUS;
    LONG_HK_data[1][29] = (LONG_HK_data[1][29]<<3) | ACS_INIT_STATUS;
                                                                
    LONG_HK_data[1][30] = ACS_DATA_ACQ_STATUS;
    LONG_HK_data[1][30] = (LONG_HK_data[1][30]<<1) | ACS_MAIN_STATUS;
    LONG_HK_data[1][30] = (LONG_HK_data[1][30]<<4) | ACS_STATUS;
    LONG_HK_data[1][30] = (LONG_HK_data[1][30]<<1) | EPS_INIT_STATUS;
    
    LONG_HK_data[1][31] = EPS_BATTERY_GAUGE_STATUS;
    LONG_HK_data[1][31] = (LONG_HK_data[1][31]<<1) | EPS_MAIN_STATUS;
    LONG_HK_data[1][31] = (LONG_HK_data[1][31]<<1) | EPS_BTRY_TMP_STATUS;;
    LONG_HK_data[1][31] = (LONG_HK_data[1][31]<<3) | EPS_STATUS;
    LONG_HK_data[1][31] = (LONG_HK_data[1][31]<<2) | CDMS_SW_STATUS;
    ////BAE_HK_data[31] = (BAE_HK_data[31]<<1) | EPS_BTRY_HTR_STATUS;//new to : implement===========
    
    LONG_HK_data[1][32] = EPS_BTRY_HTR;// to be disscussed
    //spare 4
    LONG_HK_data[1][32] = (LONG_HK_data[1][32]<<7) | BAE_STANDBY;
    
    // 6 next telemetries value to be given by registers
    LONG_HK_data[1][33] = ATS1_EVENT_STATUS_RGTR;
    LONG_HK_data[1][34] = ATS1_SENTRAL_STATUS_RGTR;
    LONG_HK_data[1][35] = ATS1_ERROR_RGTR;
    LONG_HK_data[1][36] = ATS2_EVENT_STATUS_RGTR;
    LONG_HK_data[1][37] = ATS2_SENTRAL_STATUS_RGTR;
    LONG_HK_data[1][38] = ATS2_ERROR_RGTR;
                                                                
    LONG_HK_data[1][39] = BCN_FAIL_COUNT;
    LONG_HK_data[1][40] = actual_data.power_mode;
    LONG_HK_data[1][41] = HTR_CYCLE_COUNTER;//new to : implement
                                                                
    LONG_HK_data[1][42] = BAE_I2C_COUNTER;
    LONG_HK_data[1][43] = BAE_I2C_COUNTER>>8;
    LONG_HK_data[1][44] = ACS_MAIN_COUNTER;
    LONG_HK_data[1][45] = ACS_MAIN_COUNTER>>8;
    LONG_HK_data[1][46] = BCN_TX_MAIN_COUNTER;
    LONG_HK_data[1][47] = BCN_TX_MAIN_COUNTER>>8;
    LONG_HK_data[1][48] = EPS_MAIN_COUNTER;
    LONG_HK_data[1][49] = EPS_MAIN_COUNTER>>8;
    
    uint8_t days,hours,mins;
    RETURN_UPTIME(BAE_uptime.read(),&days,&hours,&mins);
    LONG_HK_data[1][50] = days;
    RETURN_UPTIME(I2C_last.read(),&days,&hours,&mins);
    LONG_HK_data[1][50] = (LONG_HK_data[1][50]) | (hours>>2);
    LONG_HK_data[1][51] = hours;
    LONG_HK_data[1][51] = (LONG_HK_data[1][51]<<6) | mins;
    
    
    LONG_HK_data[1][52] = actual_data.bit_data_acs_mm[0];
    LONG_HK_data[1][53] = actual_data.bit_data_acs_mm[0]>>8;
    LONG_HK_data[1][54] = actual_data.bit_data_acs_mm[1];
    LONG_HK_data[1][55] = actual_data.bit_data_acs_mm[1]>>8;
    LONG_HK_data[1][56] = actual_data.bit_data_acs_mm[2];
    LONG_HK_data[1][57] = actual_data.bit_data_acs_mm[2]>>8;
                                                                
    LONG_HK_data[1][58] = actual_data.bit_data_acs_mg[0];
    LONG_HK_data[1][59] = actual_data.bit_data_acs_mg[0]>>8;
    LONG_HK_data[1][60] = actual_data.bit_data_acs_mg[1];
    LONG_HK_data[1][61] = actual_data.bit_data_acs_mg[1]>>8;
    LONG_HK_data[1][62] = actual_data.bit_data_acs_mg[2];
    LONG_HK_data[1][63] = actual_data.bit_data_acs_mg[2]>>8;
                                                                
    LONG_HK_data[1][64] = BCN_TX_OC_FAULT;
    LONG_HK_data[1][64] = (LONG_HK_data[1][64]<<1) | ACS_TR_XY_ENABLE;
    LONG_HK_data[1][64] = (LONG_HK_data[1][64]<<1) | ACS_TR_Z_ENABLE;
    LONG_HK_data[1][64] = (LONG_HK_data[1][64]<<1) | ACS_TR_XY_OC_FAULT;
    LONG_HK_data[1][64] = (LONG_HK_data[1][64]<<1) | ACS_TR_Z_OC_FAULT;
    LONG_HK_data[1][64] = (LONG_HK_data[1][64]<<1) | ACS_TR_XY_FAULT;
    LONG_HK_data[1][64] = (LONG_HK_data[1][64]<<1) | EPS_CHARGER_FAULT;
    LONG_HK_data[1][64] = (LONG_HK_data[1][64]<<1) | EPS_CHARGER_STATUS;
                                                                
    LONG_HK_data[1][65] = EPS_BATTERY_GAUGE_ALERT;
    LONG_HK_data[1][65] = (LONG_HK_data[1][65]<<1) | CDMS_OC_FAULT;
    LONG_HK_data[1][65] = (LONG_HK_data[1][65]<<1) | ACS_ATS1_OC_FAULT;
    LONG_HK_data[1][65] = (LONG_HK_data[1][65]<<1) | ACS_ATS2_OC_FAULT;
    LONG_HK_data[1][65] = (LONG_HK_data[1][65]<<1) | ACS_TR_Z_FAULT;
    LONG_HK_data[1][65] = (LONG_HK_data[1][65]<<3);
    //3 spare
                                                                
    LONG_HK_data[1][66] = ACS_TR_X_PWM;
    LONG_HK_data[1][67] = ACS_TR_Y_PWM;
    LONG_HK_data[1][68] = ACS_TR_Z_PWM;
    //spare byte
    //assigned it to counter HTR_CYCLE_COUNTER
                                                                
    //assign it b_scz_angle
    LONG_HK_data[1][69] = B_SCZ_ANGLE>>4; ; 
    LONG_HK_data[1][69] = (LONG_HK_data[1][69]<<1) | alarmmode;
    LONG_HK_data[1][69] = (LONG_HK_data[1][69]<<1) | controlmode_mms;
    LONG_HK_data[1][69] = (LONG_HK_data[1][69]<<1) | singularity_flag_mms;
    LONG_HK_data[1][69] = (LONG_HK_data[1][69]<<1);
    //1 bit spare
                                                                
    for(int i=0;i<9;i++)
        {
            LONG_HK_data[1][70+i] =  invjm_mms[i];
            LONG_HK_data[1][81+i] =  jm_mms[i];
        }
                                                                
    for(int i=0;i<2;i++)
        LONG_HK_data[1][79+i] = bb_mms[i];
                                                                
    for(int i=0;i<16;i++)
        {
            LONG_HK_data[1][90+i] = quant_data.voltage_quant[i];
            LONG_HK_data[1][106+i] = quant_data.current_quant[i];
        }
                                                                
    LONG_HK_data[1][122] = quant_data.Batt_voltage_quant;
    LONG_HK_data[1][123] = quant_data.BAE_temp_quant;
    LONG_HK_data[1][124] = (uint8_t)(actual_data.Batt_gauge_actual[1]);
    LONG_HK_data[1][125] = quant_data.Batt_temp_quant[0];
    LONG_HK_data[1][126] = quant_data.Batt_temp_quant[1];
    LONG_HK_data[1][127] = BCN_TMP;
    LONG_HK_data[1][128] = 0x00;
    LONG_HK_data[1][129] = 0x00;
    LONG_HK_data[1][130] = 0x00;
    LONG_HK_data[1][131] = 0x00;
    uint16_t crc = crc_hk_data();
    LONG_HK_data[1][132] = (uint8_t)(crc >> 8);
    LONG_HK_data[1][133] = crc;

//===================================================
/* can be retrived from the earlier code (function)*/   
}

uint8_t quantiz(float start,float step,float x)
{
    int y=(x-start)/step;
    if(y<=0)y=0;
    if(y>=255)y=255;
    return y;
}

bool firstCount=true;  // goes to EPS init


void saveMin(char x,char y){
    if(y<x){
        x=y;
    }

}
void saveMax(char x,char y){
    if (y>x)
    {
       x=y;
    }
}


void minMaxHkData(){
    if(firstCount==true){
        for (int i = 0; i < 16; ++i){    
        bae_HK_minmax.voltage_min[i] = quant_data.voltage_quant[i];
        bae_HK_minmax.voltage_max[i] = quant_data.voltage_quant[i];
        }
        for (int i = 0; i < 12; ++i){    
        bae_HK_minmax.current_min[i] = quant_data.current_quant[i];
        bae_HK_minmax.current_max[i] = quant_data.current_quant[i];   
        }

        for (int i = 0; i < 2; ++i){    
        bae_HK_minmax.Batt_temp_min[i] = quant_data.Batt_temp_quant[i];
        bae_HK_minmax.Batt_temp_max[i] = quant_data.Batt_temp_quant[i];
        }    
        /*
        for (int i = 0; i < 3; ++i){    
        bae_HK_minmax.Batt_gauge_min[i] = quant_data.Batt_gauge_quant[i];
        bae_HK_minmax.Batt_gauge_max[i] = quant_data.Batt_gauge_quant[i];
        }
        */
        bae_HK_minmax.Batt_SOC_min = quant_data.Batt_gauge_quant[1];
        bae_HK_minmax.Batt_SOC_max = quant_data.Batt_gauge_quant[1];
        
        bae_HK_minmax.BCN_TEMP_min = BCN_TMP;
        bae_HK_minmax.BCN_TEMP_min = BCN_TMP;
        
        for (int i = 0; i < 3; ++i){    
        bae_HK_minmax.bit_data_acs_mg_min[i] = actual_data.bit_data_acs_mg[i];
        bae_HK_minmax.bit_data_acs_mg_max[i] = actual_data.bit_data_acs_mg[i];
        }
        for (int i = 0; i < 3; ++i){    
        bae_HK_minmax.bit_data_acs_mm_min[i] = actual_data.bit_data_acs_mm[i];//Bvalue_quant earlier
        bae_HK_minmax.bit_data_acs_mm_max[i] = actual_data.bit_data_acs_mm[i];
        }
        bae_HK_minmax.BAE_temp_min=quant_data.BAE_temp_quant;
        bae_HK_minmax.BAE_temp_max=quant_data.BAE_temp_quant;
        bae_HK_minmax.Batt_voltage_min=quant_data.Batt_voltage_quant;
        bae_HK_minmax.Batt_voltage_max=quant_data.Batt_voltage_quant;
          
    } 
    else {
        for (int i = 0; i < 16; ++i)
        {
            saveMin(bae_HK_minmax.voltage_min[i],quant_data.voltage_quant[i]);
            saveMax(bae_HK_minmax.voltage_max[i],quant_data.voltage_quant[i]);
        }
        for (int i = 0; i < 12; ++i)
        {
            saveMin(bae_HK_minmax.current_min[i],quant_data.current_quant[i]);
            saveMax(bae_HK_minmax.current_max[i],quant_data.current_quant[i]);
        }
        
        for (int i = 0; i < 2; ++i)
        {
            saveMin(bae_HK_minmax.Batt_temp_min[i],quant_data.Batt_temp_quant[i]);
            saveMax(bae_HK_minmax.Batt_temp_max[i],quant_data.Batt_temp_quant[i]);
        }
        /*
        for (int i = 0; i < 3; ++i)
        {
            saveMin(bae_HK_minmax.Batt_gauge_min[i], quant_data.Batt_gauge_quant[i]);
            saveMax(bae_HK_minmax.Batt_gauge_max[i], quant_data.Batt_gauge_quant[i]);
        }
        */
        saveMin(bae_HK_minmax.Batt_SOC_min, quant_data.Batt_gauge_quant[1]);
        saveMax(bae_HK_minmax.Batt_SOC_max, quant_data.Batt_gauge_quant[1]);
        
        saveMin(bae_HK_minmax.BCN_TEMP_min, BCN_TMP);
        saveMin(bae_HK_minmax.BCN_TEMP_max, BCN_TMP);
        
        for (int i = 0; i < 3; ++i)
        {
            saveMin(bae_HK_minmax.bit_data_acs_mg_min[i], actual_data.bit_data_acs_mg[i]);
            saveMax(bae_HK_minmax.bit_data_acs_mg_max[i], actual_data.bit_data_acs_mg[i]);
        }
        for (int i = 0; i < 3; ++i)
        {
            saveMin(bae_HK_minmax.bit_data_acs_mm_min[i], actual_data.bit_data_acs_mm[i]);
            saveMax(bae_HK_minmax.bit_data_acs_mm_max[i], actual_data.bit_data_acs_mm[i]);
        }
        saveMin(bae_HK_minmax.BAE_temp_min,quant_data.BAE_temp_quant);
        saveMax(bae_HK_minmax.BAE_temp_max,quant_data.BAE_temp_quant);
        saveMin(bae_HK_minmax.Batt_voltage_min,quant_data.Batt_voltage_quant);
        saveMin(bae_HK_minmax.Batt_voltage_max,quant_data.Batt_voltage_quant);        
          
        
    }   
    firstCount=false;
}


//............................................BATTERY GAUGE......................................//
void FCTN_BATTERYGAUGE_INIT()
{
        disable_sleep();
        disable_hibernate();
        socChangeAlertEnabled(true); //enabling alert on soc changing by 1%
        emptyAlertThreshold(32);//setting empty alert threshold to 32% soc
        vAlertMinMaxThreshold();//set min, max value of Valrt register
        vResetThresholdSet();//set threshold voltage for reset
        vResetAlertEnabled(true);//enable alert on reset for V < Vreset
        int ack = write(REG_STATUS, read(REG_STATUS) & 0xFEFF);   //Clearing Reset Indicator bit
        if( ack == 0 ) eps_pc.printf("BTG init success\n\r");
        else eps_pc.printf("BTG init fail ack = %d\n\r", ack);
        write(REG_STATUS, read(REG_STATUS) & 0xFEFF);   //Clearing Reset Indicator bit
}

int FCTN_BATTERYGAUGE_MAIN(float Battery_parameters[4], float temp)
{
////        eps_pc.printf("\n\r battery gauge \n");

        //float temp=30;    //=Battery_temp  (from temp sensor on battery board)       //value of battery temperature in C currently given a dummy value. Should be updated everytime.
        int flag = tempCompensation(temp);
        //tempCompensation(temp);
        
         if( flag == 0 )
        {
            Battery_parameters[0]=vcell();
            
            timer_soc.reset();
            timer_soc.start();
            Battery_parameters[1]=soc();
            timer_soc.stop();
            
            Battery_parameters[2]=crate();
           
    ////        eps_pc.printf("\nVcell=%f",vcell());       //remove this for final code
    ////        eps_pc.printf("\nSOC=%f",soc());           //remove this for final code
    ////        eps_pc.printf("\nC_rate=%f",crate());      //remove this for final code
    
           
            if (alerting()== true)       //alert is on
            {   timer_alertFlags.reset();
                timer_alertFlags.start();
                Battery_parameters[3]=alertFlags();
                timer_alertFlags.stop();
            
                clearAlert();//clear alert
                clearAlertFlags();//clear all alert flags
            }
        }
       
        if( soc() == 200 || flag != 0) return 1;
        else return 0; 
        
}
int tempCompensation(float temp)
{
    //Calculate the new RCOMP value
    char rcomp;
    if (temp > 20.0) {
        rcomp = RCOMP0 + (temp - 20.0) * -0.5;
    } else {
        rcomp = RCOMP0 + (temp - 20.0) * -5.0;
    }
 
    //Update the RCOMP value
    return compensation(rcomp);
}

int compensation(char rcomp)
{
    //Read the current 16-bit register value
    unsigned short value = read(REG_CONFIG);
 
    //Update the register value
    value &= 0x00FF;
    value |= rcomp << 8;
 
    //Write the value back out
    return (write(REG_CONFIG, value));
}

int write(char reg, unsigned short data)
    {
        eps_btg_writ_flag = -1;
        //Create a temporary buffer
        char buff[3];
   
        //Load the register address and 16-bit data
        buff[0] = reg;
        buff[1] = data >> 8;
        buff[2] = data;
        
        int flag = m_I2C.write(m_ADDR, buff, 3);    //Write the data and return ack
        
        if( flag != 0 )
        {
            flag = m_I2C.write(m_ADDR, buff, 3);    //Write the data and return ack
            if( data != read(reg) )
                eps_btg_writ_flag = 1;
                //EPS_BATTERY_GAUGE_STATUS = 0;           //clear EPS_BATTERY_GAUGE_STATUS 
            else eps_btg_writ_flag = 0;
                 //Verify written data
                //EPS_BATTERY_GAUGE_STATUS = 0;           //clear EPS_BATTERY_GAUGE_STATUS 
        }
        
        return flag;
    }
   
unsigned short read(char reg)
    {
        eps_btg_read_flag = -1;
        int flag = 1;
        char buff[2];       //Create a temporary buffer
 
        //Select the register
        m_I2C.write(m_ADDR, &reg, 1, true);
        //Read the 16-bit register
        flag = m_I2C.read(m_ADDR, buff, 2);
        
        if( flag )
        {
            m_I2C.write(m_ADDR, &reg, 1, true);
            flag = m_I2C.read(m_ADDR, buff, 2);
            //if( flag )
            //    EPS_BATTERY_GAUGE_STATUS = 0;           //clear EPS_BATTERY_GAUGE_STATUS 
        }
 
        eps_btg_read_flag = flag;
        //Return the combined 16-bit value
        return (buff[0] << 8) | buff[1];
    } 
/*

unsigned short read(char reg)
    {
         
         //Create a temporary buffer
        char buff[2];
 
        //Select the register
        m_I2C.write(m_ADDR, &reg, 1, true);
 
        //Read the 16-bit register
        m_I2C.read(m_ADDR, buff, 2);
 
        //Return the combined 16-bit value
        return (buff[0] << 8) | buff[1];
    }
 

 

    
    void write(char reg, unsigned short data)
    {
        //Create a temporary buffer
        char buff[3];
 
        //Load the register address and 16-bit data
        buff[0] = reg;
        buff[1] = data >> 8;
        buff[2] = data;
 
        //Write the data
        m_I2C.write(m_ADDR, buff, 3);
    }
   
 
*/ 
    // Command the MAX17049 to perform a power-on reset
    void reset()
    {
        //Write the POR command
        write(REG_CMD, 0x5400);
        //Re-initialise gauge
        
        timer_FCTN_BATTERYGAUGE_INIT.reset();
        timer_FCTN_BATTERYGAUGE_INIT.start();
        FCTN_BATTERYGAUGE_INIT();
        timer_FCTN_BATTERYGAUGE_INIT.stop();
    }
    
    // Command the MAX17049 to perform a QuickStart
     void quickStart()
    {
        //Read the current 16-bit register value
        unsigned short value = read(REG_MODE);
  
        //Set the QuickStart bit
        value |= (1 << 14);
 
        //Write the value back out
        write(REG_MODE, value);
    }
    
    
   //disable sleep
   void disable_sleep()
    {
        unsigned short value = read(REG_MODE);
        value &= ~(1 << 13);
        write(REG_MODE, value);
    }
  
    //disable the hibernate  of the MAX17049
    void disable_hibernate()
    {
        write(REG_HIBRT, 0x0000);
    }
  
    
    // Enable or disable the SOC 1% change alert on the MAX17049
    void socChangeAlertEnabled(bool enabled)
    {
        //Read the current 16-bit register value
        unsigned short value = read(REG_CONFIG);
 
        //Set or clear the ALSC bit
        if (enabled)
            value |= (1 << 6);
        else
            value &= ~(1 << 6);
 
        //Write the value back out
        write(REG_CONFIG, value);
} 

/*
void compensation(char rcomp)
{
    //Read the current 16-bit register value
    unsigned short value = read(REG_CONFIG);
 
    //Update the register value
    value &= 0x00FF;
    value |= rcomp << 8;
 
    //Write the value back out
    write(REG_CONFIG, value);
}


void tempCompensation(float temp)
{
    //Calculate the new RCOMP value
    char rcomp;
    if (temp > 20.0) {
        rcomp = RCOMP0 + (temp - 20.0) * -0.5;
    } else {
        rcomp = RCOMP0 + (temp - 20.0) * -5.0;
    }
 
    //Update the RCOMP value
    compensation(rcomp);
}

*/
  // Command the MAX17049 to de-assert the ALRT pin
    void clearAlert()
    {
        //Read the current 16-bit register value
        unsigned short value = read(REG_CONFIG);
 
        //Clear the ALRT bit
        value &= ~(1 << 5);
 
        //Write the value back out
        write(REG_CONFIG, value);
    }
  
 
    //Set the SOC empty alert threshold of the MAX17049
    void emptyAlertThreshold(char threshold)
    {
        //Read the current 16-bit register value
        unsigned short value = read(REG_CONFIG);
 
        //Update the register value
        value &= 0xFFE0;
        value |= 32 - threshold;
 
        //Write the 16-bit register
        write(REG_CONFIG, value);
    }
    
    // Set the low and high voltage alert threshold of the MAX17049
    void vAlertMinMaxThreshold()
    {
        //Read the current 16-bit register value
        unsigned short value = read(REG_VALRT);
 
        //Mask off the old value
    
                value = 0x96D2;
     
        //Write the 16-bit register
        write(REG_VALRT, value);
    }

    
    // Set the reset voltage threshold of the MAX17049
    void vResetThresholdSet()
    {
        //Read the current 16-bit register value
        unsigned short value = read(REG_VRESET_ID);
 
        //Mask off the old //value
        value &= 0x00FF;//Dis=0
 
        value |= 0x9400;//corresponding to 2.5 V
    
 
        //Write the 16-bit register
        write(REG_VRESET_ID, value);
    }
    
    
    // Enable or disable the voltage reset alert on the MAX17049
     void vResetAlertEnabled(bool enabled)
    {
        //Read the current 16-bit register value
        unsigned short value = read(REG_STATUS);
    
        //Set or clear the EnVR bit
        if (enabled)
            value |= (1 << 14);
        else
            value &= ~(1 << 14);
 
        //Write the value back out
        write(REG_STATUS, value);
    }
 
    //Get the current alert flags on the MAX17049
    //refer datasheet-status registers section to decode it.
    char alertFlags()
    {
        //Read the 16-bit register value
        unsigned short value = read(REG_STATUS);
 
        //Return only the flag bits
        return (value >> 8) & 0x3F;
    }
    
    // Clear all the alert flags on the MAX17049
    void clearAlertFlags()
    {
        //Read the current 16-bit register value
        unsigned short value = read(REG_STATUS);
 
        //Clear the specified flag bits
        value &= ~( 0x3F<< 8);
 
        //Write the value back out
        write(REG_STATUS, value);
    }
    
    // Get the current cell voltage measurement of the MAX17049
    float vcell()
    {
        
        //Read the 16-bit raw Vcell value
        unsigned short value = read(REG_VCELL);
 
        //Return Vcell in volts
        return value * 0.000078125*2;
    }
    
    // Get the current state of charge measurement of the MAX17049 as a float
    float soc()
    {
      
         //Create a temporary buffer
        char buff[2];
         int ack = 1;
        //Select the register
        char reg = REG_SOC;         // cannot pass the hash defined values directly
        m_I2C.write(m_ADDR, &reg, 1, true);
        
 
        //Read the 16-bit register
       
        ack = m_I2C.read(m_ADDR, buff, 2);
            
////        eps_pc.printf("\n\r acknow %d", ack);
 
        //Return SOC in percent
        if(ack == 0)
        return ((buff[0] << 8) | buff[1]) * 0.00390625;
        else 
        return 200;
    }
    
   
 
    // Get the current C rate measurement of the MAX17049
    float crate()
    {
        //Read the 16-bit raw C/Rate value
        short value = read(REG_CRATE);
 
        //Return C/Rate in %/hr
        return value * 0.208;
    }
    
    // Determine whether or not the MAX17049 is asserting the ALRT pin
    bool alerting()
    {
        //Read the 16-bit register value
        unsigned short value = read(REG_CONFIG);
 
        //Return the status of the ALRT bit
        if (value & (1 << 5))
            return true;
        else
            return false;
    }
    
//.............................Battery board Temp sensor........................//
void FCTN_BATTTEMP_INIT()
{
    ssn1=1;ssn2=1;
    //PS=0;
    //HS=0;
    spi_bt.format(8,3);
    spi_bt.frequency(1000000);
    EPS_BTRY_TMP_STATUS = 1;
} 

void FCTN_BATT_TEMP_SENSOR_MAIN(float temp[2])
{
    uint8_t MSB, LSB;
    int16_t bit_data;
    float sensitivity=0.0078125;   //1 LSB = sensitivity degree celcius
    wait_ms(320);
    //can we reduce it further ??? azad.......!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    ssn1=0;

    spi_bt.write(0x80);//Reading digital data from Sensor 1
    LSB = spi_bt.write(0x00);//LSB first
    wait_ms(10);
    MSB = spi_bt.write(0x00);
    wait_ms(10);
////    pc_eps.eps_pc.printf("%d %d\n",MSB,LSB);
    bit_data= ((uint16_t)MSB<<8)|LSB;
    wait_ms(10);
    temp[0]=(float)bit_data*sensitivity;//Converting into decimal value 
    ssn1=1;
    wait_ms(10);
    ssn2=0;//Reading data from sensor 2
    spi_bt.write(0x80);
    LSB = spi_bt.write(0x00);
    wait_ms(10);
    MSB = spi_bt.write(0x00);
    wait_ms(10);
    bit_data= ((int16_t)MSB<<8)|LSB;
    wait_ms(10);
    temp[1]=(float)bit_data*sensitivity;
    ssn2=1;
    
}