BatterySwap/System/Vehicle/AppFunc.c

/*
 * @Author       : ChenJie
 * @Date         : 2021-12-15 10:40:06
 * @Version      : V3.0
 * @LastEditors  : ChenJie
 * @LastEditTime : 2022-01-11 18:14:41
 * @Description  : AppFunc
 * @FilePath     : \VehicleControl\VehicleControl\src\System\Vehicle\AppFunc.c
 */
#include "string.h"
#include "stdlib.h"
#include "HardwareLib.h"
#include "CanVar.h"
#include "math.h"
#include "AppFunc.h"
#include "Std_Types.h"
void LockAndUnlockCtrl()
{
	static uint16 TimerCounter = 0;
	TimerCounter = TimerCounter + 10;

	if (!ebcd_flg_ebcEolDone) //下线检测模式
	{
		static uint16 Delay = 0;
		Delay = Delay + 10;
		if (Delay > 10000)
		{
			PSwtDrv_Interface(_PSWT_INDEX_HBAK1_CHAN, 1);
		}
		else
		{
			PSwtDrv_Interface(_PSWT_INDEX_HBAK1_CHAN, 0);
		}
		if (Delay > 20000)
		{
			Delay = 0;
		}
	}
	else //正常工作模式
	{
	}
}
/**
 * @brief : 互锁检测函数
 * @param {*}
 * @return {*}
 */
void GetHVLockState(void)
{
	UINT8 m_flg_HVlock1 = 0, m_flg_HVlock2 = 0;
	// PWM输出，互锁1检测，频率100HZ，占空比30%
	uint16 VehCo_fTestUI = 1000;
	uint16 VehCo_rTestUW = 3000;
	uint32 PwmFreAcq = 0;
	uint16 PwmDutyAcq = 0;
	PPWMDrv_Interface(_PPWM_INDEX_HVLOCK2, VehCo_fTestUI, VehCo_rTestUW);
	PulseAcqDrv_GetChanFreq(_PULSEACQ_INDEX_HVLOCK2, &PwmFreAcq, &PwmDutyAcq);
	if (abs(PwmFreAcq - VehCo_fTestUI) < 100 && abs(PwmDutyAcq - VehCo_rTestUW) < 500)
	{
		m_flg_HVlock1 = 1;
	}
	else
	{
		m_flg_HVlock1 = 0;
	}
	//互锁2检测，配置高有效，悬空为0则未接入，高电平为1则接入，
	// DINDrv_SetChanThres(_DIN_INDEX_PLUGHVLOCK, 0, 4095 * 3);
	m_flg_HVlock2 = DINDrv_GetChanState(_DIN_INDEX_PLUGHVLOCK);
}
/**
 * @brief : 数字量传感器信号检测函数
 * @param {*}
 * @return {*}
 */
