Master_BMS/BMS Master/Sources/BMS_Set_Error_States.c

/*
 * BMS_Set_Error_States.c

 *
 *  Created on: Nov 8, 2016
 *      Author: le8041
 */

#include "BMS_Master.h"

/*
 * @brief check if Battery is charged or discharged
 * return true if discharged false if charged
 */
uint32_t BMS_set_Error_check_if_stable_discharge(MASTER_CAN0_STRUCT_t* s) {
	// stable when mean value is bigger than -150mA
	uint8_t i;
	int16_t Current=0;
	for(i=0;i<UI_CURRENT_FIFO_SIZE;i++) {
		Current=Current + s->UI_Board.IbattFiFo[i] ;
	}
	
	Current = (Current*10) /UI_CURRENT_FIFO_SIZE ;
	
	if(Current > -150) {
		// stable discharge
		return TRUE;
	}
	
	//stable discharge
	return FALSE;
	
}
/*
 * @brief ceck if battery is charged stable over given amount of samples
 */
uint32_t BMS_set_Error_check_if_stable_charge(MASTER_CAN0_STRUCT_t* s) {
	// stable when all values of FiFo are greater than -150
	uint8_t i;
	for(i=0;i<UI_CURRENT_FIFO_SIZE;i++) {
		if(s->UI_Board.IbattFiFo[i] *10 > -150) {
			// no stable charge
			return FALSE;
		}
	}
	
	//stable charge
	return TRUE;
	
}
/*
 * @brief check voltage levels Umin1 Umin2 Umin3 and set Error states accordingly
 * return True if everything is of return false if error occured
 */
uint32_t BMS_Set_Error_check_Voltage_level_min(MASTER_CAN0_STRUCT_t* s){
	uint16_t Umin;
	float PvPwrAvailable;
	Umin=s->minCellVoltage;
	PvPwrAvailable=s->inverter.rxStruct.expectedInputPower;
	
	if(Umin < BMS_SOC_UMIN_3  ) {
		// Voltage level below Umin3 
		// Kill Relays
		s->relayState.HS_closed=FALSE;
		s->relayState.LS_closed=FALSE;
		s->relayState.PRECHARGE_closed=FALSE;
		SwitchRelais( LS_RELAIS, 0);
		SwitchRelais( PRE_CHARGE_RELAIS, 0);
		SwitchRelais( HS_RELAIS, 0);
		// set Error state 3 - should be alse set directly after test of Slave Voltage in Salve
		ErrorStackPushMasterError(s,BMS_ERROR_STACK_MASTER_UMIN_3,BMS_ERROR_CLASS_3);
		return FALSE;
	}
	else if(Umin < BMS_SOC_UMIN_2 && BMS_set_Error_check_if_stable_discharge(s) && !ErrorStackCheckifUmin2ErrorRecoveryPending(s)  ) {
		// Voltage level below Umin2 and Battery is discharged 
		// Kill Relays
		SwitchRelais( LS_RELAIS, 0);
		SwitchRelais( PRE_CHARGE_RELAIS, 0);
		SwitchRelais( HS_RELAIS, 0);
		s->relayState.HS_closed=FALSE;
		s->relayState.LS_closed=FALSE;
		s->relayState.PRECHARGE_closed=FALSE;
		
		// set Error state 2
		ErrorStackPushMasterError(s,BMS_ERROR_STACK_MASTER_UMIN_2,BMS_ERROR_CLASS_2);
		return FALSE;
	}
	else if(Umin < BMS_SOC_UMIN_1 &&  BMS_set_Error_check_if_stable_discharge(s) && !ErrorStackCheckifUmin1ErrorRecoveryPending(s)   ) {
		// Voltage Level below Umin1 and battery is discharged
		// Kill Relays
		SwitchRelais( LS_RELAIS, 0);
		SwitchRelais( PRE_CHARGE_RELAIS, 0);
		SwitchRelais( HS_RELAIS, 0);
		s->relayState.HS_closed=FALSE;
		s->relayState.LS_closed=FALSE;
		s->relayState.PRECHARGE_closed=FALSE;
		// set Error state 1
		
		
		ErrorStackPushMasterError(s,BMS_ERROR_STACK_MASTER_UMIN_1,BMS_ERROR_CLASS_1);
		return FALSE;		
	}
	else {
		// everything is ok
		return TRUE;
	}	
}

