Master_BMS/BMS Master/Sources/BMS_SoC_estimator.c

/*
 * BMS_SoC_estimator.c
 *
 *  Created on: Jul 18, 2016
 *      Author: le8041
 */


/**
* @file BMS_SoC_estimator.c
* @brief SoC Estimator as implemented on embedded System
*/
#include "BMS_Master.h"





static float const_Ri_charge[BMS_SOC_NR_OF_RI_POINTS]={2.0,2.700}; 
static int8_t const_Ri_charge_temp[BMS_SOC_NR_OF_RI_POINTS]={10,20};

static float const_Ri_discharge[BMS_SOC_NR_OF_RI_POINTS]= {2.692E-3,2.700E-3};
static int8_t const_Ri_discharge_temp[BMS_SOC_NR_OF_RI_POINTS] = {10,20};


/*
* @brief setting initial SoC soly dependend from the Battery voltage
* @param voltage Cell Voltage
*/


float calc_start_SoC(uint16_t voltage) {
	// set initial Look up Table
	uint8_t LutSize=6;
	uint8_t i=0;
	float offset;
	MASTER_SOC_ESTIMATOR_INIT_SOC_LUT_t  init_SoC_Lut[6]; 
	init_SoC_Lut[0].SoC=0;
	init_SoC_Lut[0].OCV=2900;
	init_SoC_Lut[1].SoC=0.0665;
	init_SoC_Lut[1].OCV=3166;
	init_SoC_Lut[2].SoC=0.285;
	init_SoC_Lut[2].OCV=3281;
	init_SoC_Lut[3].SoC=0.6621;
	init_SoC_Lut[3].OCV=3320;
	init_SoC_Lut[4].SoC=0.94;
	init_SoC_Lut[4].OCV=3379;
	init_SoC_Lut[5].SoC=1.0;
	init_SoC_Lut[5].OCV=3450;
	
	
	// calc SoC from Current
	
	if( voltage <= init_SoC_Lut[0].OCV) {
		// SoC is 0
		return  init_SoC_Lut[0].SoC;
	}
	
	if( voltage >= init_SoC_Lut[5].OCV) {
		// SoC is 1
		return  init_SoC_Lut[5].SoC;
	}
	
	for(i=0;i< LutSize-1;i++) {
		// SoC is between 0 and 1
		// find area in SoC LUT
		if(init_SoC_Lut[i].OCV <= voltage && init_SoC_Lut[i+1].OCV >= voltage) {
			// SoC area between i and i+1
			// linear interpolation of SoC
			offset=( init_SoC_Lut[i+1].SoC - init_SoC_Lut[i].SoC) / (init_SoC_Lut[i+1].OCV - init_SoC_Lut[i].OCV ) *  (voltage -  init_SoC_Lut[i].OCV );
			
			return (offset + init_SoC_Lut[i].SoC);
		}
		
	}
	
	return -1;
}

/*
* @brief set initial Constants for SoC estimation Algorithm
* @param cell_SoC pointer to SoC estimation struct
* @param voltage cell voltage for dertermination of initial SoC after System power up
*/

int32_t bms_SoC_init_estimator(MASTER_SOC_ESTIMATOR_t* cell_SoC, uint16_t voltage) {
	uint8_t i;
	cell_SoC->capacity=26.0; // initial capacity 25.0 Ah
	cell_SoC->delta_t = 0.25;  // intergration constant in seconds
	
	
	cell_SoC->delta_t =  cell_SoC->delta_t /(60*60) ;  // integration constant in hours

	// set initial SoC
	cell_SoC->SoC_percentage=calc_start_SoC(voltage) ;
	cell_SoC->SoC_percentage_smooth= cell_SoC->SoC_percentage;
	cell_SoC->SoC_Ah=  cell_SoC->SoC_percentage * cell_SoC->capacity ;
	
	// set charge thresholds
	cell_SoC->ChargeThreshold.Ri_charge= 2.0 ; // Ri in mOhm
	cell_SoC->ChargeThreshold.U_bat_max=3450;
	cell_SoC->ChargeThreshold.U_ocv_correct=3379;
	cell_SoC->ChargeThreshold.U_ocv_SoC=0.94	;
	// set discharge thresholgs
	cell_SoC->DischargeThreshold.Ri_discharge =2.7; // Ri in mOhm 
	cell_SoC->DischargeThreshold.U_bat_min=2900;
	cell_SoC->DischargeThreshold.U_ocv_correct=3166;
	cell_SoC->DischargeThreshold.U_ocv_SoC=0.0665;
	// set initail Derating
	cell_SoC->MaxBatteryChargeCurrent=bms_calc_charge_derating(cell_SoC->SoC_percentage);
	cell_SoC->MaxBatteryDischargeCurrent=bms_calc_discharge_derating(cell_SoC->SoC_percentage);
	
	cell_SoC->state=BMS_SOC_IDLE;
	cell_SoC->flags=0;
	cell_SoC->ChargeRelaxationWaitCnt=0;
	cell_SoC->DischargeRelaxationWaitCnt=0; 
	cell_SoC->ChargeSmoothFlags=0;
	cell_SoC->Ocv_calc=voltage;

	return 0;
	
}

