/******************************************************************************

                  版权所有 (C), 2018-2099, Radkil

 ******************************************************************************
  文 件 名   : paint_robot_new.c
  版 本 号   : 初稿
  作    者   : radkil
  生成日期   : 2026年7月14日
  最近修改   :
  功能描述   : 新版喷漆主逻辑
  
  修改历史   :
  1.日    期   : 2026年7月14日
    作    者   : radkil
    修改内容   : 创建文件

******************************************************************************/
#include "BHBF.h"
#include "msg_center.h"
#include "lua_base.h"
#include "Protobuf/PSource/bsp_PV.pb.h"
#include "Protobuf/PSource/msp_MK32.pb.h"
#include "Protobuf/PSource/bsp_CV.pb.h"
#include "Protobuf/PSource/bsp_IV.pb.h"

/*----------------------------------------------*
 * 外部变量说明                                 *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 外部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 内部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 全局变量                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 模块级变量                                   *
 *----------------------------------------------*/
static uint32_t g_uiCustomModuleID = 0;
static SP_MSP_MK32_Button g_stMK32;
static PV_struct_define g_stPV;
static IV_struct_define g_stIV;
static int g_Paint_State = -1; // 喷枪状态，0表示打开，1表示关闭
static int g_angle_protect_lock = 0; // 为1表示角度异常，触发停车逻辑Move_Halt_AngleError
static int g_ipaintOffCount = 0; // Move_Halt_AngleError中的计时，即角度偏移超过多少时间触发停枪
static int g_Is_All_Button_Reset = 0; // 1表示按钮正常工作，0表示需要复位

/*----------------------------------------------*
 * 常量定义                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 宏定义                                       *
 *----------------------------------------------*/

#define IV_SEND_TIME 50 // IV上报周期（单位毫秒）

static int32_t RunTime_DistanceCm_SpeedE_2MPMin(void)
{
	return (600 * (g_stPV.LaneChangeDistance + g_stCV.Vertical_ChangeLane_Compensation) / g_stCV.Lane_Change_Speed_m_per_min);
}

static void Move_Halt_AngleError(void)
{
	static uint32_t uiLastTime = 0; 
	if (g_stPV.RunMode != 2 && g_stPV.RunMode != 3)
	{
		return;
	}

	if (1 == g_angle_protect_lock)
	{
		if (Rd_GetTime() - uiLastTime >= 600 * g_stCV.Paint_Gun_Shutdown_Distance / g_stIV.RobotMoveSpeed)
		{
			MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0);
		}
		return;
	}
	else 
	{
		uiLastTime = Rd_GetTime();
	}

	if ((abs(g_stIV.CurrentAngle - g_stCV.RobotUpAngleValue) > g_stCV.Robot_Permitted_Angler_Error_Value_E_2D) && 0 == g_Paint_State)
	{
		if(Rd_GetTime() - g_ipaintOffCount > 1000)
		{
			MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);//先关枪
			g_angle_protect_lock = 1;
			return;
		}
	}
	else
	{
		g_ipaintOffCount = Rd_GetTime();
	}
}

