File content as of revision 2:3c6c33509772:

#include "EPS.h"
#include "pin_config.h"
/***********************************************global variable declaration***************************************************************/
extern uint32_t BAE_STATUS;
extern uint32_t BAE_ENABLE;
const char RCOMP0= 0x97;                  //?
SensorData Sensor; 
SensorDataQuantised SensorQuantised;
ShortBeacy Shortbeacon;
int power_mode = 1;

/***********************************************Configuring Peripherals*******************************************************************/
Serial pc_eps(USBTX,USBRX);

I2C BG_I2C(D14,D15);                                           //i2c btwn bae and battery gauge
DigitalOut SelectLinesA[] = {PIN43,PIN44,PIN45,PIN46};         //to mux1=>voltage mux , PTA 13-16 , CHNGE TO PIN43 LATER
DigitalOut SelectLinesB[] = {PIN56,PIN57,PIN58,PIN59};         //to mux2=>current mux(differential mux) , PTB 3,7,8,9
                                                               //MSB is SelectLines[0],LSB is SelectLines[3]
AnalogIn CurrentInput(PIN53);                                  // output from Current Mux PTB0
AnalogIn VoltageInput(PIN54);                                  // output from Voltage Multiplexer  PTB1
AnalogIn VBATT(VBATT);                                         //VBATT of battery gauge

SPI BTemp_spi(PTD6,PTD7,PTD5); //MOSI,MISO,SLK
DigitalOut BTemp_ssn1(PTD4); //Slave select1
DigitalOut BTemp_ssn2(PTD2);//Slave select2
DigitalOut BTemp_PS(PTB0);
DigitalOut BTemp_HS(PTB1);

DigitalOut TRXY(TRXY_DR_EN);            //active high
DigitalOut TRZ(TRZ_DR_EN);              //active high
//----------------------------------------------------EPS INIT---------------------------------------------------------------------------//
void FCTN_EPS_INIT()
{
    BAE_STATUS |= 0x00010000;                //set EPS_INIT_STATUS flag
    FCTN_EPS_BG_INIT();                     
    FCTN_EPS_BTEMP_INIT();
    3V3AENBL = 1;                          //enable dc dc converter A
    // if battery gauge is initialised
    Sensor.soc = BG_soc();
    Sensor.Vbatt = VBATT.read()*3.3;
    FCTN_EPS_POWERMODE(Sensor.soc);
    BAE_STATUS |= 0x00020000;               //set EPS_BATTERY_GAUGE_STATUS
    //else
    power_level = 1; 
    BAE_STATUS &= 0xFFFDFFFF               ///set EPS_BATTERY_GAUGE_STATUS
    BAE_STATUS &= 0xFFFEFFFF;              //clear EPS_INIT_STATUS flag
}

//---------------------------------------------battery Temp sensor code------------------------------------------------------------------//
void FCTN_EPS_BTEMP_INIT()
{
    BTemp_ssn1=1;BTemp_ssn2=1;
    BTemp_PS=0;             //power switch control enable
    BTemp_HS=0;             //heater switch
    BTemp_spi.format(8,3);
    BTemp_spi.frequency(1000000);
}

void FCTN_EPS_BTEMP_MAIN(float btry_temp[2])
{
    
    uint8_t MSB, LSB;
    int16_t bit_data;
    float sensitivity=0.0078125;   //1 LSB = sensitivity degree celcius
    wait_ms(320);
    BTemp_ssn1=0;
    BTemp_spi.write(0x80);//Reading digital data from Sensor 1
    LSB = BTemp_spi.write(0x00);//LSB first
    MSB = BTemp_spi.write(0x00);
    printf("%d %d\n",MSB,LSB);
    bit_data= ((uint16_t)MSB<<8)|LSB;
    btry_temp[0]=(float)bit_data*sensitivity;//Converting into decimal value 
    BTemp_ssn1=1;
    BTemp_ssn2=0;//Reading data from sensor 2
    BTemp_spi.write(0x80);
    LSB = BTemp_spi.write(0x00);
    MSB = BTemp_spi.write(0x00);
    bit_data= ((int16_t)MSB<<8)|LSB;
    btry_temp[1]=(float)bit_data*sensitivity;
    BTemp_ssn2=1;
    printf("\n\r battery temperature %f %f ",btry_temp[0],btry_temp[1]);
}



//----------------------------------------------------Battery Gauge code-----------------------------------------------------------------//

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

void BG_writeReg(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
    BG_I2C.write(BG_ADDR, buff, 3);
}
   
 
 
// Command the MAX17049 to perform a power-on reset
void BG_reset()                         
{
    //Write the POR command
    BG_writeReg(REG_CMD, 0x5400);
}
    