/*
* @brief set initial Constants for SoC estimation Algorithm
* @param cell_SoC pointer to SoC estimation struct
* @param voltage cell voltage for dertermination of initial SoC after System power up
*/

int32_t bms_SoC_init_estimator_FRAM(MASTER_SOC_ESTIMATOR_t* cell_SoC, uint16_t voltage) {
	float SoC=50.0;
	cell_SoC->capacity=26.0; // initial capacity 25.0 Ah
	cell_SoC->delta_t = 0.25;  // intergration constant in seconds
	//read_fram_float(&SoC,10,BMS_STARTUP_SOC);
	SoC= read_fram_get_SoC();
	
	cell_SoC->delta_t =  cell_SoC->delta_t /(60*60) ;  // integration constant in hours

	// set initial SoC
	cell_SoC->SoC_percentage=SoC ;
	cell_SoC->SoC_percentage_smooth= cell_SoC->SoC_percentage;
	cell_SoC->SoC_Ah=  cell_SoC->SoC_percentage * cell_SoC->capacity ;
	
	// set charge thresholds
	cell_SoC->ChargeThreshold.Ri_charge= 2.0 ; // Ri in mOhm
	cell_SoC->ChargeThreshold.U_bat_max=3450;
	cell_SoC->ChargeThreshold.U_ocv_correct=3379;
	cell_SoC->ChargeThreshold.U_ocv_SoC=0.94	;
	// set discharge thresholgs
	cell_SoC->DischargeThreshold.Ri_discharge =2.7; // Ri in mOhm 
	cell_SoC->DischargeThreshold.U_bat_min=2900;
	cell_SoC->DischargeThreshold.U_ocv_correct=3166;
	cell_SoC->DischargeThreshold.U_ocv_SoC=0.0665;
	// set initail Derating
	cell_SoC->MaxBatteryChargeCurrent=bms_calc_charge_derating(cell_SoC->SoC_percentage);
	cell_SoC->MaxBatteryDischargeCurrent=bms_calc_discharge_derating(cell_SoC->SoC_percentage);
	
	cell_SoC->state=BMS_SOC_IDLE;
	cell_SoC->flags=0;
	cell_SoC->ChargeRelaxationWaitCnt=0;
	cell_SoC->DischargeRelaxationWaitCnt=0; 
	cell_SoC->ChargeSmoothFlags=0;
	cell_SoC->Ocv_calc=voltage;

	return 0;
	
}

/*
* @brief finite State machien for the estimation of the State of charge
* @param est pointer to estimator struct
* @param I_batt current in mA
* @param U_batt_max maximum Cell Voltage in System in mV
* @param U_batt_min minimum Cell Voltage in System in mV
* @param temp_batt temperature of battery
*/

