Master_BMS/BMS Master/Sources/BMS_Connect_Inverter.c

/*
 * BMS_Connect_Inverter.c
 *
 *  Created on: Mar 14, 2017
 *      Author: le8041
 */

#include "BMS_Master.h"
#include "BMS_Connect_Inverter.h"

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

uint32_t check_if_voltage_equal(MASTER_CAN0_STRUCT_t* s) {
	float U_ui=(float)s->UI_Board.Ubatt *100 ;
	float U_cells=(float)s->systemVoltage ;
	
	if(get_Norm(U_ui - U_cells) < U_cells *BMS_CONNECT_VOLTAGE_EQUAL_MARGIN) {
		return TRUE;
	}
	else {
		return FALSE;
	}
}

uint32_t check_if_Ui_voltage_is_stable(MASTER_CAN0_STRUCT_t* s) {
	// find highest and lowest UI-Voltage
	uint8_t i=0;
	uint32_t Umin=s->UI_Board.UbattFiFo[0];
	uint32_t Umax=s->UI_Board.UbattFiFo[0];
	
	for(i=0;i<UI_VOLTAGE_STABILITY_CHECK;i++) {
		if(Umin > s->UI_Board.UbattFiFo[i] ) {
			Umin = s->UI_Board.UbattFiFo[i];
		}
		if(Umax < s->UI_Board.UbattFiFo[i]) {
			Umax = s->UI_Board.UbattFiFo[i];
		}
	}
	if ((Umax - Umin) *100 > UI_VOLTAGE_STABILITY_MAX_DEVIATION*1000) {
		// max deviation reached
		return FALSE;
	}
	else {
		return TRUE;
	}
}

/*
 * @brief check if expected voltage is at clamps
 * if return = FALSE shortcircuit at clamps
 */

uint32_t check_if_expected_clamp_voltage_occures(MASTER_CAN0_STRUCT_t* s) {
	float U_ui=(float)s->UI_Board.Ubatt *100 ;
	if(get_Norm(U_ui - CONNECT_INVERTER_SHORT_CIRCUIT_TEST_VOLTAGE) < CONNECT_INVERTER_SHORT_CIRCUIT_TEST_VOLTAGE *BMS_CONNECT_VOLTAGE_TEST_MARGIN_MV) {
		return TRUE;
	}
	else {
		return FALSE;
	}
}


