Seeker of Truth ,
QITH FLAGS
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
TF_conops_BAE1_3
by 
Team Fox
Diff: EPS.cpp
- Revision:
- 1:7185136654ce
- Parent:
- 0:246d1b5f11ae
--- a/EPS.cpp Tue Jun 30 05:55:48 2015 +0000
+++ b/EPS.cpp Mon Jul 06 07:34:00 2015 +0000
@@ -1,15 +1,432 @@
#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()
{
- //FCTN_BTRY_GAUGE //read soc value
- //FCTN_BTRY_TEMP
+ 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, ®, 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)
{
@@ -20,41 +437,49 @@
quant_data = 15;
return quant_data;
}
+
+//---------------------------------------------------func to fill beac structure---------------------------------------------------------//
+
-void FCTN_WRITE_BEASTRUCT(ShortBeacy* x,SensorDataQuantised y)
+void FCTN_WRITE_BEASTRUCT(ShortBeacy* bea_struct,SensorDataQuantised quant_data)
{
- (*x).Voltage[0] = 2; //quantised value
- (*x).Temp[0] = y.PanelTemperature[0]; //quantised value
- (*x).Temp[1] = y.PanelTemperature[1]; //quantised value
- (*x).AngularSpeed[0] = y.AngularSpeed[0];
- (*x).AngularSpeed[1] = y.AngularSpeed[1];
+ (*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];
- (*x).SubsystemStatus[0] = 145; //dummy values----------to be changed-------------------
- (*x).ErrorFlag[0] = 3; //dummy values----------to be changed-------------------
+ (*bea_struct).SubsystemStatus[0] = 145; //dummy values----------to be changed-------------------
+ (*bea_struct).ErrorFlag[0] = 3; //dummy values----------to be changed-------------------
}
-SensorData Sensor;
-SensorDataQuantised SensorQuantised;
-ShortBeacy Shortbeacon;
+//---------------------------------------------------HK_MAIN-----------------------------------------------------------------------//
-void FCTN_EPS_MAIN()
+void FCTN_EPS_HK_MAIN()
{
SelectLinesA[0] = SelectLinesA[1] = SelectLinesA[2] = SelectLinesA[3] = 0;
- SelectLinesB[0] = SelectLinesB[1] = SelectLinesB[2] = 0; //initialise all selectlines to zeroes->1st line of muxes selected
+ 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
{
- //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<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]);
+ 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
- SensorQuantised.Voltage[(loop_iterator)/2] = SensorQuantised.Voltage[(loop_iterator)/2]<<4 + FCTN_QUANTIZE(vstart,vstep,Sensor.Voltage[loop_iterator]);
-
+ {
+ 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--)
{
@@ -71,7 +496,7 @@
//measurement from current sensor=> 8 sensors in place
- for(loop_iterator = 0; loop_iterator < 8; loop_iterator++)
+ 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));
@@ -79,9 +504,9 @@
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 = 2;select_line_iterator >= 0;select_line_iterator--)
+ for(select_line_iterator = 3;select_line_iterator >= 0;select_line_iterator--)
{
if(SelectLinesB[select_line_iterator] == 0)
{
@@ -108,11 +533,10 @@
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
-
+ FCTN_WRITE_BEASTRUCT(&Shortbeacon,SensorQuantised);//Shortbeacon is passed
+
}
-