/*
 * @brief check  max voltage level
 */

uint32_t BMS_Set_Error_check_Voltage_level_max(MASTER_CAN0_STRUCT_t* s) {
	uint16_t Umax=s->maxCellVoltage;
	
	if(Umax > BMS_SOC_UMAX_3) {
		// set Error State 1
		// Kill Relays
		SwitchRelais( LS_RELAIS, 0);
		SwitchRelais( PRE_CHARGE_RELAIS, 0);
		SwitchRelais( HS_RELAIS, 0);
		s->relayState.HS_closed=FALSE;
		s->relayState.LS_closed=FALSE;
		s->relayState.PRECHARGE_closed=FALSE;
		// set Error state 2
		ErrorStackPushMasterError(s,BMS_ERROR_STACK_MASTER_UMAX_3,BMS_ERROR_CLASS_3);
		return FALSE;
	}


	
	else if(Umax > BMS_SOC_UMAX_2) {
		// set Error State 1
		// Kill Relays
		SwitchRelais( LS_RELAIS, 0);
		SwitchRelais( PRE_CHARGE_RELAIS, 0);
		SwitchRelais( HS_RELAIS, 0);
		s->relayState.HS_closed=FALSE;
		s->relayState.LS_closed=FALSE;
		s->relayState.PRECHARGE_closed=FALSE;
		// set Error state 2
		ErrorStackPushMasterError(s,BMS_ERROR_STACK_MASTER_UMAX_2,BMS_ERROR_CLASS_1);
		return FALSE;
	}
	else if(Umax > BMS_SOC_UMAX_1 ) {
		// set Error State 0 not implemented yet
		// TODO return TRUE; everything is ok
		return TRUE;
	}
	
	else {
		// everything is ok
		return TRUE;
	}
	
	
}



/*
 * @brief check if system exceeds maximum System Voltage in case customer plays around with the system
 */

uint32_t BMS_Set_Error_check_System_voltage_level_max(MASTER_CAN0_STRUCT_t* s) {
	uint32_t Uges=s->systemVoltage;
	if(Uges >BMS_ERROR_THRESHOLD_UMAX_SYSTEM) {
		// Kill Relays
		SwitchRelais( LS_RELAIS, 0);
		SwitchRelais( PRE_CHARGE_RELAIS, 0);
		SwitchRelais( HS_RELAIS, 0);
		s->relayState.HS_closed=FALSE;
		s->relayState.LS_closed=FALSE;
		s->relayState.PRECHARGE_closed=FALSE;
		// set Error state 3
		ErrorStackPushMasterError(s,BMS_ERROR_STACK_MASTER_UMAX_SYSTEM,BMS_ERROR_CLASS_3);
		return FALSE;
	}
	else {
		//everything is ok
		return TRUE;
	}

}

/*
 *@brief do linear interpolation
 */
float BMS_Set_Error_linear_interpolate(float x1,float x2,float 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;
 }

/*
 * 
 */
float BMS_Set_Error_generate_Fcn(float* x_values,float* y_values,uint8_t NrOfPoints, float x) {
	uint8_t i;
	if(x < x_values[0]) {
		// value is to the left of the described area => return left most value
		return y_values[0];
	}
	else if( x> x_values[NrOfPoints -1] ) {
		// value is to the right of the described area => return right most value
		return y_values[NrOfPoints -1];
	}
	
	// find left x_value to x
	for(i=0;i< NrOfPoints; i++) {
		if(x_values[i] <= x && x_values[i+1] >= x ) {
			// x is between x_value[i] and x_value[i+1]
			return BMS_Set_Error_linear_interpolate(x_values[i],x_values[i+1],x,y_values[i],y_values[i+1]);
		}
	}
}