uint32_t bms_SoC_running_fsm(MASTER_SOC_ESTIMATOR_t* est, int16_t I_batt, uint16_t U_batt_max,uint16_t U_batt_min, int8_t temp_min, int8_t temp_max) {
	
	uint16_t OCV; // Open Clamp Voltage according to battery Model
	float R_i=0;
	switch(est->state) {
		case BMS_SOC_IDLE:
			// idle state
			// check if battery is charged, discharged or resting

			if(I_batt < -1*BMS_SOC_MIN_CURRENT_MA) {
				//I_bat < 0 => Battery is charged
				est->state=BMS_SOC_CHARGE;
				return 0;
			}
			else if(I_batt > BMS_SOC_MIN_CURRENT_MA) {
				//I_bat > 0 => Battery is discharged
				est->state=BMS_SOC_DISCHARGE;
				return 0;
			}
			else {
				// Battery is at rest
				est->state=BMS_SOC_REST;
				return 0;
			}			
			
			return -1;
		break;
		case BMS_SOC_CHARGE:
			// Battery is charged
			// if Battery has been discharged to 0% SoC remove Discharge Flag
			if(est->flags==BMS_SOC_FLAG_SOC_0 && est->SoC_percentage > est->DischargeThreshold.U_ocv_SoC) {
				est->flags=0;		
			}
			
			if(est->flags==BMS_SOC_FLAG_SOC_LOW && est->SoC_percentage > est->DischargeThreshold.U_ocv_SoC) {
				// clear BMS_SOC_LOW flage
				est->flags=0;
			}
			if(est->ChargeSmoothFlags==  BMS_SOC_DISCHARGE_SMOOTH_01 &&  est->SoC_percentage >0.01) {
				// 	 clear BMS_SOC_DISCHARGE_SMOOTH_01 flage 
				est->ChargeSmoothFlags=0;
			}
			// calculate OCV
			// ToDo: add temperature dependency of Ri 
			R_i=est->ChargeThreshold.Ri_charge;
			OCV=   U_batt_max - R_i * (-1)*(float)I_batt/1000; // I_batt is negative ;
			est->Ocv_calc = OCV;
			// Check in Which Area SoC is
			if(est->flags==BMS_SOC_FLAG_SOC_100) {
				// SoC = 100% has been reached keep SoC at 100% until discharging starts;
				// => do nothing
				est->state=BMS_SOC_CHARGE_SMOOTHING;
				return 0;
			}

			else if(U_batt_max >  est->ChargeThreshold.U_bat_max) {
				// if Cell voltage is higer than a threshold battery is considered fully charged
				// comparison with cell voltage is necessary because battery model might not be accurate enought
				est->state=BMS_SOC_CHARGE_AREA_FULL;
				return 0;
			}
			if(est->ChargeSmoothFlags==  BMS_SOC_CHARGE_SMOOTH_99) {
				// SoC => 99% wait till U_bat max is reached
				est->state=BMS_SOC_CHARGE_SMOOTHING;
				return 0;
			}
			
			else if(OCV > est->ChargeThreshold.U_ocv_correct) {
				// open clamp Voltage is higer than threshold
				// => Battery is nearly fully charged => SoC from columb counting has to be updated
				// Wait for 20 Seconds for update of SoC
				est->state=BMS_SOC_CHARGE_WAIT_FOR_RELAXATION;
				return 0;
			}
			else {
				// no special Area to take care of
				// proceed with columb counting
				est->state=BMS_SOC_CHARGE_C_CNT;
				// no more need to wait for relaxation
				est->ChargeRelaxationWaitCnt=0;
				return 0;
			}
			return -1;	
		break;
		case BMS_SOC_CHARGE_AREA_FULL:
			// battery is considered full
			// set SoC to 100%
			est->SoC_percentage=1.0;
			est->SoC_Ah=est->capacity;
			// set maximum charge current to 0
			est->MaxBatteryChargeCurrent=0;
			// set Flag to prevent SOC from falling in case of relaxation
			est->flags=BMS_SOC_FLAG_SOC_100;
			est->state=BMS_SOC_READY;
			return 0;
		break;
		case BMS_SOC_CHARGE_WAIT_FOR_RELAXATION:
			// wait a couple of seconds for Relaxation to prevent 
			// unjustified update of SoC
			est->ChargeRelaxationWaitCnt++;
			
			if(est->ChargeRelaxationWaitCnt > 100) {
				est->state=	BMS_SOC_CHARGE_AREA_HIGH;
			}
			else {
				est->state= BMS_SOC_CHARGE_C_CNT;
			}
			return 0;
		break;	
		case BMS_SOC_CHARGE_AREA_HIGH:
			// open clamp Voltage is higer than threshold
			// => Battery is nearly fully charged => SoC from columb counting has to be updated	
			// set SoC predefined Value
			if(est->flags!=BMS_SOC_FLAG_SOC_HIGH) {
				// save old SoC value for Smoothing
				est->SoC_Ah_smooth=est->SoC_Ah;
				est->SoC_percentage_smooth=est->SoC_percentage;
				
				// update SoC value
				est->SoC_percentage=est->ChargeThreshold.U_ocv_SoC;
				est->SoC_Ah=est->SoC_percentage * est->capacity;
				// set high flag to indicate that SoC has already been updated
				est->flags=BMS_SOC_FLAG_SOC_HIGH;
			}
			// continue columb cnt
			est->state=BMS_SOC_CHARGE_C_CNT;
			return 0;	
		break;
		case  BMS_SOC_CHARGE_C_CNT: 
			// calculate SoC using coulumb counting
			est->SoC_Ah=est->SoC_Ah +  est->delta_t* (-1)*(float)I_batt/1000 ;
			est->SoC_percentage=est->SoC_Ah/est->capacity;
			est->state=BMS_SOC_CHARGE_SMOOTHING;
			return 0;
		break;
		case BMS_SOC_CHARGE_SMOOTHING:
			// prevents jumps in SoC 
			if(est->flags == BMS_SOC_FLAG_SOC_100) {
				est->ChargeSmoothFlags=0;
				est->SoC_percentage_smooth=est->SoC_percentage ;
				est->state=BMS_SOC_READY;  
				return 0;							   ; 
			}
			else if(est->SoC_percentage > 0.99 && est->flags != BMS_SOC_FLAG_SOC_100) {
				// keep SoC at 99,5% until U_bat Max is reached
				if(est->ChargeSmoothFlags != BMS_SOC_CHARGE_SMOOTH_99){
					est->ChargeSmoothFlags=  BMS_SOC_CHARGE_SMOOTH_99;
					est->SoC_percentage_smooth=est->SoC_percentage;
					est->SoC_Ah_smooth=est->SoC_Ah;
				}
				else {
					// do nothing	
				}
				est->state=BMS_SOC_CHARGE_DERATE;  
				return 0;
			}
			else if(est->SoC_percentage >= est->ChargeThreshold.U_ocv_SoC && est->flags != BMS_SOC_FLAG_SOC_HIGH && est->ChargeSmoothFlags != BMS_SOC_CHARGE_SMOOTH_OVERSHOOT) {
				 // Overshoot event has occured
				 // this means that the SoC estimated by coulumb counting is higher than
				 // the SoC based on the OCV => implement slowed coulumb cnt
				 
				 // save SoC
				 est->SoC_Ah_smooth=est->SoC_Ah;
				 est->SoC_percentage_smooth=est->SoC_percentage;
				 // Set Flag to indicate that overshoot correction is enabled
				 est->ChargeSmoothFlags=BMS_SOC_CHARGE_SMOOTH_OVERSHOOT;
				 est->state=BMS_SOC_CHARGE_DERATE;  
				return 0;
			}
			else if( est->ChargeSmoothFlags == BMS_SOC_CHARGE_SMOOTH_OVERSHOOT) {
				// Smooth out overshoot by slowed down coulumb counting
				est->SoC_Ah_smooth=est->SoC_Ah_smooth + est->delta_t* (-1)*(float)I_batt/1000*0.5 ;
				est->SoC_percentage_smooth=est->SoC_Ah_smooth/est->capacity;
				
				if( bms_SoC_get_norm(est->SoC_percentage_smooth - est->SoC_percentage) <= 0.001) {
					// if Error is smaller than 0.5 % stopp correction
					est->ChargeSmoothFlags=0;
					est->SoC_percentage_smooth=est->SoC_percentage;
					est->SoC_Ah_smooth=est->SoC_Ah;  
					
				}
				est->state=BMS_SOC_CHARGE_DERATE;  
				return 0;
			}
			else if( est->flags == BMS_SOC_FLAG_SOC_HIGH && est->ChargeSmoothFlags !=BMS_SOC_CHARGE_SMOOTH_UNDERSHOOT) {
				// Undershoot event hast occured
				// this means that SoC estimated by coulumb counting is lower
				// than SoC based on the OCV => implement faster coulumb cnt
				
				 // Set Flag to indicate that undershoot correction is enabled
				 est->ChargeSmoothFlags=BMS_SOC_CHARGE_SMOOTH_UNDERSHOOT;
				 est->state=BMS_SOC_CHARGE_DERATE;  
				 return 0;
			}
			else if( est->flags == BMS_SOC_FLAG_SOC_HIGH && est->ChargeSmoothFlags ==BMS_SOC_CHARGE_SMOOTH_UNDERSHOOT) {
				// smooth out under shoot by speeding up coulumb counting
				est->SoC_Ah_smooth=est->SoC_Ah_smooth + est->delta_t* (-1)*(float)I_batt/1000*4 ;
				est->SoC_percentage_smooth=est->SoC_Ah_smooth/est->capacity;
				if( bms_SoC_get_norm(est->SoC_percentage_smooth - est->SoC_percentage) <= 0.001) {
					// if Error is smaller than 0.1 % stopp correction
					est->ChargeSmoothFlags=0;
					est->SoC_percentage_smooth=est->SoC_percentage;
					 est->SoC_Ah_smooth=est->SoC_Ah;  
				}
				est->state=BMS_SOC_CHARGE_DERATE;  
				return 0;
			}
			else {
				est->SoC_Ah_smooth=est->SoC_Ah;
				est->SoC_percentage_smooth=est->SoC_percentage; 
				est->state=BMS_SOC_CHARGE_DERATE;  
				return 0;
			}
			return 0;
		break;	
		case BMS_SOC_CHARGE_DERATE:
			// derate SoC depending on the SoC and temperature
			bms_set_derating(est, temp_min, temp_max);
			est->state=BMS_SOC_READY;
			return 0;	
		break;
		case BMS_SOC_DISCHARGE:
			// Battery is discharged
			// if Battery has been charged to 100% SoC remove charge Flag
			
			// no more need to wait for relaxation at charging
			est->ChargeRelaxationWaitCnt=0;
				
			if(est->flags==BMS_SOC_FLAG_SOC_100 && est->SoC_percentage < est->ChargeThreshold.U_ocv_SoC) {
				est->flags=0;		
			}
			
			if(est->flags==BMS_SOC_FLAG_SOC_HIGH && est->SoC_percentage < est->ChargeThreshold.U_ocv_SoC) {
				// clear BMS_SOC_HIGH flag
				est->flags=0;
			}
			
			if(est->ChargeSmoothFlags==  BMS_SOC_CHARGE_SMOOTH_99 &&  est->SoC_percentage <0.99) {
				// 	 clear BMS_SOC_DISCHARGE_SMOOTH_01 flage 
				est->ChargeSmoothFlags=0;
			}
			// calculate OCV
			// ToDo: add temperature dependency of Ri 
			R_i=est->DischargeThreshold.Ri_discharge;
			OCV=   U_batt_min + R_i *(float)I_batt/1000; // I_batt is positive ;
			est->Ocv_calc = OCV;
			if(est->flags==BMS_SOC_FLAG_SOC_0) {
				// SoC = 0% has been reached keep SoC at 0% until charging starts;
				// => do nothing
				est->state=BMS_SOC_DISCHARGE_SMOOTHING;
				return 0;
			}
			else if(U_batt_min <  est->DischargeThreshold.U_bat_min ) {
				// if Cell voltage is lower than a threshold battery is considered fully discharged
				// comparison with cell voltage is necessary because battery model might not be accurate enought
				est->state=BMS_SOC_DISCHARGE_AREA_EMPTY;
				return 0;
			}
			else if(est->ChargeSmoothFlags==  BMS_SOC_DISCHARGE_SMOOTH_01) {
				// SoC => 99% wait till U_bat max is reached
				est->state=BMS_SOC_CHARGE_SMOOTHING;
				return 0;
			}
			else if(OCV < est->DischargeThreshold.U_ocv_correct)  {
				// open clamp Voltage is lower than threshold
				// => Battery is nearly fully discharged => SoC from columb counting has to be updated
				est->state=BMS_SOC_DISCHARGE_WAIT_FOR_RELAXATION;
				return 0;
			}
			else {
				// no special Area to take care of
				// proceed with columb counting
				est->state=BMS_SOC_DISCHARGE_C_CNT;
				
				est->DischargeRelaxationWaitCnt=0;
				return 0;
			}
			return 0;
		break;
		case BMS_SOC_DISCHARGE_AREA_EMPTY:
			// Battery is considered empty => Set SoC to 0%
			est->SoC_percentage=0;
			est->SoC_Ah=0;
			// set Maximum discharge Current to 0
			est->MaxBatteryDischargeCurrent =0;
			// set empty flag
			est->flags=BMS_SOC_FLAG_SOC_0;
			est->state=BMS_SOC_DISCHARGE_SMOOTHING;
			return 0;
		break;
		case BMS_SOC_DISCHARGE_WAIT_FOR_RELAXATION: 
			// wait a couple of seconds for Relaxation to prevent 
			// unjustified update of SoC
			est->DischargeRelaxationWaitCnt++;
			
			if(est->DischargeRelaxationWaitCnt > 100) {
				est->state=	BMS_SOC_DISCHARGE_AREA_LOW;
			}
			else {
				est->state= BMS_SOC_DISCHARGE_C_CNT;
			}
			return 0;			
		break;
		case  BMS_SOC_DISCHARGE_AREA_LOW:
			// open clamp Voltage is lower than threshold
			// => Battery is nearly fully dicharged => SoC from columb counting has to be updated

			// set SoC predefined Value	
			if(est->flags!=BMS_SOC_FLAG_SOC_LOW) {
				// save SoC for Smoothing
				est->SoC_Ah_smooth=est->SoC_Ah;
				est->SoC_percentage_smooth=est->SoC_percentage;
				est->SoC_percentage=est->DischargeThreshold.U_ocv_SoC;
				est->SoC_Ah=est->SoC_percentage * est->capacity;
				// set LOW flag to indicate that SoC has already been updated
				est->flags=BMS_SOC_FLAG_SOC_LOW;
			}
			// continue columb cnt
			est->state=BMS_SOC_DISCHARGE_C_CNT;
			return 0;
		break;
		case BMS_SOC_DISCHARGE_C_CNT:
			// calculate SoC using coulumb counting  
			est->SoC_Ah=est->SoC_Ah - est->delta_t* (float)I_batt/1000;
			est->SoC_percentage=  est->SoC_Ah/est->capacity;
			est->state=BMS_SOC_DISCHARGE_SMOOTHING;
			return 0;
		break;
		case BMS_SOC_DISCHARGE_SMOOTHING:
			// prevents jumps in SoC 
			if(est->flags == BMS_SOC_FLAG_SOC_0) {
				est->ChargeSmoothFlags=0;
				est->SoC_percentage_smooth=est->SoC_percentage ;
				est->state=BMS_SOC_READY;  
				return 0;							   ; 
			}
			else if(est->SoC_percentage <= 0.01 && est->flags != BMS_SOC_FLAG_SOC_0) {
				// keep SoC at 0,5% until U_bat Max is reached
				if(est->ChargeSmoothFlags != BMS_SOC_DISCHARGE_SMOOTH_01){
					est->ChargeSmoothFlags=  BMS_SOC_DISCHARGE_SMOOTH_01;
					est->SoC_percentage_smooth=est->SoC_percentage;
					est->SoC_Ah_smooth=est->SoC_Ah;
				}
				else {
					// do nothing	
				}
				est->state=BMS_SOC_DISCHARGE_DERATE;  
				return 0;
			}
			else if(est->SoC_percentage <= est->DischargeThreshold.U_ocv_SoC && est->flags != BMS_SOC_FLAG_SOC_LOW && est->ChargeSmoothFlags != BMS_SOC_DISCHARGE_SMOOTH_OVERSHOOT) {
				 // Overshoot event has occured
				 // this means that the SoC estimated by coulumb counting is lower than
				 // the SoC based on the OCV => implement slowed coulumb cnt
				 
				 // save SoC
				 est->SoC_Ah_smooth=est->SoC_Ah;
				 est->SoC_percentage_smooth=est->SoC_percentage;
				 // Set Flag to indicate that overshoot correction is enabled
				 est->ChargeSmoothFlags=BMS_SOC_DISCHARGE_SMOOTH_OVERSHOOT;
				 est->state=BMS_SOC_DISCHARGE_DERATE;  
				return 0;
			}
			else if( est->ChargeSmoothFlags == BMS_SOC_DISCHARGE_SMOOTH_OVERSHOOT) {
				// Smooth out overshoot by slowed down coulumb counting
				est->SoC_Ah_smooth=est->SoC_Ah_smooth - est->delta_t*(float)I_batt/1000*0.5 ;
				est->SoC_percentage_smooth=est->SoC_Ah_smooth/est->capacity;
				
				if( bms_SoC_get_norm(est->SoC_percentage_smooth - est->SoC_percentage) <= 0.001) {
					// if Error is smaller than 0.1 % stopp correction
					est->ChargeSmoothFlags=0;
					est->SoC_percentage_smooth=est->SoC_percentage;
					est->SoC_Ah_smooth=est->SoC_Ah;
					
				}
				est->state=BMS_SOC_DISCHARGE_DERATE;  
				return 0;
			}
			else if( est->flags == BMS_SOC_FLAG_SOC_LOW && est->ChargeSmoothFlags !=BMS_SOC_DISCHARGE_SMOOTH_UNDERSHOOT) {
				// Undershoot event has occured
				// this means that SoC estimated by coulumb counting is higher
				// than SoC based on the OCV => implement faster coulumb cnt
				
				 // Set Flag to indicate that undershoot correction is enabled
				 est->ChargeSmoothFlags=BMS_SOC_DISCHARGE_SMOOTH_UNDERSHOOT;
				 est->state=BMS_SOC_CHARGE_DERATE;  
				 return 0;
			}
			else if( est->flags == BMS_SOC_FLAG_SOC_LOW && est->ChargeSmoothFlags ==BMS_SOC_DISCHARGE_SMOOTH_UNDERSHOOT) {
				// smooth out under shoot by speeding up coulumb counting
				est->SoC_Ah_smooth=est->SoC_Ah_smooth - est->delta_t*(float)I_batt/1000*4 ;
				est->SoC_percentage_smooth=est->SoC_Ah_smooth/est->capacity;
				if( bms_SoC_get_norm(est->SoC_percentage_smooth - est->SoC_percentage) <= 0.001) {
					// if Error is smaller than 0.1 % stopp correction
					est->ChargeSmoothFlags=0;
					est->SoC_percentage_smooth=est->SoC_percentage;
					est->SoC_Ah_smooth=est->SoC_Ah;
				}
				est->state=BMS_SOC_DISCHARGE_DERATE;  
				return 0;
			}
			else {
				est->SoC_percentage_smooth=est->SoC_percentage;
				est->SoC_Ah_smooth=est->SoC_Ah;  
				est->state=BMS_SOC_DISCHARGE_DERATE;  
				return 0;
			}
			return 0;
		break;			
		case BMS_SOC_DISCHARGE_DERATE:
			// derate SoC depending on the SoC and temperature
			bms_set_derating(est, temp_min, temp_max);
			est->state=BMS_SOC_READY; 
			return 0;
		break;
		case BMS_SOC_REST:
			// no charging or discharging => do nothing
			est->state=BMS_SOC_READY;
			return 0;
		break;
		case BMS_SOC_READY:
			// SoC Estimation complete  do nothing
			
			// push FIFO
			pushSoCFiFo(est); 
			return 0;
		break;
		
		
	}
	
}