uint32_t OPModeFSM(MASTER_CAN0_STRUCT_t* s){
	float SoC=s->SoC_estimator.SoC_percentage_smooth;
	// check if Inverster still allows startup
	if(s->inverter.rxStruct.BatteryControlStatus.batteryDisconnected == 1 && \
			s->inverter.rxStruct.BatteryControlStatus.relayTestPermitted==0 && \
			s->inverter.rxStruct.BatteryControlStatus.readyToStart ==0) {
		if(s->RunMode.OperationMode == OP_MODE_INIT ||\
				s->RunMode.OperationMode ==	OP_MODE_CHECK_STARTUP_CONDITIONS || \
				s->RunMode.OperationMode == BAT_CONNECT_CHECK_SHORTCIRCUIT_REQUEST_RELAY_TEST) {	
		}
		else {
			// FSM  not in first 3 states => go back to initial state
			s->RunMode.OperationMode = OP_MODE_INIT;
			// open all relays
			SwitchRelais( HS_RELAIS, 0);
			s->relayState.HS_closed=FALSE;
			SwitchRelais( LS_RELAIS, 0);
			s->relayState.LS_closed=FALSE;
			SwitchRelais( PRE_CHARGE_RELAIS, 0);
			s->relayState.PRECHARGE_closed=FALSE;	
		}
	}
		
	
	switch(s->RunMode.OperationMode) {
		case OP_MODE_INIT:
			// just jump to checkup startup conditions
			s->RunMode.OperationMode=OP_MODE_CHECK_STARTUP_CONDITIONS;
			s->RunMode.BatteryExtraStatus.value=1;
			CLEAR_OUTPIN(PIN_REGNR_LED4);
			return TRUE;
		break;
		case OP_MODE_CHECK_STARTUP_CONDITIONS:
			if(s->allValuesInitialized==TRUE) {
				// all values initalized and no error occured
				// => Startup conditions apply => start startup
				
				// check if all relays are open
				if(s->relayState.HS_closed==FALSE && s->relayState.LS_closed == FALSE && s->relayState.PRECHARGE_closed ==FALSE) {
					// everything ok oll relay open
				}
				else{
					// relays somehow closed open relays
					SwitchRelais( HS_RELAIS, 0);
					s->relayState.HS_closed=FALSE;
					SwitchRelais( LS_RELAIS, 0);
					s->relayState.LS_closed=FALSE;
					SwitchRelais( PRE_CHARGE_RELAIS, 0);
					s->relayState.PRECHARGE_closed=FALSE;
				}
				
				// set battery status
				s->RunMode.BatteryExtraStatus.batteryDisconnected = 1;
				s->RunMode.BatteryExtraStatus.readyToStart=0;
				s->RunMode.BatteryExtraStatus.requestRelayTest=0;
				
				s->RunMode.OperationModeTimestamp=Global_1msCounter;
				s->RunMode.OperationMode=BAT_CONNECT_CHECK_SHORTCIRCUIT_REQUEST_RELAY_TEST;
				s->RunMode.onCounter=0;
				return TRUE;
			}
			else {
				// wait till all values are initialiezd
				s->RunMode.OperationMode=OP_MODE_CHECK_STARTUP_CONDITIONS;
				return TRUE;
			}
		break;
		case BAT_CONNECT_CHECK_SHORTCIRCUIT_REQUEST_RELAY_TEST:
			// set battery status - request relay test by inverter
			s->RunMode.BatteryExtraStatus.batteryDisconnected = 1;
			s->RunMode.BatteryExtraStatus.readyToStart=0;
			s->RunMode.BatteryExtraStatus.requestRelayTest=1;
			if(s->inverter.rxStruct.BatteryControlStatus.relayTestPermitted == TRUE) {
				// relay tests are enabled for inverter side
				// inverter does its own relais tests
				// inverter sets Voltage at clamps to 100V
				// inverter disconnects from grid - Power from PV
				s->RunMode.OperationMode=BAT_CONNECT_CHECK_SHORTCIRCUIT_NET_DISCONNECTED;
				return TRUE;
			}
			else {
				// wait for relay test permmited flag from inverter
				s->RunMode.OperationMode=BAT_CONNECT_CHECK_SHORTCIRCUIT_REQUEST_RELAY_TEST;
				return TRUE;
			}
			
			return TRUE;
		break;
		case BAT_CONNECT_CHECK_SHORTCIRCUIT_NET_DISCONNECTED:
			// grid is now disconnected
			s->RunMode.OperationMode=BAT_CONNECT_CHECK_SHORTCIRCUIT_CHECK_CLAMPS;
			return TRUE;
		break;	
		case BAT_CONNECT_CHECK_SHORTCIRCUIT_CHECK_CLAMPS:
			if(check_if_expected_clamp_voltage_occures(s) == FALSE) {
				// voltage is not as expected, therefore shorcircuit exitsts
				// DONT TURN ON BATTERY
				s->RunMode.OperationMode=BAT_CONNECT_ERROR_SHORTCIRCUIT_DETECTED;
				return TRUE;
			}
			else {
				// everything ok,proceed
				s->RunMode.OperationMode=BAT_CONNECT_CHECK_IF_LS_STUCK;
				s->RunMode.OperationModeTimestamp=Global_1msCounter;
				return TRUE;
			}
			return TRUE;
		break;	
		case BAT_CONNECT_CHECK_IF_LS_STUCK:
			// close PC RELAIS
			// IF PC Relays is closed and LS relay is stuck closed Battery voltage should be detectet
			SwitchRelais( PRE_CHARGE_RELAIS, 1);
			s->relayState.PRECHARGE_closed=TRUE;
			if(s->RunMode.OperationModeTimestamp + CONNECT_INVERTER_MIN_WAIT_TIME_MS <= Global_1msCounter) {
				// everything ok, proceed
				s->RunMode.OperationMode=BAT_CONNECT_CHECK_IF_LS_OR_PC_BROKEN;
				return TRUE;
			}
			else if(check_if_voltage_equal(s)) {
				// Battery voltage == clamp voltage => LS is stuck close: ERROR
				s->RunMode.OperationMode=BAT_CONNECT_ERROR_LS_STUCK;
				return TRUE;
			}
			else {
				//wait
				s->RunMode.OperationMode=BAT_CONNECT_CHECK_IF_LS_STUCK;
				return TRUE;
			}
			
			return TRUE;
		break;
		case BAT_CONNECT_CHECK_IF_LS_OR_PC_BROKEN:
			// close LS and PRECHARGE
			// expected current dependend on Battery voltage, therefore not clear yet
			// skip
			SwitchRelais( LS_RELAIS, 1);
			s->relayState.LS_closed=TRUE;
			
			s->RunMode.OperationModeTimestamp=Global_1msCounter;
			s->RunMode.OperationMode=BAT_CONNECT_CHECK_IF_LS_OR_PC_BROKEN_OK;
			return TRUE;
		break;
		case BAT_CONNECT_CHECK_IF_LS_OR_PC_BROKEN_OK:
			// open PC RELAIS AGAIN
			
			SwitchRelais( PRE_CHARGE_RELAIS, 0);
			s->relayState.PRECHARGE_closed=FALSE;
			
			if(s->RunMode.OperationModeTimestamp + 1000 <= Global_1msCounter) {
				// wait for LS to be open everything ok, proceed
				s->RunMode.OperationModeTimestamp=Global_1msCounter;
				s->RunMode.OperationMode=BAT_CONNECT_CHECK_IF_HS_STUCK;
				return TRUE;
			}
			else {
				//wait
				s->RunMode.OperationMode=BAT_CONNECT_CHECK_IF_LS_OR_PC_BROKEN_OK;
			}
			
			return TRUE;
		break;
		case BAT_CONNECT_CHECK_IF_HS_STUCK:
			// check if 100V is at clamps again
			if(check_if_expected_clamp_voltage_occures(s)) {
				// everything ok
				s->RunMode.OperationMode=BAT_CONNECT_TEST_COMPLETE;
				s->RunMode.OperationModeTimestamp=Global_1msCounter;
				return TRUE;
			}
			else if(s->RunMode.OperationModeTimestamp + CONNECT_INVERTER_MAX_WAIT_TIME_MS <= Global_1msCounter) {
				// timeout, error occured, HS relays is stuck
				s->RunMode.OperationMode=BAT_CONNECT_ERROR_HS_STUCK;
				return TRUE;
			}
			else {
				//wait
				s->RunMode.OperationMode=BAT_CONNECT_CHECK_IF_HS_STUCK;
				return TRUE;
			}
			return TRUE;
		break;
		case BAT_CONNECT_TEST_COMPLETE:
			// all tests are complete
			// close PC AND LS
			SwitchRelais( HS_RELAIS, 0);
			s->relayState.HS_closed=FALSE;
			SwitchRelais( LS_RELAIS, 1);
			s->relayState.LS_closed=TRUE;
			SwitchRelais( PRE_CHARGE_RELAIS, 1);
			s->relayState.PRECHARGE_closed=TRUE;
			
			//disable relays test mode
			
			s->RunMode.BatteryExtraStatus.batteryDisconnected = 1;
			s->RunMode.BatteryExtraStatus.readyToStart=1;
			s->RunMode.BatteryExtraStatus.requestRelayTest=0;
			
			// wait for relays to open
			
			if(s->RunMode.OperationModeTimestamp + 1000 <= Global_1msCounter) {
				s->RunMode.OperationMode=BAT_CONNECT_WAIT_FOR_READY_TO_START_ACK;
				return TRUE;
			}
			else {
				//wait
				s->RunMode.OperationMode=BAT_CONNECT_TEST_COMPLETE;
				return TRUE;
			}
			return TRUE;
		break;
		case BAT_CONNECT_CLOSE_LS:
			// set Low Side relays unnecessary state because ls already closed
			s->relayState.LS_closed=TRUE;
			SwitchRelais( LS_RELAIS, 1);
					
			if(s->RunMode.OperationModeTimestamp + 1000 <= Global_1msCounter) {
				s->RunMode.OperationMode=BAT_CONNECT_CLOSE_PC;
				s->RunMode.OperationModeTimestamp = Global_1msCounter;
				return TRUE;
			}
			else {
				s->RunMode.OperationMode= BAT_CONNECT_CLOSE_LS;
				return TRUE;
			}
		break;
		case BAT_CONNECT_CLOSE_PC:
			// close precharge relays
			s->relayState.PRECHARGE_closed=TRUE;
			SwitchRelais( PRE_CHARGE_RELAIS, 1);
			
			if(s->RunMode.OperationModeTimestamp + 1000 <= Global_1msCounter) {
				s->RunMode.OperationMode=BAT_CONNECT_WAIT_FOR_READY_TO_START_ACK;
				s->RunMode.OperationModeTimestamp = Global_1msCounter;
				return TRUE;
			}
			else {
				s->RunMode.OperationMode= BAT_CONNECT_CLOSE_PC;
				return TRUE;
			}			
			return TRUE;
		break;
		case BAT_CONNECT_WAIT_FOR_READY_TO_START_ACK:
			//set ready to start bit
			s->RunMode.BatteryExtraStatus.batteryDisconnected = 1;
			s->RunMode.BatteryExtraStatus.readyToStart=1;
			s->RunMode.BatteryExtraStatus.requestRelayTest=0;
			// wait till inverter has connected
			if(s->inverter.rxStruct.BatteryControlStatus.readyToStart ==TRUE) {
				s->RunMode.OperationMode=BAT_CONNECT_WAIT_FOR_VOLTAGE_IS_EQUAL;
				return TRUE;
			}
			else {
				//wait;
				return TRUE;
			}
			return TRUE;
		break;
		case BAT_CONNECT_WAIT_FOR_VOLTAGE_IS_EQUAL:
			if(check_if_voltage_equal(s)== TRUE) {
				// go to voltage stability check
				s->RunMode.OperationMode=BAT_CONNECT_CHECK_IF_VOLTAGE_IS_STABLE;
				s->RunMode.OperationModeTimestamp = Global_1msCounter;
				return TRUE;
			}
			else {
				// wait for voltage to equalize
				s->RunMode.OperationMode=BAT_CONNECT_WAIT_FOR_VOLTAGE_IS_EQUAL;
				return TRUE;
			}
			return TRUE;
		break;
		case BAT_CONNECT_CHECK_IF_VOLTAGE_IS_STABLE:
			if(check_if_voltage_equal(s)== FALSE) {
				// return to BAT_CONNECT_CLOSE_PC and try again
				s->RunMode.OperationMode= BAT_CONNECT_CLOSE_PC;
				return TRUE;		
			}
			
			else if((s->RunMode.OperationModeTimestamp + CONNECT_INVERTER_MIN_WAIT_TIME_MS > Global_1msCounter) && check_if_Ui_voltage_is_stable(s)) {
				// Voltages are equal Highside relays can be closed
				s->RunMode.OperationMode= BAT_CONNECT_CLOSE_HS;
				s->RunMode.OperationModeTimestamp=Global_1msCounter;
				return TRUE;
			}
			else if(s->RunMode.OperationModeTimestamp + CONNECT_INVERTER_MAX_WAIT_TIME_MS > Global_1msCounter) {
				s->RunMode.OperationMode= BAT_CONNECT_WAIT_FOR_VOLTAGE_IS_EQUAL;
				return TRUE;
			}
			else {
				// just wait
				s->RunMode.OperationMode=BAT_CONNECT_CHECK_IF_VOLTAGE_IS_STABLE;
				return TRUE;
			}
			return TRUE;
		break;
		case BAT_CONNECT_CLOSE_HS:
			// Switch Highside relais
			SwitchRelais( HS_RELAIS, 1);
			s->relayState.HS_closed=TRUE;
			if(s->RunMode.OperationModeTimestamp + CONNECT_INVERTER_WAIT_FOR_HS_TO_CLOSE_MS > Global_1msCounter) {
				s->RunMode.OperationMode= BAT_CONNECT_OPEN_PC;
				return TRUE;
			}
			else {
				// do nothing
				s->RunMode.OperationMode= BAT_CONNECT_CLOSE_HS;
				return TRUE;
			}
			return TRUE;
		break;
		case BAT_CONNECT_OPEN_PC:
			s->RunMode.OperationMode=OP_MODE_NORMAL;
			s->relayState.PRECHARGE_closed=FALSE;
			SwitchRelais( PRE_CHARGE_RELAIS, 0);			
			return TRUE;
		break;
		case OP_MODE_NORMAL:
			// do nothing until SoC becomes low
			s->RunMode.BatteryExtraStatus.batteryDisconnected = 0;
			s->RunMode.BatteryExtraStatus.readyToStart=0;
			s->RunMode.BatteryExtraStatus.requestRelayTest=0;
			s->RunMode.onCounter++;
			if ( SoC < BMS_SOC_LOW_MARK) {
				s->RunMode.OperationMode=OP_MODE_SOC_LOW;
				return TRUE;
			}
			else {
				s->RunMode.OperationMode=OP_MODE_NORMAL;
				return TRUE;
			}				
			return TRUE;
		break;
		case OP_MODE_SOC_LOW:
			s->RunMode.onCounter++;
			// do nothing until SoC becomes high again
			if ( SoC > BMS_SOC_LOW_MARK) {
				s->RunMode.OperationMode=OP_MODE_NORMAL;
				return TRUE;
			}
			else {
				s->RunMode.OperationMode=OP_MODE_SOC_LOW;
				return TRUE;
			}	
		break;	
		
		case BAT_CONNECT_ERROR_SHORTCIRCUIT_DETECTED:
			// stay here and don't turn on anything
			// just to be sure open everything
			SwitchRelais( HS_RELAIS, 0);
			s->relayState.HS_closed=FALSE;
			SwitchRelais( LS_RELAIS, 0);
			s->relayState.LS_closed=FALSE;
			SwitchRelais( PRE_CHARGE_RELAIS, 0);
			s->relayState.PRECHARGE_closed=FALSE;
			ErrorStackPushMasterError(s,BMS_ERROR_STACK_MASTER_INVERTER_CONNECT_SHORT_CIRCUIT,BMS_ERROR_CLASS_3);
			
			// set battery status
			s->RunMode.BatteryExtraStatus.batteryDisconnected = 1;
			s->RunMode.BatteryExtraStatus.readyToStart=0;
			s->RunMode.BatteryExtraStatus.requestRelayTest=0;
			s->RunMode.OperationMode=BAT_CONNECT_ERROR_SHORTCIRCUIT_DETECTED;
			
			return TRUE;
		break;	
		case BAT_CONNECT_ERROR_LS_STUCK:
			// stay here and don't turn on anything
			// just to be sure open everything
			SwitchRelais( HS_RELAIS, 0);
			s->relayState.HS_closed=FALSE;
			SwitchRelais( LS_RELAIS, 0);
			s->relayState.LS_closed=FALSE;
			SwitchRelais( PRE_CHARGE_RELAIS, 0);
			s->relayState.PRECHARGE_closed=FALSE;
			ErrorStackPushMasterError(s,BMS_ERROR_STACK_MASTER_INVERTER_CONNECT_LS_STUCK,BMS_ERROR_CLASS_3);
			
			// set battery status
			s->RunMode.BatteryExtraStatus.batteryDisconnected = 1;
			s->RunMode.BatteryExtraStatus.readyToStart=0;
			s->RunMode.BatteryExtraStatus.requestRelayTest=0;
			return TRUE;
		break;	
		default:
			// something went horribly wrong
			return FALSE;
		break;
	}
	
}