void GetDIOState(void)
{
	uint8 temp[4];
	//松开传感器检测，配置低有效，底层悬空为1，触发为0,应用层输出悬空为0，触发为1
	memset(temp, 0x00, 4);
	ebcd_st_unlockSensor = 0;
	DINDrv_SetChanThres(_DIN_INDEX_UNLOCKSENSOR1, 1, 4095U);
	DINDrv_SetChanThres(_DIN_INDEX_UNLOCKSENSOR2, 1, 4095U);
	DINDrv_SetChanThres(_DIN_INDEX_UNLOCKSENSOR3, 1, 4095U);
	DINDrv_SetChanThres(_DIN_INDEX_UNLOCKSENSOR4, 1, 4095U);
	temp[0] = !DINDrv_GetChanState(_DIN_INDEX_UNLOCKSENSOR1);
	temp[1] = !DINDrv_GetChanState(_DIN_INDEX_UNLOCKSENSOR2);
	temp[2] = !DINDrv_GetChanState(_DIN_INDEX_UNLOCKSENSOR3);
	temp[3] = !DINDrv_GetChanState(_DIN_INDEX_UNLOCKSENSOR4);
	ebcd_st_unlockSensor = (getbit(temp[3], 0) << 3) | (getbit(temp[2], 0) << 2) | (getbit(temp[1], 0) << 1) | (getbit(temp[0], 0) << 0);

	//夹紧传感器检测，配置低有效，底层悬空为1，触发为0,应用层输出悬空为0，触发为1
	memset(temp, 0x00, 4);
	ebcd_st_lockSensor = 0;
	DINDrv_SetChanThres(_DIN_INDEX_LOCKSENSOR1, 1, 4095U);
	DINDrv_SetChanThres(_DIN_INDEX_LOCKSENSOR2, 1, 4095U);
	DINDrv_SetChanThres(_DIN_INDEX_LOCKSENSOR3, 1, 4095U);
	DINDrv_SetChanThres(_DIN_INDEX_LOCKSENSOR4, 1, 4095U);
	temp[0] = !DINDrv_GetChanState(_DIN_INDEX_LOCKSENSOR1);
	temp[1] = !DINDrv_GetChanState(_DIN_INDEX_LOCKSENSOR2);
	temp[2] = !DINDrv_GetChanState(_DIN_INDEX_LOCKSENSOR3);
	temp[3] = !DINDrv_GetChanState(_DIN_INDEX_LOCKSENSOR4);
	ebcd_st_lockSensor = (getbit(temp[3], 0) << 3) | (getbit(temp[2], 0) << 2) | (getbit(temp[1], 0) << 1) | (getbit(temp[0], 0) << 0);

	//落座传感器检测，高有效，悬空为0则未接入，高电平为1则接入
	memset(temp, 0x00, 4);
	temp[0] = !DINDrv_GetChanState(_DIN_INDEX_READYSENSOR1);
	temp[1] = !DINDrv_GetChanState(_DIN_INDEX_READYSENSOR2);
	ebcd_st_pedstSensor = (getbit(temp[1], 0) << 1) | (getbit(temp[0], 0) << 0);

	//根据上述量得到，运动状态值
	if (ebcd_st_lockSensor == 0x0F)
	{
		ebcd_st_lockSucJug = 1;
	}
	else if (ebcd_st_lockSensor == 0x00)
	{
		ebcd_st_lockSucJug = 0;
	}
	else
	{
		ebcd_st_lockSucJug = 2;
	}
	if (ebcd_st_unlockSensor == 0x0F)
	{
		ebcd_st_unlockSucJug = 1;
	}
	else if (ebcd_st_unlockSensor == 0x00)
	{
		ebcd_st_unlockSucJug = 2;
	}
	else
	{
		ebcd_st_unlockSucJug = 0;
	}
	if (ebcd_st_pedstSucJug == 0x03)
	{
		ebcd_st_pedstSucJug = 1;
	}
	else if (ebcd_st_pedstSucJug == 0x00)
	{
		ebcd_st_pedstSucJug = 0;
	}
	else
	{
		ebcd_st_pedstSucJug = 2;
	}
}

/**
 * @brief : 获取模拟量输入值，并进行转换
 * @param {*}
 * @return {*}
 */