/*
 * set all SOC FIFO entries to start value;
 * SoC FIFO necessary to give inverter time to react to derating and Master to transmit Derating info to inverter
 */
uint32_t initSoCFifo(MASTER_CAN0_STRUCT_t* s) {
	uint8_t i;
	for(i=0;i<SOC_SMOOTH_FIFO_SIZE;i++) {
		s->SoC_estimator.SoC_percentage_smooth_FiFo[i]=s->SoC_estimator.SoC_percentage_smooth ;
	}
	return TRUE;
}
uint32_t pushSoCFiFo(MASTER_SOC_ESTIMATOR_t* est) {
	uint8_t i;
	for(i=SOC_SMOOTH_FIFO_SIZE-1;i>0;i--) {
		est->SoC_percentage_smooth_FiFo[i]=est->SoC_percentage_smooth_FiFo[i-1];
	}
	est->SoC_percentage_smooth_FiFo[0]=est->SoC_percentage_smooth;
	return TRUE;
}


	



float bms_SoC_get_norm(float value) {
	if(value >=0) {
		return value;
	}
	else {
		return value * (-1);
	}
}

/*
 * @brief set Derating
 */
uint32_t bms_set_derating(MASTER_SOC_ESTIMATOR_t* est, int8_t minTemp, int8_t maxTemp) {
	float derating_charge_temp_high;
	float derating_charge_temp_low;
	float derating_charge_SoC;
	float output_derate;
	
	float derating_discharge_temp_high;
	float derating_discharge_temp_low;
	float derating_discharge_SoC;

	// charging
	derating_charge_SoC=bms_calc_charge_derating(est->SoC_percentage_smooth);
	derating_charge_temp_high=bms_calc_charge_temp_derating(maxTemp);
	derating_charge_temp_low=bms_calc_charge_temp_derating(minTemp);
	
	// lowest current is choosen to be set as max charge current
	
	if(derating_charge_temp_low < derating_charge_temp_high) {
		output_derate = derating_charge_temp_low;
	}
	else{
		output_derate =derating_charge_temp_high;
	}
	
	if(output_derate > derating_charge_SoC ) {
		output_derate = derating_charge_SoC;
	}
	est->MaxBatteryChargeCurrent=output_derate;
	
	// discharging
	derating_discharge_SoC= bms_calc_discharge_derating(est->SoC_percentage_smooth);
	derating_discharge_temp_high = bms_calc_discharge_temp_derating(maxTemp);
	derating_discharge_temp_low = bms_calc_discharge_temp_derating(minTemp);
	
	// lowest current is choosen to be set as max charge current
	
	if(derating_discharge_temp_low < derating_discharge_temp_high) {
		output_derate = derating_discharge_temp_low;
	}
	else{
		output_derate =derating_discharge_temp_high;
	}
	
	if(output_derate > derating_discharge_SoC ) {
		output_derate = derating_discharge_SoC;
	}
	
	est->MaxBatteryDischargeCurrent=output_derate;
	
	return 0;
}