// Command the MAX17049 to perform a QuickStart
void BG_quickStart()                    
{
    //Read the current 16-bit register value
    unsigned short value = BG_readReg(REG_MODE);
 
    //Set the QuickStart bit
    value |= (1 << 14);
 
    //Write the value back out
    BG_writeReg(REG_MODE, value);
}
    
    
//disable sleep
void BG_disableSleep()
{
    unsigned short value = BG_readReg(REG_MODE);
    value &= ~(1 << 13);
    BG_writeReg(REG_MODE, value);
}
  
//disable the hibernate  of the MAX17049
void BG_disableHibernate()
{
    BG_writeReg(REG_HIBRT, 0x0000);
}
  
    
// Enable or disable the SOC 1% change alert on the MAX17049
void BG_socChangeAlertEnabled(bool enabled)
{
    //Read the current 16-bit register value
    unsigned short value = BG_readReg(REG_CONFIG);
 
    //Set or clear the ALSC bit
    if (enabled)
        value |= (1 << 6);
    else
        value &= ~(1 << 6);
 
    //Write the value back out
    BG_writeReg(REG_CONFIG, value);
} 

float BG_compensation()
{
    //Read the 16-bit register value
    unsigned short value = BG_readReg(REG_CONFIG);
 
    //Return only the upper byte
    return (char)(value >> 8);
}
 
void BG_compensation(char rcomp)
{
    //Read the current 16-bit register value
    unsigned short value = BG_readReg(REG_CONFIG);
 
    //Update the register value
    value &= 0x00FF;
    value |= rcomp << 8;
 
    //Write the value back out
    BG_writeReg(REG_CONFIG, value);
}

 
void BG_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
    BG_compensation(rcomp);
}

// Determine whether or not the MAX17049 is asserting the ALRT pin
bool BG_alerting()
{
    //Read the 16-bit register value
    unsigned short value = BG_readReg(REG_CONFIG);
 
    //Return the status of the ALRT bit
    if (value & (1 << 5))
        return true;
    else
        return false;
}
    
// Command the MAX17049 to de-assert the ALRT pin
void BG_clearAlert()
{
    //Read the current 16-bit register value
    unsigned short value = BG_readReg(REG_CONFIG);

    //Clear the ALRT bit
    value &= ~(1 << 5);
 
    //Write the value back out
    BG_writeReg(REG_CONFIG, value);
}
  
 
//Set the SOC empty alert threshold of the MAX17049
void BG_emptyAlertThreshold(char threshold)
{
    //Read the current 16-bit register value
    unsigned short value = BG_readReg(REG_CONFIG);

    //Update the register value
    value &= 0xFFE0;
    value |= 32 - threshold;
 
    //Write the 16-bit register
    BG_writeReg(REG_CONFIG, value);
}
    
// Set the low and high voltage alert threshold of the MAX17049
void BG_vAlertMinMaxThreshold()
{
    //Read the current 16-bit register value
    unsigned short value = BG_readReg(REG_VALRT);
 
    //Mask off the old value
    value = 0x7FFF;                              //???????????????
     
    //Write the 16-bit register
    BG_writeReg(REG_VALRT, value);
}

    
// Set the reset voltage threshold of the MAX17049
void BG_vResetThresholdSet()
{
    //Read the current 16-bit register value
    unsigned short value = BG_readReg(REG_VRESET_ID);
 
    //Mask off the old //value
    value &= 0x00FF;//Dis=0
 
    value |= 0x7C00;//corresponding to 2.5 V
    
 
    //write the 16-bit register
    BG_writeReg(REG_VRESET_ID, value);
}
        
    
// Enable or disable the voltage reset alert on the MAX17049
void BG_vResetAlertEnabled(bool enabled)
{
    //Read the current 16-bit register value
    unsigned short value = BG_readReg(REG_STATUS);
    
    //Set or clear the EnVR bit
    if (enabled)
        value |= (1 << 14);
    else
        value &= ~(1 << 14);
 
    //Write the value back out
    BG_writeReg(REG_STATUS, value);
}
 
//Get the current alert flags on the MAX17049
//refer datasheet-status registers section to decode it.
char BG_alertFlags()
{
    //Read the 16-bit register value
    unsigned short value = BG_readReg(REG_STATUS);
 
    //Return only the flag bits
    return (value >> 8) & 0x3F;
}
    
// Clear all the alert flags on the MAX17049
void BG_clearAlertFlags()
{
    //Read the current 16-bit register value
    unsigned short value = BG_readReg(REG_STATUS);
 
    //Clear the specified flag bits
    value &= ~( 0x3F<< 8);
 
    //Write the value back out
    BG_writeReg(REG_STATUS, value);
}
    