void GetAIOValue(void)
{
	uint16 AirPressureTemp_Vol = 0;
	uint16 PluginTemp1_Vol = 0;
	uint32 PluginTemp1_R = 0;
	uint16 PluginTemp2_Vol = 0;
	uint32 PluginTemp2_R = 0;
	uint16 PluginTemp3_Vol = 0;
	uint32 PluginTemp3_R = 0;
	uint16 PluginTemp4_Vol = 0;
	uint32 PluginTemp4_R = 0;

	AirPressureTemp_Vol = ATDDrv_GetChanResult(_ATD_INDEX_AIRPRESSURE);
	AirPressureTemp_Vol = (uint16)(AirPressureTemp_Vol * 1000 / 4095.0);
	/*气压数据转换*/
	AirPressureTemp_Vol = MAX(AirPressureTemp_Vol, 500);
	AirPressureTemp_Vol = MIN(AirPressureTemp_Vol, 4500);
	ebcd_P_airSensor = (uint8)((AirPressureTemp_Vol - 500) / 40);
	/*温度采集获取*/
	PluginTemp1_Vol = ATDDrv_GetChanResult(_ATD_INDEX_PLUGINTEMP1);
	PluginTemp2_Vol = ATDDrv_GetChanResult(_ATD_INDEX_PLUGINTEMP2);
	PluginTemp3_Vol = ATDDrv_GetChanResult(_ATD_INDEX_PLUGINTEMP3);
	PluginTemp4_Vol = ATDDrv_GetChanResult(_ATD_INDEX_PLUGINTEMP4);
	PluginTemp1_R = (uint32)((PluginTemp1_Vol / (5.0 * 4095 - PluginTemp1_Vol)) * 1000.0);
	PluginTemp2_R = (uint32)((PluginTemp2_Vol / (5.0 * 4095 - PluginTemp2_Vol)) * 1000.0);
	PluginTemp3_R = (uint32)((PluginTemp3_Vol / (5.0 * 4095 - PluginTemp3_Vol)) * 1000.0);
	PluginTemp4_R = (uint32)((PluginTemp4_Vol / (5.0 * 4095 - PluginTemp4_Vol)) * 1000.0);
	ebcd_T_plugin[0] = (uint8)Look1_u32u8(PluginTemp1_R, R_table, T_table, 240);
	ebcd_T_plugin[1] = (uint8)Look1_u32u8(PluginTemp2_R, R_table, T_table, 240);
	ebcd_T_plugin[2] = (uint8)Look1_u32u8(PluginTemp3_R, R_table, T_table, 240);
	ebcd_T_plugin[3] = (uint8)Look1_u32u8(PluginTemp4_R, R_table, T_table, 240);
}
/**
 * @brief : lookUp Table Fun
 * @param {uint32} u0 x
 * @param {uint32} bp0 x_table
 * @param {uint16} table y_table
 * @param {uint16} maxIndex
 * @return {*}
 */