uint32_t BMS_Set_Error_Check_derating(MASTER_CAN0_STRUCT_t* s) {
	float Current = (float)(s->UI_Board.Ibatt*10);

	float minTemp = (float)s->minCellTemp;
	float maxTemp = (float)s->maxCellTemp;
	float SoC = s->SoC_estimator.SoC_percentage_smooth_FiFo[5];
	float capacity = 26.0 ; // capacity 26.0 Ah
	float temp_discharge_x[4]={0,25,40,55};
	float temp_discharge_y[4]={0.3,1.1,1.1,0};
	float temp_charge_x[5]={-5,0,25,40,50};
	float temp_charge_y[5]={0.3,0.6,1.1,1.1,0};
	
	float SoC_charge_x[5]={0,0.8,0.95,0.999,1};
	float SoC_charge_y[5]={25,25,2.5,2.5,0};
	
	float SoC_discharge_x[5]={0,0.001,0.05,0.2,1};
	float SoC_discharge_y[5]={0,2.5,2.5,25,25};
	
	s->currentLimits.I_max_charge_temp_max = BMS_Set_Error_generate_Fcn(temp_charge_x,temp_charge_y,5,maxTemp)*25*1000;
	s->currentLimits.I_max_charge_temp_min = BMS_Set_Error_generate_Fcn(temp_charge_x,temp_charge_y,5,minTemp)*25*1000;
	s->currentLimits.I_max_charge_SoC      = BMS_Set_Error_generate_Fcn(SoC_charge_x,SoC_charge_y,5,SoC)*1000 *1.1 ;
	
	s->currentLimits.I_max_discharge_temp_max 	= BMS_Set_Error_generate_Fcn(temp_discharge_x,temp_discharge_y,4,maxTemp)*25*1000;
	s->currentLimits.I_max_discharge_temp_min 	= BMS_Set_Error_generate_Fcn(temp_discharge_x,temp_discharge_y,4,minTemp)*25*1000;
	s->currentLimits.I_max_discharge_SoC		= BMS_Set_Error_generate_Fcn(SoC_discharge_x,SoC_discharge_y,5,SoC)*1000 *1.1 ;	

	if(Current < -100) {
		// Battery is charged
		
		// check temperature derating at tmin
		// check if low temperature is violated 
		if((-1)*Current > s->currentLimits.I_max_charge_temp_min) {
			// current to high
			ErrorStackPushMasterError(s,BMS_ERROR_STACK_MASTER_I_MAX_CHARGE_OP,BMS_ERROR_CLASS_1);
			// Kill Relays
			SwitchRelais( LS_RELAIS, 0);
			SwitchRelais( PRE_CHARGE_RELAIS, 0);
			SwitchRelais( HS_RELAIS, 0);
			s->relayState.HS_closed=FALSE;
			s->relayState.LS_closed=FALSE;
			s->relayState.PRECHARGE_closed=FALSE;
			return FALSE;
		}
		// check temperature derating at tmax
		// check if low temperature is violated 
		else if((-1)*Current > s->currentLimits.I_max_charge_temp_max ) {
			// current too high
			ErrorStackPushMasterError(s,BMS_ERROR_STACK_MASTER_I_MAX_CHARGE_OP,BMS_ERROR_CLASS_1);
			// Kill Relays
			SwitchRelais( LS_RELAIS, 0);
			SwitchRelais( PRE_CHARGE_RELAIS, 0);
			SwitchRelais( HS_RELAIS, 0);
			s->relayState.HS_closed=FALSE;
			s->relayState.LS_closed=FALSE;
			s->relayState.PRECHARGE_closed=FALSE;
			return FALSE;
		}
		// check SoC derating // normal derating curve + 10%
		else if((-1)*Current > s->currentLimits.I_max_charge_SoC) {
			// current too high
			ErrorStackPushMasterError(s,BMS_ERROR_STACK_MASTER_I_MAX_CHARGE_OP,BMS_ERROR_CLASS_1);
			// Kill Relays
			SwitchRelais( LS_RELAIS, 0);
			SwitchRelais( PRE_CHARGE_RELAIS, 0);
			SwitchRelais( HS_RELAIS, 0);
			s->relayState.HS_closed=FALSE;
			s->relayState.LS_closed=FALSE;
			s->relayState.PRECHARGE_closed=FALSE;
			return FALSE;
		}
		else {
			// derating is ok
			return TRUE;
		}				
	}
	else if(Current > 100) {
		// Battery is discharged
		
		// check temperature derating at tmin
		// check if low temperature is violated 
		if(Current > s->currentLimits.I_max_discharge_temp_min ) {
			// current to high
			ErrorStackPushMasterError(s,BMS_ERROR_STACK_MASTER_I_MAX_DISCHARGE_OP,BMS_ERROR_CLASS_1);
			// Kill Relays
			SwitchRelais( LS_RELAIS, 0);
			SwitchRelais( PRE_CHARGE_RELAIS, 0);
			SwitchRelais( HS_RELAIS, 0);
			s->relayState.HS_closed=FALSE;
			s->relayState.LS_closed=FALSE;
			s->relayState.PRECHARGE_closed=FALSE;
			return FALSE;
		}
		// check temperature derating at tmax
		// check if low temperature is violated 
		else if(Current > s->currentLimits.I_max_discharge_temp_max ) {
			// current too high
			ErrorStackPushMasterError(s,BMS_ERROR_STACK_MASTER_I_MAX_DISCHARGE_OP,BMS_ERROR_CLASS_1);
			// Kill Relays
			SwitchRelais( LS_RELAIS, 0);
			SwitchRelais( PRE_CHARGE_RELAIS, 0);
			SwitchRelais( HS_RELAIS, 0);
			s->relayState.HS_closed=FALSE;
			s->relayState.LS_closed=FALSE;
			s->relayState.PRECHARGE_closed=FALSE;
			return FALSE;
		}
		// check SoC derating // normal derating curve + 10%
		else if(Current > s->currentLimits.I_max_discharge_SoC) {
			// current too high
			ErrorStackPushMasterError(s,BMS_ERROR_STACK_MASTER_I_MAX_DISCHARGE_OP,BMS_ERROR_CLASS_1);
			// Kill Relays
			SwitchRelais( LS_RELAIS, 0);
			SwitchRelais( PRE_CHARGE_RELAIS, 0);
			SwitchRelais( HS_RELAIS, 0);
			s->relayState.HS_closed=FALSE;
			s->relayState.LS_closed=FALSE;
			s->relayState.PRECHARGE_closed=FALSE;
			return FALSE;
		}
		else {
			// derating is ok
			return TRUE;
		}				
	}
	
	else {
		// nothing happens
		return TRUE;
	}	
	return TRUE;
}