// Get the current cell voltage measurement of the MAX17049
float BG_vcell()
{
    //Read the 16-bit raw Vcell value
    unsigned short value = BG_readReg(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 BG_soc()
{
    //Read the 16-bit raw SOC value
    unsigned short value = BG_readReg(REG_SOC);
 
    //Return SOC in percent
    return value * 0.00390625;
}
    
   
 
// Get the current C rate measurement of the MAX17049
float BG_crate()
{
    //Read the 16-bit raw C/Rate value
    short value = BG_readReg(REG_CRATE);
 
    //Return C/Rate in %/hr
    return value * 0.208;
}

 
void FCTN_EPS_BG_INIT()
{
    BG_disableSleep();
    BG_disableHibernate();
    BG_socChangeAlertEnabled(true); //enabling alert on soc changing by 1%
    BG_emptyAlertThreshold(32);//setting empty alert threshold to 32% soc
    BG_vAlertMinMaxThreshold();//set min, max value of Valrt register
    BG_vResetThresholdSet();//set threshold voltage for reset
    BG_vResetAlertEnabled(true);//enable alert on reset for V < Vreset
}

void FCTN_EPS_BG_MAIN()
{
    float temp=Sensor.BatteryTemperature ; //(from temp sensor on battery board)       //value of battery temperature in degree Celsius. Should be updated everytime.
    float Battery_parameters[4];
    BG_tempCompensation(temp);
    Battery_parameters[0]=BG_vcell();
    Battery_parameters[1]=BG_soc();
    Battery_parameters[2]=BG_crate();
    FCTN_EPS_POWERMODE(Battery_parameters[1]) ;    //updating power mode
    printf("\nVcell=%f",BG_vcell());       //remove this for final code
    printf("\nSOC=%f",BG_soc());           //remove this for final code
    printf("\nC_rate=%f",BG_crate());      //remove this for final code
    if (BG_alerting()== true)       //alert is on
    {   
        Battery_parameters[3]=BG_alertFlags();
        BG_clearAlert();//clear alert
        BG_clearAlertFlags();//clear all alert flags
    }
}

//----------------------------------------------------Power algo code--------------------------------------------------------------------//
int FCTN_EPS_POWERMODE(float soc)              //dummy algo
{
    if(soc >= 80)
        power_mode = 4;
    else if(soc >= 70 & soc < 80)
        power_mode = 3;
    else if(soc >= 60 & soc < 70)
        power_mode = 2;
    else if(soc < 60)
        power_mode = 1;
}

extern int beacon_sc;
extern int acs_pflag;
void FCTN_EPS_CTRLPOWER(int power_mode)                  //algo has to be changed based on report from eps team
{
    printf("Entered Power Management \n"); 
    printf("Battery Level %d \n",btrylvl);
    switch(power_mode)
    {
        case 1: beacon_sc = 3;             //high power mode : everything is on
                acs_pflag = 1;             
                TRXY = 1;
                TRZ = 1; 
                break;
        
        case 2: beacon_sc = 3;
                acs_pflag = 0;              //stops control algo and pwmgen in code
                TRXY = 0;
                TRZ = 0; 
                break;    
                
        case 3: beacon_sc = 3;
                acs_pflag = 0;              //stops control algo and pwmgen in code
                TRXY = 0;
                TRZ = 0;
                break; 
        
        default : beacon_sc = 30;             //least power mode  : beacon is in low power mode : 
                  acs_pflag = 0;              //stops control algo and pwmgen in code
                  TRXY = 0;
                  TRZ = 0;  
}
  
//---------------------------------------------------func to quantize--------------------------------------------------------------------//

int FCTN_QUANTIZE(float start,float step,float input_data)                  // accepts min and measured values and step->quantises on a scale 0-15..(4 bit quantisation)
{
    int quant_data = (input_data - start)/step;
    if(quant_data <= 0)
         quant_data = 0;
    if(quant_data >= 15)
         quant_data = 15;
    return quant_data;
}

//---------------------------------------------------func to fill beac structure---------------------------------------------------------//

 
void FCTN_WRITE_BEASTRUCT(ShortBeacy* bea_struct,SensorDataQuantised quant_data)  
{
    (*bea_struct).Voltage[0] = 2;                                       //quantised value
    (*bea_struct).Temp[0] = quant_data.Temperature[0];                      //quantised value
    (*bea_struct).Temp[1] = quant_data.Temperature[1];                      //quantised value
    (*bea_struct).AngularSpeed[0] = quant_data.AngularSpeed[0];
    (*bea_struct).AngularSpeed[1] = quant_data.AngularSpeed[1];
    
    (*bea_struct).SubsystemStatus[0] = 145;                             //dummy values----------to be changed-------------------
    (*bea_struct).ErrorFlag[0] = 3;                                     //dummy values----------to be changed-------------------
}

//---------------------------------------------------HK_MAIN-----------------------------------------------------------------------//

void FCTN_EPS_HK_MAIN()             
{
    SelectLinesA[0] = SelectLinesA[1] = SelectLinesA[2] = SelectLinesA[3] = 0;
    SelectLinesB[3]= SelectLinesB[2] = SelectLinesB[1]=SelectLinesB[0] = 0;  //initialise all selectlines to zeroes->1st line of muxes selected
    int loop_iterator = 0;
    int select_line_iterator = 3;
        
    for(loop_iterator = 0; loop_iterator < 16; loop_iterator++)  //measurement from voltage sensor=> 16 sensors in place
    {       
        if(loop_iterator<15)
        {
            //read the sensor values and stores them in 'SensorData' structure's variable 'Sensor'
            Sensor.Voltage[loop_iterator] = (VoltageInput.read()*3.3*5.545454);//resistors in voltage divider=>15Mohm,3.3Mohm
               
            if(loop_iterator%2 == 0)
                SensorQuantised.Voltage[loop_iterator/2] = FCTN_QUANTIZE(vstart,vstep,Sensor.Voltage[loop_iterator]);
                           
            else
                SensorQuantised.Voltage[(loop_iterator)/2] = SensorQuantised.Voltage[(loop_iterator)/2]<<4 + FCTN_QUANTIZE(vstart,vstep,Sensor.Voltage[loop_iterator]);
        }
        else
            {
                Sensor.Temperature[1] = (VoltageInput.read()*3.3*(-90.7)+ 190.1543);
                SensorQuantised.Temperature[0]=FCTN_QUANTIZE(tstart,tstep,Sensor.Temperature[0]);
            }
        //iterate the select lines from 0 to 15
        for(select_line_iterator = 3;select_line_iterator >= 0;select_line_iterator--)
        {
            if(SelectLinesA[select_line_iterator] == 0)
            {
                SelectLinesA[select_line_iterator] = 1;
                break;
            }
            else SelectLinesA[select_line_iterator] = 0;
        }
    
        wait_us(10.0); //  A delay of 10 microseconds between each sensor output. Can be changed.
    }
 
 
    //measurement from current sensor=>  8 sensors in place 
    for(loop_iterator = 0; loop_iterator < 7; loop_iterator++) 
    {       
        //read the sensor values and stores them in 'SensorData' structure variable 'Sensor'
        Sensor.Current[loop_iterator] = (CurrentInput.read()*3.3/(50*rsens));
        if(loop_iterator%2 == 0)
            SensorQuantised.Current[loop_iterator/2] = FCTN_QUANTIZE(cstart,cstep,Sensor.Current[loop_iterator]);
        else
            SensorQuantised.Current[(loop_iterator)/2] = SensorQuantised.Current[(loop_iterator)/2]<<4 + FCTN_QUANTIZE(cstart,cstep,Sensor.Current[loop_iterator]);
 
        //iterate the select lines from 0 to 7
        for(select_line_iterator = 3;select_line_iterator >= 0;select_line_iterator--)
        {
            if(SelectLinesB[select_line_iterator] == 0)
            {
                SelectLinesB[select_line_iterator] = 1;
                break;
            }
            else SelectLinesB[select_line_iterator] = 0;
    
        }
    
        wait_us(10.0); //  A delay of 10 microseconds between each sensor output. Can be changed.
    }
    
    
 
    //update magnetometer data->
    //populate values in structure variable 'Sensor' from data to be given by Green
     SensorQuantised.AngularSpeed[0] = FCTN_QUANTIZE(AngularSpeed_start,AngularSpeed_step,Sensor.AngularSpeed[0]);
     SensorQuantised.AngularSpeed[0] = SensorQuantised.AngularSpeed[0]<<4 + FCTN_QUANTIZE(AngularSpeed_start,AngularSpeed_step,Sensor.AngularSpeed[1]);
     SensorQuantised.AngularSpeed[1] = FCTN_QUANTIZE(AngularSpeed_start,AngularSpeed_step,Sensor.AngularSpeed[2]);
     
    //update gyro data->
    //populate values in structure variable 'Sensor' from data to be given by Green
     SensorQuantised.Bnewvalue[0] = FCTN_QUANTIZE(Bnewvalue_start,Bnewvalue_step,Sensor.Bnewvalue[0]);
     SensorQuantised.Bnewvalue[0] = SensorQuantised.Bnewvalue[0]<<4 + FCTN_QUANTIZE(Bnewvalue_start,Bnewvalue_step,Sensor.Bnewvalue[1]);
     SensorQuantised.Bnewvalue[1] = FCTN_QUANTIZE(Bnewvalue_start,Bnewvalue_step,Sensor.Bnewvalue[2]);
 
     //update beacon structure
     FCTN_WRITE_BEASTRUCT(&Shortbeacon,SensorQuantised);//Shortbeacon is passed
    
}