float bms_calc_charge_derating(float SoC) {
	float delta_I=0;
	float I_C_10=2.5;	// Current at C tens
	float delta_I_max= BMS_SLAVE_MAX_CURRENT_A - I_C_10 ;
	float delta_SoC = 0.15; // Linear interpolation region from 80 to 95 %
	if( SoC <= 0.8) {
		//return maximum allowed current
		return 	BMS_SLAVE_MAX_CURRENT_A ;
	}
	else if( SoC > 0.8 && SoC < 0.95) {
		//return linear interpolation between max aurrent and C/10
		delta_I=  (delta_I_max * (0.95 - SoC)) / (delta_SoC);
		return I_C_10 + delta_I;
	}
	else if( SoC >= 0.95 && SoC < 0.999){
		// finish loading with C/10
		return   I_C_10 ;
	}
	else{
		return 0;	
	}
	return -1;
	
}

float bms_calc_discharge_derating(float SoC) {
	float delta_I=0;
	float I_C_10=2.5;	// Current at C tens
	float delta_I_max= BMS_SLAVE_MAX_CURRENT_A - I_C_10 ;
	float delta_SoC = 0.15; // Linear interpolation region from 20 to 5 %
	if( SoC >= 0.2) {
		//return maximum allowed current
		return 	BMS_SLAVE_MAX_CURRENT_A ;
	}
	else if( SoC < 0.2 && SoC > 0.05) {
		//return linear interpolation between max aurrent and C/10
		delta_I=  (delta_I_max * ( SoC - 0.05)) / (delta_SoC);
		return I_C_10 + delta_I;
	}
	else if(SoC <= 0.05 && SoC > 0.001) {
		// finish discharging with C/10
		return   I_C_10 ;
	}
	else {
		// 
		return 0;
	}
	return -1;
	
}