uint32_t BMS_Set_Error_CAN1_Timeout(MASTER_CAN0_STRUCT_t* s) {
	// CAN 1 Timeout Has occured
	ErrorStackPushMasterError(s,BMS_ERROR_STACK_MASTER_EXTERN_CAN_ERROR,BMS_ERROR_CLASS_1);
	// Kill Relays
	SwitchRelais( LS_RELAIS, 0);
	SwitchRelais( PRE_CHARGE_RELAIS, 0);
	SwitchRelais( HS_RELAIS, 0);
	s->relayState.HS_closed=FALSE;
	s->relayState.LS_closed=FALSE;
	s->relayState.PRECHARGE_closed=FALSE;
	
	return TRUE;
	
}

/*
 * @brief check if current mesured by UI is consitent wit current measured by Inverter
 */
uint32_t BMS_Set_Error_Check_current_consitency(MASTER_CAN0_STRUCT_t* s) {
	uint8_t i=0;
	float I_av_ui =0 ;
	float I_av_inverter =0;
	
	for(i=0; i<UI_CURRENT_FIFO_SIZE;i++) {
		I_av_ui = I_av_ui + (float)(s->UI_Board.IbattFiFo[i]);
	}
	
	I_av_ui = I_av_ui *10; // convert to mA
	I_av_ui = I_av_ui /UI_CURRENT_FIFO_SIZE ; // calc mean
	
	for(i=0;i<UI_CURRENT_FIFO_SIZE;i++ ) {
		I_av_inverter = I_av_inverter + s->UI_Board.Ibatt_Inverter_FIFO[i];
	}
	I_av_inverter = I_av_inverter *1000 /UI_CURRENT_FIFO_SIZE ; // convert to mA and calc mean
	
	// compare
	s->UiInverterInconsistency = bms_SoC_get_norm(I_av_inverter) - bms_SoC_get_norm(I_av_ui) ;
	
	if( s->UiInverterInconsistency  > BMS_MAX_CURRENT_INCONSITENCY_MA || s->UiInverterInconsistency <( (-1) * BMS_MAX_CURRENT_INCONSITENCY_MA) ) {
		// error has occured
		ErrorStackPushUiError(s,BMS_ERROR_STACK_UI_CURRENT_INCONSIST,BMS_ERROR_CLASS_1);
		// Kill Relays
		SwitchRelais( LS_RELAIS, 0);
		SwitchRelais( PRE_CHARGE_RELAIS, 0);
		SwitchRelais( HS_RELAIS, 0);
		s->relayState.HS_closed=FALSE;
		s->relayState.LS_closed=FALSE;
		s->relayState.PRECHARGE_closed=FALSE;
		return TRUE;
	
	}
	return TRUE;
}