static void Custom_ModuleHandler(const Msg_t *pstMsg)
{
    if (NULL == pstMsg)
    {
        return;
    }

    static int iLeft_Compensation = 0;
    static int iRight_Compensation = 0;
	static int iVehicleSpeed = 1;
	static int iIsStopOffPaint = 0;
	static int iPaint = 1;
    switch (pstMsg->m_uiMsgID)
    {
		case CUSTOM_RESET_PAINT:
		{
			iPaint = 1;
        	break;
        }
		case CUSTOM_SET_STOP_OFF_PAINT:
		{
			iIsStopOffPaint = 1;
		}
    	case CUSTOM_GET_DAEMON_CODE:
		{
			if (pstMsg->m_uiDataLen >= sizeof(int32_t))
			{
				RD_MEMCPY(&g_stIV.SystemError, pstMsg->m_aucData, sizeof(int32_t));
			}
			break;
		}
    	case CUSTOM_GET_FAULT_CODE:
		{
			uint32_t uiFailtCode[2] = {0};
			if (pstMsg->m_uiDataLen >= sizeof(uiFailtCode))
			{
				RD_MEMCPY(uiFailtCode, pstMsg->m_aucData, sizeof(uiFailtCode));
				if (uiFailtCode[0] == 1) g_stIV.Left_Motor_Err = uiFailtCode[1];
				else g_stIV.Right_Motor_Err = uiFailtCode[1];
			}
			break;
		}
    	case CUSTOM_GET_SPEED:
		{
			int32_t iCurrentSpeed[2] = {0};
			if (pstMsg->m_uiDataLen >= sizeof(iCurrentSpeed))
			{
				RD_MEMCPY(iCurrentSpeed, pstMsg->m_aucData, sizeof(iCurrentSpeed));
				if (1 == iCurrentSpeed[0])
				{
					g_stIV.CurrentSpeed = iCurrentSpeed[1];
					log_d("speed = %d", g_stIV.CurrentSpeed);
				}
			}
			break;
		}
    	case CUSTOM_GET_TL720D_ROLL:
    	{
    		if (pstMsg->m_uiDataLen >= sizeof(int32_t))
			{
				RD_MEMCPY(&g_stIV.CurrentAngle, pstMsg->m_aucData, sizeof(int32_t));
			}
    		break;
    	}
		case CUSTOM_GET_PV:
		{
			if (pstMsg->m_uiDataLen >= sizeof(g_stPV))
			{
				RD_MEMCPY(&g_stPV, pstMsg->m_aucData, sizeof(g_stPV));
			}
			break;
		}
		case CUSTOM_CMD_STRAIGHT_DRIVE:
		{
			BHBF_straight_drive_Cmd stCmd = {0};
			if (pstMsg->m_uiDataLen >= sizeof(stCmd))
			{
				RD_MEMCPY(&stCmd, pstMsg->m_aucData, sizeof(stCmd));
				int iTargetAngle = g_stCV.RobotUpAngleValue;
				MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
			}
			break;
		}
		case CUSTOM_CMD_TURN_ANGLE:
		{
			int iTargetAngle = 0;
			if (pstMsg->m_uiDataLen >= sizeof(int))
			{
				RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int));
				if (iTargetAngle == g_stCV.RobotUpAngleValue)
				{
					log_d("change line success");
				}
				else if (iTargetAngle == g_stCV.RobotLeftAngleValue)
				{
					if (g_stPV.RunMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上
					{
						BHBF_straight_drive_Cmd stCmd = {
							.m_iMode = 1,
							.m_iAngle = g_stCV.RobotLeftAngleValue + iLeft_Compensation,
							.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
							.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
							};
						MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
					}
					else if (g_stPV.RunMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
					{
						BHBF_straight_drive_Cmd stCmd = {
							.m_iMode = -1,
							.m_iAngle = g_stCV.RobotLeftAngleValue - iRight_Compensation,
							.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
							.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
							};
						MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
					}
				}
				else if (iTargetAngle == g_stCV.RobotRightAngleValue)
				{
					if (g_stPV.RunMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
					{
						BHBF_straight_drive_Cmd stCmd = {
							.m_iMode = -1,
							.m_iAngle = g_stCV.RobotRightAngleValue + iLeft_Compensation,
							.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
							.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
							};
						MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
					}
					else if (g_stPV.RunMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
					{
						BHBF_straight_drive_Cmd stCmd = {
							.m_iMode = 1,
							.m_iAngle = g_stCV.RobotRightAngleValue - iRight_Compensation,
							.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
							.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
							};
						MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
					}
				}
				else if (iTargetAngle == g_stCV.RobotDownAngleValue)
				{

				}
			}
			break;
		}
		case COMMON_CMD_STOP_ALL:
		{
			HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, 1);
			g_Paint_State = 1;
			break;
		}
		case CUSTOM_CMD_PAINTGUN:
		{
			int paintstate = -1;
			if (pstMsg->m_uiDataLen >= sizeof(paintstate))
			{
				RD_MEMCPY(&paintstate, pstMsg->m_aucData, sizeof(paintstate));
				HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, paintstate);
			}
			if (0 == paintstate) iPaint = 0;
			break;
		}
		case CUSTOM_GET_MK32:
		{
			if (pstMsg->m_uiDataLen >= sizeof(g_stMK32))
			{
				RD_MEMCPY(&g_stMK32, pstMsg->m_aucData, sizeof(g_stMK32));
			}

		    if (g_Is_All_Button_Reset == 0)
			{
				if (g_stMK32.CH4_SA == 0 && g_stMK32.CH5_SB == 0 && g_stMK32.CH6_SC == 0
						&& g_stMK32.CH7_SD == 0 && g_stMK32.IsOnline == 1)
				{
					MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_ENABLE, NULL, 0);
					g_Is_All_Button_Reset = 1;
				}
			}
			else if (g_Is_All_Button_Reset == 1)
			{
				MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_BUTTON_RESET, NULL, 0);
			}

			// 急停或者遥控器失联
			if ((g_stMK32.CH8_SE == -1000 && g_stMK32.CH9_SF == -1000)
			 || (g_stMK32.IsOnline == 0 && (g_stMK32.CH8_SE == -1000 || g_stMK32.CH8_SE == 1000 || g_stMK32.CH8_SE == 0) 
			                            && (g_stMK32.CH9_SF == -1000 || g_stMK32.CH9_SF == 1000 || g_stMK32.CH9_SF == 0))
			 || g_stIV.Left_Motor_Err != 0 || g_stIV.Right_Motor_Err != 0)
			{
				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_DISABLE, NULL, 0);
				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
				g_Is_All_Button_Reset = 0;
				break;
			}

			// 按键处于默认位置安卓界面可控
			if ((fabs(g_stMK32.CH2_LY_V) <= 200) && (fabs(g_stMK32.CH3_LY_H) <= 200)
				&& (fabs(g_stMK32.CH0_RY_H) <= 200) && (fabs(g_stMK32.CH1_RY_V) <= 200)
				&& (g_stMK32.CH4_SA ==0) && (g_stMK32.CH5_SB == 0)
				&& (g_stMK32.CH6_SC != -1000) )
			{
				g_stIV.IsWorking = 0;
				g_angle_protect_lock = 0; // 遥控器复位认为打开角度锁
				g_ipaintOffCount = 0;
			}
			else
			{
				g_stIV.IsWorking = 1;
			}

			// 更新速度旋钮值
			int iSpeedSelection = 3 * (g_stMK32.CH11_RD1 + 1000) / 200;
			if (iSpeedSelection > 0)
			{
				iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30;
			}
			g_stIV.RobotMoveSpeed = iVehicleSpeed;
			MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int));
			MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int));

			Move_Halt_AngleError();

			int iTargetAngle = 0;
			// 根据模式判断换道（仅在竖直向左或向右生效）
			if (g_stMK32.CH4_SA == 1000)
			{
				if (g_stPV.RunMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上
				{
					iTargetAngle = g_stCV.RobotLeftAngleValue;
					MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
				}
				else if (g_stPV.RunMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
				{
					iTargetAngle = g_stCV.RobotRightAngleValue;
					MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
				}
				break;
			}
			else if (g_stMK32.CH4_SA == -1000)
			{
				if (g_stPV.RunMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
				{
					iTargetAngle = g_stCV.RobotRightAngleValue;
					MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
				}
				else if (g_stPV.RunMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
				{
					iTargetAngle = g_stCV.RobotLeftAngleValue;
					MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
				}
				break;
			}
			else // 重置前进计时器以便下一次换道重新计数
			{
				if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
				{
					MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_RESET_STRAIGHT, NULL, 0);
				}
			}

			if (0 == g_angle_protect_lock)
			{
				// 开关喷枪
				if (g_stPV.RunMode == 1)
				{
					if (g_stMK32.CH6_SC == -1000)
					{
						if (0 == iIsStopOffPaint)
						{
							g_Paint_State = 0;
							MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_Paint_State, sizeof(int));
						}
					}
					else
					{
						iIsStopOffPaint = 0;
						g_Paint_State = 1;
						MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_Paint_State, sizeof(int));
					}
				}
				else if(g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
				{
					static int CH13_S2_Value = 0;
					if (g_stMK32.CH13_S2 != CH13_S2_Value)
					{
						if(g_stIV.CurrentSpeed != 0)
						{
							g_Paint_State = 0;
							MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_Paint_State, sizeof(int));
						}
					}
					CH13_S2_Value = g_stMK32.CH13_S2;
				}
			}

			// 自动巡航
			if (g_stMK32.CH5_SB == -1000)
			{
				if (g_stPV.RunMode == 1)
				{
					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0);
				}
				else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
				{
					if (0 == g_angle_protect_lock)
					{
						BHBF_straight_drive_Cmd stCmd = {
							.m_iMode = 1,
							.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
							.m_iTime = -1,
							.m_iSpeed = iVehicleSpeed
							};
						MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));						
					}
				}
				break;
			}
			else if(g_stMK32.CH5_SB == 1000)
			{
				if (g_stPV.RunMode == 1)
				{
					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0);
				}
				else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
				{
					if (0 == g_angle_protect_lock)
					{
						BHBF_straight_drive_Cmd stCmd = {
							.m_iMode = -1,
							.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
							.m_iTime = -1,
							.m_iSpeed = iVehicleSpeed
							};
						MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
					}
				}
				break;
			}

			// 【注意!!!】摇杆行程死区判断开始，除摇杆以外按键在上边都处理完，不然走不下去
			if (abs(g_stMK32.CH2_LY_V) <= g_stCV.Joy_Sticker_Value_Allowance && abs(g_stMK32.CH3_LY_H) <= g_stCV.Joy_Sticker_Value_Allowance)
			{
				if ((g_stPV.RunMode == 2 || g_stPV.RunMode == 3) && 0 == iPaint)
				{
					MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_STOP_AFTER, NULL, 0);
					log_e("MOTOR_CMD_STOP_AFTER");
				}
				else
				{
					MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0);
				}
				break;
			}
			// 【注意!!!】摇杆行程死区判断结束，摇杆处理都放在这以后，否则行程死区可能不生效

			// 摇杆角度死区判断
			int angle = atan2(g_stMK32.CH2_LY_V, g_stMK32.CH3_LY_H) * 180 / M_PI;

			if (abs(angle - 90) <= g_stCV.Joy_Sticker_Angle_Allowance)
			{
				if (g_stPV.RunMode == 1)
				{
					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0);
				}
				else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
				{
					if (0 == g_angle_protect_lock)
					{
						BHBF_straight_drive_Cmd stCmd = {
							.m_iMode = 1,
							.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
							.m_iTime = -1,
							.m_iSpeed = iVehicleSpeed
							};
						MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
					}
				}
			}
			else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance)
			{
				if (g_stPV.RunMode == 1)
				{
					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0);
				}
				else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
				{
					if (0 == g_angle_protect_lock)
					{
						BHBF_straight_drive_Cmd stCmd = {
							.m_iMode = -1,
							.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
							.m_iTime = -1,
							.m_iSpeed = iVehicleSpeed
							};
						MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
					}
				}
			}
			else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance)
			{
				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNRIGHT, NULL, 0);
			}
			else if (abs(angle - 180) <= g_stCV.Joy_Sticker_Angle_Allowance || abs(angle - (-180)) <= g_stCV.Joy_Sticker_Angle_Allowance)
			{
				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNLEFT, NULL, 0);
			}
			else
			{
				MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0);
			}
			break;
		}
		case COMMON_CMD_SHOW_INFO:
		{
			lua_print("MK32 Info\nRxIndex:%d\tIsOnline:%d\n", g_stMK32.RxIndex, g_stMK32.IsOnline);
			lua_print("CH0_RY_H\t%d\n", g_stMK32.CH0_RY_H);
			lua_print("CH1_RY_V\t%d\n", g_stMK32.CH1_RY_V);
			lua_print("CH2_LY_V\t%d\n", g_stMK32.CH2_LY_V);
			lua_print("CH3_LY_H\t%d\n", g_stMK32.CH3_LY_H);
			lua_print("CH4_SA  \t%d\n", g_stMK32.CH4_SA);
			lua_print("CH5_SB  \t%d\n", g_stMK32.CH5_SB);
			lua_print("CH6_SC  \t%d\n", g_stMK32.CH6_SC);
			lua_print("CH7_SD  \t%d\n", g_stMK32.CH7_SD);
			lua_print("CH8_SE  \t%d\n", g_stMK32.CH8_SE);
			lua_print("CH9_SF  \t%d\n", g_stMK32.CH9_SF);
			lua_print("CH10_LD1\t%d\n", g_stMK32.CH10_LD1);
			lua_print("CH11_RD1\t%d\n", g_stMK32.CH11_RD1);
			lua_print("CH12_S1 \t%d\n", g_stMK32.CH12_S1);
			lua_print("CH13_S2 \t%d\n", g_stMK32.CH13_S2);
			lua_print("CH14_LT \t%d\n", g_stMK32.CH14_LT);
			lua_print("CH15_RT \t%d\n", g_stMK32.CH15_RT);
			lua_print("PV Info\n");
			lua_print("{%d, %d, %d, %d, %d, %ld}\n",
					g_stPV.RunMode, g_stPV.RobotSpeed, g_stPV.LaneChangeDistance, (int)g_stPV.Vertical_Calibration, g_stPV.IV_IsRestart_Notified, (long long)g_stPV.TimeStamp);
			lua_print("\niPaint = %d\ng_Paint_State = %d\ng_angle_protect_lock = %d\ng_ipaintOffCount = %d\ng_Is_All_Button_Reset = %d\n",
					iPaint, g_Paint_State, g_angle_protect_lock, g_ipaintOffCount, g_Is_All_Button_Reset);
			break;
		}
        default:
            break;
    }
}

void Custom_Task(void *argument)
{
	g_uiCustomModuleID = MsgCenter_Register(MODULE_NAME_CUSTOM, Custom_ModuleHandler);
    uint32_t uiLastSendTick = Rd_GetTime();
    while(1)
    {
        MsgCenter_ProcessWait(g_uiCustomModuleID, 2);

        uint32_t uiNowTick = Rd_GetTime();
        if ((int32_t)(uiNowTick - uiLastSendTick) >= IV_SEND_TIME)
        {
            uiLastSendTick = uiNowTick;
            g_stIV.SBUS_State = g_stMK32.IsOnline;
            MsgCenter_SendTo(MODULE_NAME_SENDIV, SENDIV_SET_IV, (void *)&g_stIV, sizeof(g_stIV));
        }
    }
}