Seeker of Truth ,
QITH FLAGS
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
TF_conops_BAE1_3
by 
Team Fox
Embed:
(wiki syntax)
Show/hide line numbers
EPS.cpp
00001 #include "EPS.h" 00002 #include "pin_config.h" 00003 /***********************************************global variable declaration***************************************************************/ 00004 extern uint32_t BAE_STATUS; 00005 extern uint32_t BAE_ENABLE; 00006 const char RCOMP0= 0x97; //? 00007 SensorData Sensor; 00008 SensorDataQuantised SensorQuantised; 00009 ShortBeacy Shortbeacon; 00010 int power_mode = 1; 00011 00012 /***********************************************Configuring Peripherals*******************************************************************/ 00013 Serial pc_eps(USBTX,USBRX); 00014 00015 I2C BG_I2C(D14,D15); //i2c btwn bae and battery gauge 00016 DigitalOut SelectLinesA[] = {PIN43,PIN44,PIN45,PIN46}; //to mux1=>voltage mux , PTA 13-16 , CHNGE TO PIN43 LATER 00017 DigitalOut SelectLinesB[] = {PIN56,PIN57,PIN58,PIN59}; //to mux2=>current mux(differential mux) , PTB 3,7,8,9 00018 //MSB is SelectLines[0],LSB is SelectLines[3] 00019 AnalogIn CurrentInput(PIN53); // output from Current Mux PTB0 00020 AnalogIn VoltageInput(PIN54); // output from Voltage Multiplexer PTB1 00021 AnalogIn VBATT(VBATT); //VBATT of battery gauge 00022 00023 SPI BTemp_spi(PTD6,PTD7,PTD5); //MOSI,MISO,SLK 00024 DigitalOut BTemp_ssn1(PTD4); //Slave select1 00025 DigitalOut BTemp_ssn2(PTD2);//Slave select2 00026 DigitalOut BTemp_PS(PTB0); 00027 DigitalOut BTemp_HS(PTB1); 00028 00029 DigitalOut TRXY(TRXY_DR_EN); //active high 00030 DigitalOut TRZ(TRZ_DR_EN); //active high 00031 //----------------------------------------------------EPS INIT---------------------------------------------------------------------------// 00032 void FCTN_EPS_INIT() 00033 { 00034 BAE_STATUS |= 0x00010000; //set EPS_INIT_STATUS flag 00035 FCTN_EPS_BG_INIT(); 00036 FCTN_EPS_BTEMP_INIT(); 00037 3V3AENBL = 1; //enable dc dc converter A 00038 // if battery gauge is initialised 00039 Sensor.soc = BG_soc(); 00040 Sensor.Vbatt = VBATT.read()*3.3; 00041 FCTN_EPS_POWERMODE(Sensor.soc); 00042 BAE_STATUS |= 0x00020000; //set EPS_BATTERY_GAUGE_STATUS 00043 //else 00044 power_level = 1; 00045 BAE_STATUS &= 0xFFFDFFFF ///set EPS_BATTERY_GAUGE_STATUS 00046 BAE_STATUS &= 0xFFFEFFFF; //clear EPS_INIT_STATUS flag 00047 } 00048 00049 //---------------------------------------------battery Temp sensor code------------------------------------------------------------------// 00050 void FCTN_EPS_BTEMP_INIT() 00051 { 00052 BTemp_ssn1=1;BTemp_ssn2=1; 00053 BTemp_PS=0; //power switch control enable 00054 BTemp_HS=0; //heater switch 00055 BTemp_spi.format(8,3); 00056 BTemp_spi.frequency(1000000); 00057 } 00058 00059 void FCTN_EPS_BTEMP_MAIN(float btry_temp[2]) 00060 { 00061 00062 uint8_t MSB, LSB; 00063 int16_t bit_data; 00064 float sensitivity=0.0078125; //1 LSB = sensitivity degree celcius 00065 wait_ms(320); 00066 BTemp_ssn1=0; 00067 BTemp_spi.write(0x80);//Reading digital data from Sensor 1 00068 LSB = BTemp_spi.write(0x00);//LSB first 00069 MSB = BTemp_spi.write(0x00); 00070 printf("%d %d\n",MSB,LSB); 00071 bit_data= ((uint16_t)MSB<<8)|LSB; 00072 btry_temp[0]=(float)bit_data*sensitivity;//Converting into decimal value 00073 BTemp_ssn1=1; 00074 BTemp_ssn2=0;//Reading data from sensor 2 00075 BTemp_spi.write(0x80); 00076 LSB = BTemp_spi.write(0x00); 00077 MSB = BTemp_spi.write(0x00); 00078 bit_data= ((int16_t)MSB<<8)|LSB; 00079 btry_temp[1]=(float)bit_data*sensitivity; 00080 BTemp_ssn2=1; 00081 printf("\n\r battery temperature %f %f ",btry_temp[0],btry_temp[1]); 00082 } 00083 00084 00085 00086 //----------------------------------------------------Battery Gauge code-----------------------------------------------------------------// 00087 00088 unsigned short BG_readReg(char reg) 00089 { 00090 //Create a temporary buffer 00091 char buff[2]; 00092 00093 //Select the register 00094 BG_I2C.write(BG_ADDR, ®, 1, true); 00095 00096 //Read the 16-bit register 00097 BG_I2C.read(BG_ADDR, buff, 2); 00098 00099 //Return the combined 16-bit value 00100 return (buff[0] << 8) | buff[1]; 00101 } 00102 00103 void BG_writeReg(char reg, unsigned short data) 00104 { 00105 //Create a temporary buffer 00106 char buff[3]; 00107 00108 //Load the register address and 16-bit data 00109 buff[0] = reg; 00110 buff[1] = data >> 8; 00111 buff[2] = data; 00112 00113 //Write the data 00114 BG_I2C.write(BG_ADDR, buff, 3); 00115 } 00116 00117 00118 00119 // Command the MAX17049 to perform a power-on reset 00120 void BG_reset() 00121 { 00122 //Write the POR command 00123 BG_writeReg(REG_CMD, 0x5400); 00124 } 00125 00126 // Command the MAX17049 to perform a QuickStart 00127 void BG_quickStart() 00128 { 00129 //Read the current 16-bit register value 00130 unsigned short value = BG_readReg(REG_MODE); 00131 00132 //Set the QuickStart bit 00133 value |= (1 << 14); 00134 00135 //Write the value back out 00136 BG_writeReg(REG_MODE, value); 00137 } 00138 00139 00140 //disable sleep 00141 void BG_disableSleep() 00142 { 00143 unsigned short value = BG_readReg(REG_MODE); 00144 value &= ~(1 << 13); 00145 BG_writeReg(REG_MODE, value); 00146 } 00147 00148 //disable the hibernate of the MAX17049 00149 void BG_disableHibernate() 00150 { 00151 BG_writeReg(REG_HIBRT, 0x0000); 00152 } 00153 00154 00155 // Enable or disable the SOC 1% change alert on the MAX17049 00156 void BG_socChangeAlertEnabled(bool enabled) 00157 { 00158 //Read the current 16-bit register value 00159 unsigned short value = BG_readReg(REG_CONFIG); 00160 00161 //Set or clear the ALSC bit 00162 if (enabled) 00163 value |= (1 << 6); 00164 else 00165 value &= ~(1 << 6); 00166 00167 //Write the value back out 00168 BG_writeReg(REG_CONFIG, value); 00169 } 00170 00171 float BG_compensation() 00172 { 00173 //Read the 16-bit register value 00174 unsigned short value = BG_readReg(REG_CONFIG); 00175 00176 //Return only the upper byte 00177 return (char)(value >> 8); 00178 } 00179 00180 void BG_compensation(char rcomp) 00181 { 00182 //Read the current 16-bit register value 00183 unsigned short value = BG_readReg(REG_CONFIG); 00184 00185 //Update the register value 00186 value &= 0x00FF; 00187 value |= rcomp << 8; 00188 00189 //Write the value back out 00190 BG_writeReg(REG_CONFIG, value); 00191 } 00192 00193 00194 void BG_tempCompensation(float temp) 00195 { 00196 //Calculate the new RCOMP value 00197 char rcomp; 00198 if (temp > 20.0) { 00199 rcomp = RCOMP0 + (temp - 20.0) * -0.5; 00200 } else { 00201 rcomp = RCOMP0 + (temp - 20.0) * -5.0; 00202 } 00203 00204 //Update the RCOMP value 00205 BG_compensation(rcomp); 00206 } 00207 00208 // Determine whether or not the MAX17049 is asserting the ALRT pin 00209 bool BG_alerting() 00210 { 00211 //Read the 16-bit register value 00212 unsigned short value = BG_readReg(REG_CONFIG); 00213 00214 //Return the status of the ALRT bit 00215 if (value & (1 << 5)) 00216 return true; 00217 else 00218 return false; 00219 } 00220 00221 // Command the MAX17049 to de-assert the ALRT pin 00222 void BG_clearAlert() 00223 { 00224 //Read the current 16-bit register value 00225 unsigned short value = BG_readReg(REG_CONFIG); 00226 00227 //Clear the ALRT bit 00228 value &= ~(1 << 5); 00229 00230 //Write the value back out 00231 BG_writeReg(REG_CONFIG, value); 00232 } 00233 00234 00235 //Set the SOC empty alert threshold of the MAX17049 00236 void BG_emptyAlertThreshold(char threshold) 00237 { 00238 //Read the current 16-bit register value 00239 unsigned short value = BG_readReg(REG_CONFIG); 00240 00241 //Update the register value 00242 value &= 0xFFE0; 00243 value |= 32 - threshold; 00244 00245 //Write the 16-bit register 00246 BG_writeReg(REG_CONFIG, value); 00247 } 00248 00249 // Set the low and high voltage alert threshold of the MAX17049 00250 void BG_vAlertMinMaxThreshold() 00251 { 00252 //Read the current 16-bit register value 00253 unsigned short value = BG_readReg(REG_VALRT); 00254 00255 //Mask off the old value 00256 value = 0x7FFF; //??????????????? 00257 00258 //Write the 16-bit register 00259 BG_writeReg(REG_VALRT, value); 00260 } 00261 00262 00263 // Set the reset voltage threshold of the MAX17049 00264 void BG_vResetThresholdSet() 00265 { 00266 //Read the current 16-bit register value 00267 unsigned short value = BG_readReg(REG_VRESET_ID); 00268 00269 //Mask off the old //value 00270 value &= 0x00FF;//Dis=0 00271 00272 value |= 0x7C00;//corresponding to 2.5 V 00273 00274 00275 //write the 16-bit register 00276 BG_writeReg(REG_VRESET_ID, value); 00277 } 00278 00279 00280 // Enable or disable the voltage reset alert on the MAX17049 00281 void BG_vResetAlertEnabled(bool enabled) 00282 { 00283 //Read the current 16-bit register value 00284 unsigned short value = BG_readReg(REG_STATUS); 00285 00286 //Set or clear the EnVR bit 00287 if (enabled) 00288 value |= (1 << 14); 00289 else 00290 value &= ~(1 << 14); 00291 00292 //Write the value back out 00293 BG_writeReg(REG_STATUS, value); 00294 } 00295 00296 //Get the current alert flags on the MAX17049 00297 //refer datasheet-status registers section to decode it. 00298 char BG_alertFlags() 00299 { 00300 //Read the 16-bit register value 00301 unsigned short value = BG_readReg(REG_STATUS); 00302 00303 //Return only the flag bits 00304 return (value >> 8) & 0x3F; 00305 } 00306 00307 // Clear all the alert flags on the MAX17049 00308 void BG_clearAlertFlags() 00309 { 00310 //Read the current 16-bit register value 00311 unsigned short value = BG_readReg(REG_STATUS); 00312 00313 //Clear the specified flag bits 00314 value &= ~( 0x3F<< 8); 00315 00316 //Write the value back out 00317 BG_writeReg(REG_STATUS, value); 00318 } 00319 00320 // Get the current cell voltage measurement of the MAX17049 00321 float BG_vcell() 00322 { 00323 //Read the 16-bit raw Vcell value 00324 unsigned short value = BG_readReg(REG_VCELL); 00325 00326 //Return Vcell in volts 00327 return value * 0.000078125*2; 00328 } 00329 00330 // Get the current state of charge measurement of the MAX17049 as a float 00331 float BG_soc() 00332 { 00333 //Read the 16-bit raw SOC value 00334 unsigned short value = BG_readReg(REG_SOC); 00335 00336 //Return SOC in percent 00337 return value * 0.00390625; 00338 } 00339 00340 00341 00342 // Get the current C rate measurement of the MAX17049 00343 float BG_crate() 00344 { 00345 //Read the 16-bit raw C/Rate value 00346 short value = BG_readReg(REG_CRATE); 00347 00348 //Return C/Rate in %/hr 00349 return value * 0.208; 00350 } 00351 00352 00353 void FCTN_EPS_BG_INIT() 00354 { 00355 BG_disableSleep(); 00356 BG_disableHibernate(); 00357 BG_socChangeAlertEnabled(true); //enabling alert on soc changing by 1% 00358 BG_emptyAlertThreshold(32);//setting empty alert threshold to 32% soc 00359 BG_vAlertMinMaxThreshold();//set min, max value of Valrt register 00360 BG_vResetThresholdSet();//set threshold voltage for reset 00361 BG_vResetAlertEnabled(true);//enable alert on reset for V < Vreset 00362 } 00363 00364 void FCTN_EPS_BG_MAIN() 00365 { 00366 float temp=Sensor.BatteryTemperature ; //(from temp sensor on battery board) //value of battery temperature in degree Celsius. Should be updated everytime. 00367 float Battery_parameters[4]; 00368 BG_tempCompensation(temp); 00369 Battery_parameters[0]=BG_vcell(); 00370 Battery_parameters[1]=BG_soc(); 00371 Battery_parameters[2]=BG_crate(); 00372 FCTN_EPS_POWERMODE(Battery_parameters[1]) ; //updating power mode 00373 printf("\nVcell=%f",BG_vcell()); //remove this for final code 00374 printf("\nSOC=%f",BG_soc()); //remove this for final code 00375 printf("\nC_rate=%f",BG_crate()); //remove this for final code 00376 if (BG_alerting()== true) //alert is on 00377 { 00378 Battery_parameters[3]=BG_alertFlags(); 00379 BG_clearAlert();//clear alert 00380 BG_clearAlertFlags();//clear all alert flags 00381 } 00382 } 00383 00384 //----------------------------------------------------Power algo code--------------------------------------------------------------------// 00385 int FCTN_EPS_POWERMODE(float soc) //dummy algo 00386 { 00387 if(soc >= 80) 00388 power_mode = 4; 00389 else if(soc >= 70 & soc < 80) 00390 power_mode = 3; 00391 else if(soc >= 60 & soc < 70) 00392 power_mode = 2; 00393 else if(soc < 60) 00394 power_mode = 1; 00395 } 00396 00397 extern int beacon_sc; 00398 extern int acs_pflag; 00399 void FCTN_EPS_CTRLPOWER(int power_mode) //algo has to be changed based on report from eps team 00400 { 00401 printf("Entered Power Management \n"); 00402 printf("Battery Level %d \n",btrylvl); 00403 switch(power_mode) 00404 { 00405 case 1: beacon_sc = 3; //high power mode : everything is on 00406 acs_pflag = 1; 00407 TRXY = 1; 00408 TRZ = 1; 00409 break; 00410 00411 case 2: beacon_sc = 3; 00412 acs_pflag = 0; //stops control algo and pwmgen in code 00413 TRXY = 0; 00414 TRZ = 0; 00415 break; 00416 00417 case 3: beacon_sc = 3; 00418 acs_pflag = 0; //stops control algo and pwmgen in code 00419 TRXY = 0; 00420 TRZ = 0; 00421 break; 00422 00423 default : beacon_sc = 30; //least power mode : beacon is in low power mode : 00424 acs_pflag = 0; //stops control algo and pwmgen in code 00425 TRXY = 0; 00426 TRZ = 0; 00427 } 00428 00429 //---------------------------------------------------func to quantize--------------------------------------------------------------------// 00430 00431 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) 00432 { 00433 int quant_data = (input_data - start)/step; 00434 if(quant_data <= 0) 00435 quant_data = 0; 00436 if(quant_data >= 15) 00437 quant_data = 15; 00438 return quant_data; 00439 } 00440 00441 //---------------------------------------------------func to fill beac structure---------------------------------------------------------// 00442 00443 00444 void FCTN_WRITE_BEASTRUCT(ShortBeacy* bea_struct,SensorDataQuantised quant_data) 00445 { 00446 (*bea_struct).Voltage[0] = 2; //quantised value 00447 (*bea_struct).Temp[0] = quant_data.Temperature[0]; //quantised value 00448 (*bea_struct).Temp[1] = quant_data.Temperature[1]; //quantised value 00449 (*bea_struct).AngularSpeed[0] = quant_data.AngularSpeed[0]; 00450 (*bea_struct).AngularSpeed[1] = quant_data.AngularSpeed[1]; 00451 00452 (*bea_struct).SubsystemStatus[0] = 145; //dummy values----------to be changed------------------- 00453 (*bea_struct).ErrorFlag[0] = 3; //dummy values----------to be changed------------------- 00454 } 00455 00456 //---------------------------------------------------HK_MAIN-----------------------------------------------------------------------// 00457 00458 void FCTN_EPS_HK_MAIN() 00459 { 00460 SelectLinesA[0] = SelectLinesA[1] = SelectLinesA[2] = SelectLinesA[3] = 0; 00461 SelectLinesB[3]= SelectLinesB[2] = SelectLinesB[1]=SelectLinesB[0] = 0; //initialise all selectlines to zeroes->1st line of muxes selected 00462 int loop_iterator = 0; 00463 int select_line_iterator = 3; 00464 00465 for(loop_iterator = 0; loop_iterator < 16; loop_iterator++) //measurement from voltage sensor=> 16 sensors in place 00466 { 00467 if(loop_iterator<15) 00468 { 00469 //read the sensor values and stores them in 'SensorData' structure's variable 'Sensor' 00470 Sensor.Voltage[loop_iterator] = (VoltageInput.read()*3.3*5.545454);//resistors in voltage divider=>15Mohm,3.3Mohm 00471 00472 if(loop_iterator%2 == 0) 00473 SensorQuantised.Voltage[loop_iterator/2] = FCTN_QUANTIZE(vstart,vstep,Sensor.Voltage[loop_iterator]); 00474 00475 else 00476 SensorQuantised.Voltage[(loop_iterator)/2] = SensorQuantised.Voltage[(loop_iterator)/2]<<4 + FCTN_QUANTIZE(vstart,vstep,Sensor.Voltage[loop_iterator]); 00477 } 00478 else 00479 { 00480 Sensor.Temperature[1] = (VoltageInput.read()*3.3*(-90.7)+ 190.1543); 00481 SensorQuantised.Temperature[0]=FCTN_QUANTIZE(tstart,tstep,Sensor.Temperature[0]); 00482 } 00483 //iterate the select lines from 0 to 15 00484 for(select_line_iterator = 3;select_line_iterator >= 0;select_line_iterator--) 00485 { 00486 if(SelectLinesA[select_line_iterator] == 0) 00487 { 00488 SelectLinesA[select_line_iterator] = 1; 00489 break; 00490 } 00491 else SelectLinesA[select_line_iterator] = 0; 00492 } 00493 00494 wait_us(10.0); // A delay of 10 microseconds between each sensor output. Can be changed. 00495 } 00496 00497 00498 //measurement from current sensor=> 8 sensors in place 00499 for(loop_iterator = 0; loop_iterator < 7; loop_iterator++) 00500 { 00501 //read the sensor values and stores them in 'SensorData' structure variable 'Sensor' 00502 Sensor.Current[loop_iterator] = (CurrentInput.read()*3.3/(50*rsens)); 00503 if(loop_iterator%2 == 0) 00504 SensorQuantised.Current[loop_iterator/2] = FCTN_QUANTIZE(cstart,cstep,Sensor.Current[loop_iterator]); 00505 else 00506 SensorQuantised.Current[(loop_iterator)/2] = SensorQuantised.Current[(loop_iterator)/2]<<4 + FCTN_QUANTIZE(cstart,cstep,Sensor.Current[loop_iterator]); 00507 00508 //iterate the select lines from 0 to 7 00509 for(select_line_iterator = 3;select_line_iterator >= 0;select_line_iterator--) 00510 { 00511 if(SelectLinesB[select_line_iterator] == 0) 00512 { 00513 SelectLinesB[select_line_iterator] = 1; 00514 break; 00515 } 00516 else SelectLinesB[select_line_iterator] = 0; 00517 00518 } 00519 00520 wait_us(10.0); // A delay of 10 microseconds between each sensor output. Can be changed. 00521 } 00522 00523 00524 00525 //update magnetometer data-> 00526 //populate values in structure variable 'Sensor' from data to be given by Green 00527 SensorQuantised.AngularSpeed[0] = FCTN_QUANTIZE(AngularSpeed_start,AngularSpeed_step,Sensor.AngularSpeed[0]); 00528 SensorQuantised.AngularSpeed[0] = SensorQuantised.AngularSpeed[0]<<4 + FCTN_QUANTIZE(AngularSpeed_start,AngularSpeed_step,Sensor.AngularSpeed[1]); 00529 SensorQuantised.AngularSpeed[1] = FCTN_QUANTIZE(AngularSpeed_start,AngularSpeed_step,Sensor.AngularSpeed[2]); 00530 00531 //update gyro data-> 00532 //populate values in structure variable 'Sensor' from data to be given by Green 00533 SensorQuantised.Bnewvalue[0] = FCTN_QUANTIZE(Bnewvalue_start,Bnewvalue_step,Sensor.Bnewvalue[0]); 00534 SensorQuantised.Bnewvalue[0] = SensorQuantised.Bnewvalue[0]<<4 + FCTN_QUANTIZE(Bnewvalue_start,Bnewvalue_step,Sensor.Bnewvalue[1]); 00535 SensorQuantised.Bnewvalue[1] = FCTN_QUANTIZE(Bnewvalue_start,Bnewvalue_step,Sensor.Bnewvalue[2]); 00536 00537 //update beacon structure 00538 FCTN_WRITE_BEASTRUCT(&Shortbeacon,SensorQuantised);//Shortbeacon is passed 00539 00540 } 00541 00542
Generated on Tue Jul 12 2022 23:01:05 by
1.7.2