/*
 * @brief check if Voltage measured by UI and by Summation of Slaves are equal
 */

uint32_t BMS_Set_Error_Check_voltage_inconsitency(MASTER_CAN0_STRUCT_t* s) {
	uint8_t i=0;
	uint32_t addedCellVoltage_mean=0;
	uint32_t UI_Board_voltage_mean=0;
	
	for(i=0;i<UI_VOLTAGE_FIFO_SIZE;i++) {
		addedCellVoltage_mean = addedCellVoltage_mean + s->UI_Board.SystemVoltageFiFo[i];
		UI_Board_voltage_mean = UI_Board_voltage_mean + s->UI_Board.UbattFiFo[i];				
	}
	addedCellVoltage_mean = addedCellVoltage_mean /UI_VOLTAGE_FIFO_SIZE ; // voltage in mV
	UI_Board_voltage_mean = UI_Board_voltage_mean *100 / UI_VOLTAGE_FIFO_SIZE ;
	
	s->UiSlaveInconsistency = bms_SoC_get_norm((float)addedCellVoltage_mean - (float)UI_Board_voltage_mean) ;
	
	// if voltage error is bigger than threshold and All relais are closed kill relays
	if((s->UiSlaveInconsistency > (float)addedCellVoltage_mean * 0.05 && s->RunMode.onCounter > UI_VOLTAGE_FIFO_SIZE* CAN0_MAX_NR_OF_SLAVES) &&
			(s->relayState.HS_closed==TRUE &&		s->relayState.LS_closed==TRUE && s->relayState.PRECHARGE_closed==TRUE) ){
		ErrorStackPushUiError(s,BMS_ERROR_STACK_UI_VOLTAGE_INCONSIST,BMS_ERROR_CLASS_1);
		// Kill Relays
		SwitchRelais( LS_RELAIS, 0);
		SwitchRelais( PRE_CHARGE_RELAIS, 0);
		SwitchRelais( HS_RELAIS, 0);
		s->relayState.HS_closed=FALSE;
		s->relayState.LS_closed=FALSE;
		s->relayState.PRECHARGE_closed=FALSE;
		s->RunMode.onCounter=0;
		return TRUE;
		
	}
	
	return TRUE;
}