uint16 Look1_u32u8(uint32 u0, uint32 *bp0, uint8 *table, uint16 MaxLen)
{
	uint32 bpIdx = 0;
	uint32 iLeft = 0;
	uint32 iRght = 0;
	uint16 y = 0;
	uint32 yL_0d0 = 0;
	uint32 yR_0d0 = 0;
	uint32 maxIndex = MaxLen - 1;
	if (u0 <= bp0[0U])
	{
		iLeft = 0U;
		iRght = 0U;
	}
	else if (u0 < bp0[maxIndex])
	{
		//对折法寻找u0的位置
		bpIdx = maxIndex >> 1U;
		iLeft = 0U;
		iRght = maxIndex;
		while ((iRght - iLeft) > 1)
		{
			if (u0 < bp0[bpIdx])
			{
				iRght = bpIdx;
			}
			else
			{
				iLeft = bpIdx;
			}
			bpIdx = (iRght + iLeft) >> 1U;
		}
	}
	else
	{
		iLeft = maxIndex;
		iRght = maxIndex;
	}
	//找到位置以后计算插值
	if (iLeft != iRght)
	{
		//线性插值
		yR_0d0 = table[iLeft + 1U];
		yL_0d0 = table[iLeft];
		if (yR_0d0 >= yL_0d0)
		{
			y = (uint16)(((uint32)(u0 - bp0[iLeft]) * (yR_0d0 - yL_0d0)) / (bp0[iLeft + 1] - bp0[iLeft]) + yL_0d0);
		}
		else
		{
			y = (uint16)(yL_0d0 - ((uint32)(u0 - bp0[iLeft]) * (yL_0d0 - yR_0d0)) / (bp0[iLeft + 1] - bp0[iLeft]));
		}
	}
	else
	{
		y = (uint16)table[iLeft];
	}
	return y;
}
#ifdef _APP_TEST_CODE
void TestDeviceFun(void)
{
	uint8 ManuEnable = 0;
	//控制算法
	AccPedCD_Update();
	AccPedCD_Monitor();

	PulseAcqDrv_GetChanFreq(_PULSEACQ_INDEX_CP_CHAN, &VehCo_fInputUIA[0], &VehCo_rInputUWA[0]);

	// DODrv_SetChanState(_DO_INDEX_CPCTL_CHAN,(uint8)VehCo_bTestCPCtrlUW_C);

	uint16 DistenceBufferV[2] = {0, 0}, DistenceBufferR[2] = {0, 0};
	//开关1采集
	DINDrv_SetChanThres(_DIN_INDEX_BAK1_CHAN, 1, 4095U);
	Switch1 = !DINDrv_GetChanState(_DIN_INDEX_BAK1_CHAN);
	//手动控制采集
	DINDrv_SetChanThres(_DIN_INDEX_BAK2_CHAN, 1, 4095U);
	ManuEnable = !DINDrv_GetChanState(_DIN_INDEX_BAK2_CHAN);
	//位移量采集
	DistenceBufferV[0] = ATDDrv_GetChanResult(_ATD_INDEX_BAK3_CHAN);
	DistenceBufferR[0] = (uint16)((DistenceBufferV[0] / (5.0 * 4095 - DistenceBufferV[0])) * 1000.0);
	Distence1 = (uint16)((2132 - DistenceBufferR[0]) * (0.1219));
	//压力采集
	PressureValueBuffer = ATDDrv_GetChanResult(_ATD_INDEX_ACCPED1_CHAN);
	PressureValue = (uint32)(PressureValueBuffer * 1000 / 4095.0);
	//控制输出
	VehCo_ctEEPTestUI += 10;
	VehCo_ctEEPTestUB += 10;
	if (VehCo_ctEEPTestUI >= 4 * 1000)
	{
		PSwtDrv_Interface(_PSWT_INDEX_HBAK1_CHAN, 1); //输出 解锁持续9-4s
		ControlState = 1;							  //解锁
	}
	else
	{
		PSwtDrv_Interface(_PSWT_INDEX_HBAK1_CHAN, 0); //不输出
		ControlState = 2;							  //锁定 4 秒
	}
	if (VehCo_ctEEPTestUI >= 9 * 1000)
	{
		Control_Times++;
		VehCo_ctEEPTestUI = 0;
	}
	if (ManuEnable == 0)
	{
		Control_Times = 0;
		VehCo_ctEEPTestUI = 0;
	}
}
#endif
#ifdef _APP_TEST_MOTOR_CODE
void MotorTestFun(void)
{
	static uint32 MotorRunTimer = 0;
	static uint16 MotorLockReadyTimer = 0;
	static sint8 LockMotorNum = -1;
	static sint8 unLockMotorNum = -1;
	static uint32 testTimer = 0;
	GetDIOState();
	GetAIOValue();
	testTimer = testTimer + 10;
	MotorControlLockNum = LockMotorNum;
	MotorControlunLockNum = unLockMotorNum;
	if (MotorControlCmd == 0) //可进入调试模式
	{
		if (MotorDebugCmd == 1)
		{
			if (getbit(MotorControlEnable, 0) == 1)
			{
				MotorControlFunc(0, 1);
				return;
			}
			else if (getbit(MotorControlEnable, 1) == 1)
			{
				MotorControlFunc(1, 1);
				return;
			}
			else if (getbit(MotorControlEnable, 2) == 1)
			{
				MotorControlFunc(2, 1);
				return;
			}
			else if (getbit(MotorControlEnable, 3) == 1)
			{
				MotorControlFunc(3, 1);
				return;
			}
			else
			{
				return;
			}
		}
		else if (MotorDebugCmd == 2)
		{
			if (getbit(MotorControlEnable, 0) == 1)
			{
				MotorControlFunc(0, 2);
				return;
			}
			else if (getbit(MotorControlEnable, 1) == 1)
			{
				MotorControlFunc(1, 2);
				return;
			}
			else if (getbit(MotorControlEnable, 2) == 1)
			{
				MotorControlFunc(2, 2);
				return;
			}
			else if (getbit(MotorControlEnable, 3) == 1)
			{
				MotorControlFunc(3, 2);
				return;
			}
			else
			{
				return;
			}
		}
		else
		{
			if (MotorLifeTestCmd == 1)
			{
				if (LockMotorNum < 4 && testTimer > 5000)
				{
					MotorControlCmd = 1;
				}
				else if (unLockMotorNum < 5 && LockMotorNum == 4 && testTimer > 5000)
				{
					MotorControlCmd = 2;
				}
			}
			else
			{
				MotorControlFunc(0, 0xF1);
				LockMotorNum = -1;
				unLockMotorNum = -1;
				return;
			}
		}
	}
	else if (MotorControlCmd == 1) //控制锁紧的自动流程
	{
		switch (LockMotorNum)
		{
		case -1:
		{
			MotorControlFunc(0, 0xF1);
			LockMotorNum = 0;
			break;
		}
		case 0:
		{
			if ((MotorRunTimer >= RunFailedDelay) || (MotorLockReadyTimer >= LockDelay) || getbit(MotorControlEnable, LockMotorNum) != 1)
			{
				MotorControlFunc(LockMotorNum, 0xF1);
				LockMotorNum = 1;
				MotorRunTimer = 0;
				MotorLockReadyTimer = 0;
			}
			else
			{
				if (getbit(ebcd_st_lockSensor, LockMotorNum) == 1)
				{
					MotorLockReadyTimer = MotorLockReadyTimer + 10;
				}
				MotorControlFunc(LockMotorNum, 1);
				MotorRunTimer = MotorRunTimer + 10;
			}
			break;
		}
		case 1:
		{
			if ((MotorRunTimer >= RunFailedDelay) || (MotorLockReadyTimer >= LockDelay) || getbit(MotorControlEnable, LockMotorNum) != 1)
			{
				MotorControlFunc(LockMotorNum, 0xF1);
				LockMotorNum = 2;
				MotorRunTimer = 0;
				MotorLockReadyTimer = 0;
			}
			else
			{
				if (getbit(ebcd_st_lockSensor, LockMotorNum) == 1)
				{
					MotorLockReadyTimer = MotorLockReadyTimer + 10;
				}
				MotorControlFunc(LockMotorNum, 1);
				MotorRunTimer = MotorRunTimer + 10;
			}
			break;
		}
		case 2:
		{
			if ((MotorRunTimer >= RunFailedDelay) || (MotorLockReadyTimer >= LockDelay) || getbit(MotorControlEnable, LockMotorNum) != 1)
			{
				MotorControlFunc(LockMotorNum, 0xF1);
				LockMotorNum = 3;
				MotorRunTimer = 0;
				MotorLockReadyTimer = 0;
			}
			else
			{
				if (getbit(ebcd_st_lockSensor, LockMotorNum) == 1)
				{
					MotorLockReadyTimer = MotorLockReadyTimer + 10;
				}
				MotorControlFunc(LockMotorNum, 1);
				MotorRunTimer = MotorRunTimer + 10;
			}
			break;
		}
		case 3:
		{
			if ((MotorRunTimer >= RunFailedDelay) || (MotorLockReadyTimer >= LockDelay) || getbit(MotorControlEnable, LockMotorNum) != 1)
			{
				MotorControlFunc(LockMotorNum, 0xF1);
				LockMotorNum = 4;
				unLockMotorNum = -1;
				MotorRunTimer = 0;
				MotorLockReadyTimer = 0;
				testTimer = 0;
			}
			else
			{
				if (getbit(ebcd_st_lockSensor, LockMotorNum) == 1)
				{
					MotorLockReadyTimer = MotorLockReadyTimer + 10;
				}
				MotorControlFunc(LockMotorNum, 1);
				MotorRunTimer = MotorRunTimer + 10;
			}
			break;
		}
		default:
			break;
		}
	}
	else if (MotorControlCmd == 2) //控制解锁的自动流程
	{
		switch (unLockMotorNum)
		{
		case -1:
		{
			MotorControlFunc(0, 0xF2);
			unLockMotorNum = 0;
			break;
		}
		case 0:
		{
			if ((MotorRunTimer >= RunFailedDelay) || (MotorLockReadyTimer >= 1) || getbit(MotorControlEnable, unLockMotorNum) != 1)
			{
				MotorControlFunc(unLockMotorNum, 0xF2);
				unLockMotorNum = 1;
				MotorRunTimer = 0;
				MotorLockReadyTimer = 0;
			}
			else
			{
				if (getbit(ebcd_st_unlockSensor, unLockMotorNum) == 1)
				{
					MotorLockReadyTimer = MotorLockReadyTimer + 10;
				}
				MotorControlFunc(unLockMotorNum, 2);
				MotorRunTimer = MotorRunTimer + 10;
			}
			break;
		}
		case 1:
		{
			if ((MotorRunTimer >= RunFailedDelay) || (MotorLockReadyTimer >= 1) || getbit(MotorControlEnable, unLockMotorNum) != 1)
			{
				MotorControlFunc(unLockMotorNum, 0xF2);
				unLockMotorNum = 2;
				MotorRunTimer = 0;
				MotorLockReadyTimer = 0;
			}
			else
			{
				if (getbit(ebcd_st_unlockSensor, unLockMotorNum) == 1)
				{
					MotorLockReadyTimer = MotorLockReadyTimer + 10;
				}
				MotorControlFunc(unLockMotorNum, 2);
				MotorRunTimer = MotorRunTimer + 10;
			}
			break;
		}
		case 2:
		{
			if ((MotorRunTimer >= RunFailedDelay) || (MotorLockReadyTimer >= 1) || getbit(MotorControlEnable, unLockMotorNum) != 1)
			{
				MotorControlFunc(unLockMotorNum, 0xF2);
				unLockMotorNum = 3;
				MotorRunTimer = 0;
				MotorLockReadyTimer = 0;
			}
			else
			{
				if (getbit(ebcd_st_unlockSensor, unLockMotorNum) == 1)
				{
					MotorLockReadyTimer = MotorLockReadyTimer + 10;
				}
				MotorControlFunc(unLockMotorNum, 2);
				MotorRunTimer = MotorRunTimer + 10;
			}
			break;
		}
		case 3:
		{
			if ((MotorRunTimer >= RunFailedDelay) || (MotorLockReadyTimer >= 1) || getbit(MotorControlEnable, unLockMotorNum) != 1)
			{
				MotorControlFunc(unLockMotorNum, 0xF2);
				unLockMotorNum = 4;
				MotorRunTimer = 0;
				MotorLockReadyTimer = 0;
			}
			else
			{
				if (getbit(ebcd_st_unlockSensor, unLockMotorNum) == 1)
				{
					MotorLockReadyTimer = MotorLockReadyTimer + 10;
				}
				MotorControlFunc(unLockMotorNum, 2);
				MotorRunTimer = MotorRunTimer + 10;
			}
			break;
		}
		case 4:
		{
			MotorControlFunc(0, 0xF1);
			unLockMotorNum = 5;
			LockMotorNum = -1;
			if (MotorLifeTestCmd == 1)
			{
				LifeTestCounter++;
				testTimer = 0;
				EEPROMDrv_bSaveInstantUB = 1;
			}
			break;
		}
		default:
			break;
		}
	}
}
void MotorControlFunc(UINT8 MotorIdx, UINT8 RotateDirec) // MotorIdx 0-3 表示四个电机，RotateDirec F1停止正转，F2表示反转停止，1表示正转，2表示反转
{
	switch (MotorIdx)
	{
	case 0:
	{
		switch (RotateDirec)
		{
		case 0xF1:
		{
			PSwtDrv_Interface(_M1_C, 0);
			PSwtDrv_Interface(_M2_C, 0);
			PSwtDrv_Interface(_M3_C, 0);
			PSwtDrv_Interface(_M4_C, 0);
			PSwtDrv_Interface(_M_D_C, 0);
			break;
		}
		case 0xF2:
		{
			PSwtDrv_Interface(_M1_C, 1);
			PSwtDrv_Interface(_M2_C, 1);
			PSwtDrv_Interface(_M3_C, 1);
			PSwtDrv_Interface(_M4_C, 1);
			PSwtDrv_Interface(_M_D_C, 1);
			break;
		}
		case 1:
		{
			PSwtDrv_Interface(_M1_C, 0);
			PSwtDrv_Interface(_M_D_C, 0);
			PSwtDrv_Interface(_M1_C, 1);
			break;
		}
		case 2:
		{
			PSwtDrv_Interface(_M1_C, 1);
			PSwtDrv_Interface(_M_D_C, 1);
			PSwtDrv_Interface(_M1_C, 0);
			break;
		}
		default:
			break;
		}
		break;
	}
	case 1:
	{
		switch (RotateDirec)
		{
		case 0xF1:
		{
			PSwtDrv_Interface(_M1_C, 0);
			PSwtDrv_Interface(_M2_C, 0);
			PSwtDrv_Interface(_M3_C, 0);
			PSwtDrv_Interface(_M4_C, 0);
			PSwtDrv_Interface(_M_D_C, 0);
			break;
		}
		case 0xF2:
		{
			PSwtDrv_Interface(_M1_C, 1);
			PSwtDrv_Interface(_M2_C, 1);
			PSwtDrv_Interface(_M3_C, 1);
			PSwtDrv_Interface(_M4_C, 1);
			PSwtDrv_Interface(_M_D_C, 1);
			break;
		}
		case 0:
		{
			PSwtDrv_Interface(_M2_C, 0);
			PSwtDrv_Interface(_M_D_C, 0);
			break;
		}
		case 1:
		{
			PSwtDrv_Interface(_M2_C, 0);
			PSwtDrv_Interface(_M_D_C, 0);
			PSwtDrv_Interface(_M2_C, 1);
			break;
		}
		case 2:
		{
			PSwtDrv_Interface(_M2_C, 1);
			PSwtDrv_Interface(_M_D_C, 1);
			PSwtDrv_Interface(_M2_C, 0);
			break;
		}
		default:
			break;
		}
		break;
	}
	case 2:
	{
		switch (RotateDirec)
		{
		case 0xF1:
		{
			PSwtDrv_Interface(_M1_C, 0);
			PSwtDrv_Interface(_M2_C, 0);
			PSwtDrv_Interface(_M3_C, 0);
			PSwtDrv_Interface(_M4_C, 0);
			PSwtDrv_Interface(_M_D_C, 0);
			break;
		}
		case 0xF2:
		{
			PSwtDrv_Interface(_M1_C, 1);
			PSwtDrv_Interface(_M2_C, 1);
			PSwtDrv_Interface(_M3_C, 1);
			PSwtDrv_Interface(_M4_C, 1);
			PSwtDrv_Interface(_M_D_C, 1);
			break;
		}
		case 0:
		{
			PSwtDrv_Interface(_M3_C, 0);
			PSwtDrv_Interface(_M_D_C, 0);
			break;
		}
		case 1:
		{
			PSwtDrv_Interface(_M3_C, 0);
			PSwtDrv_Interface(_M_D_C, 0);
			PSwtDrv_Interface(_M3_C, 1);
			break;
		}
		case 2:
		{
			PSwtDrv_Interface(_M3_C, 1);
			PSwtDrv_Interface(_M_D_C, 1);
			PSwtDrv_Interface(_M3_C, 0);
			break;
		}
		default:
			break;
		}
		break;
	}
	case 3:
	{
		switch (RotateDirec)
		{
		case 0xF1:
		{
			PSwtDrv_Interface(_M1_C, 0);
			PSwtDrv_Interface(_M2_C, 0);
			PSwtDrv_Interface(_M3_C, 0);
			PSwtDrv_Interface(_M4_C, 0);
			PSwtDrv_Interface(_M_D_C, 0);
			break;
		}
		case 0xF2:
		{
			PSwtDrv_Interface(_M1_C, 1);
			PSwtDrv_Interface(_M2_C, 1);
			PSwtDrv_Interface(_M3_C, 1);
			PSwtDrv_Interface(_M4_C, 1);
			PSwtDrv_Interface(_M_D_C, 1);
			break;
		}
		case 0:
		{
			PSwtDrv_Interface(_M4_C, 0);
			PSwtDrv_Interface(_M_D_C, 0);
			break;
		}
		case 1:
		{
			PSwtDrv_Interface(_M4_C, 0);
			PSwtDrv_Interface(_M_D_C, 0);
			PSwtDrv_Interface(_M4_C, 1);
			break;
		}
		case 2:
		{
			PSwtDrv_Interface(_M4_C, 1);
			PSwtDrv_Interface(_M_D_C, 1);
			PSwtDrv_Interface(_M4_C, 0);
			break;
		}
		default:
			break;
		}
		break;
	}
	default:
		break;
	}
}
#endif