/*
 * @ brief derate maximum charge current
 */
float bms_calc_charge_temp_derating(int8_t Temp) {
	float maxCurrent;
	float I_C1=25.0; // Current at C1 = 25A
	// calc derated current for min temperature
	if( Temp <= -5) {
		maxCurrent= I_C1*0.2;
		return maxCurrent;
	}
	else if(Temp > -5 && Temp <=0) {
		maxCurrent=linear_interpolate(-5,0,Temp,I_C1*0.2,I_C1*0.5);
		return maxCurrent;
	}
	else if(Temp >0 && Temp <=25) {
		maxCurrent=linear_interpolate(0,25,Temp,I_C1*0.5,I_C1);
		return maxCurrent;
	}
	else if(Temp> 25 && Temp <=35) {
		maxCurrent=I_C1;
		return maxCurrent;
	}
	else if(Temp> 35 && Temp <= 45) {
		maxCurrent=linear_interpolate(35,45,Temp,I_C1,0);
		return maxCurrent;
	}
	else{
		return 0;
	}
}

/*
 * @ brief derate maximum discharge current 
 */
float bms_calc_discharge_temp_derating(int8_t Temp) {
	float maxCurrent;
	float I_C1=25.0; // Current at C1 = 25A
	// calc derated current for min temperature
	if( Temp <= 0) {
		maxCurrent= I_C1*0.2;
		return maxCurrent;
	}
	else if(Temp > 0 && Temp <=25) {
		maxCurrent=linear_interpolate(0,25,Temp,I_C1*0.2,I_C1);
		return maxCurrent;
	}
	else if(Temp >25 && Temp <=40) {
		maxCurrent=I_C1;
		return maxCurrent;
	}
	else if(Temp> 40 && Temp <=45) {
		maxCurrent=I_C1=linear_interpolate(40,45,Temp,I_C1,0);
		return maxCurrent;
	}
	else{
		return 0;
	}
	
}

/*
 *@brief do linear interpolation
 */
float linear_interpolate(int8_t x1,int8_t x2,int8_t x,float y1,float y2) {
	// solve linear eq  y1= m*x1 + b
	//					y2= m*x2 + b
	float m = 	(y2-y1)/(float)(x2-x1);
	float b =	y1 - m*(float)(x1);
	
	//
	return m*(float)(x) + b;
 }