S3315_RK3588/NeoTracker/src/Arith_GetCommand.cpp

#include "Arith_EOController.h"
#include "Arith_CoordModule.h"
#include "Arith_ImgOperate.h"
#include "opencv2/opencv.hpp"
////////////////////////////////////////////////////////////////////////////////
//--函数定义
////////////////////////////////////////////////////////////////////////////////
/**********************************************************
* 函数名称：CMD_GetCommand()
* 功能描述：获取并响应任务机命令
* 输入参数：无
* 输出参数：无
* 返 回 值：无
* 调用关系：无
* 其它说明：无
**********************************************************/
void Arith_EOController::CMD_GetCommand(void)
{
	//系统初始化
	if (GLB_CMD_SYSINITIAL == g_GLB_stCommand.ubCmd)
	{
		g_GLB_bInitialize = FALSE;//初始化状态标记为：未初始化
	}
	else
	{
		g_GLB_bInitialize = TRUE;//初始化状态标记为：已初始化
	}

	//g_GLB_stPara.bHasRrObj = g_GLB_stCommand.unUpdateRrObj;
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//记录最后一次指令
	if (LockMode::LOCK_NONE != g_GLB_stCommand.ubCmd)
	{
		g_GLB_stCommand.ubCmdLast = g_GLB_stCommand.ubCmd;
	}	

	// 记录输出当前帧指令
	memcpy(&g_GLB_stCommand_Pre, &g_GLB_stCommand, sizeof(GLB_PCCOMMAND));

}

/**********************************************************
* 函数名称：CMD_RespondCommand()
* 功能描述：响应控制指令
* 输入参数：SINT32 nWidth   --  图像宽度
*           SINT32 nHeight  --  图像高度
* 输出参数：无
* 返 回 值：无
* 调用关系：无
* 其它说明：无
**********************************************************/
void Arith_EOController::CMD_RespondCommand(void)
{
	//// 场景模式切换
	//switch (g_GLB_stCommand.ubMutiTrack)
	//{
	//case 1:
	//	g_GLB_stPara.bMutiTrack = false;
	//	break;
	//case 2:
	//	g_GLB_stPara.bMutiTrack = true;
	//	break;
	//default:
	//	break;
	//}
	//++++++++++++++1.响应系统工作模式指令+++++++++++++++++++
	if (g_GLB_stCommand.ubSysStatusCmd != g_GLB_stPara.nSysMode)
	{
		switch (g_GLB_stCommand.ubSysStatusCmd)
		{
			// 空闲模式
		case GLB_SYS_WAIT:
			g_GLB_stPara.nSysMode = GLB_SYS_WAIT;
			g_GLB_stPara.nStatus = GLB_STATUS_WAIT;
			//清空算法输出
			GLB_InitOutputs();
			break;

			// 扫描模式
		case GLB_SYS_FSCAN:
			g_GLB_stPara.nSysMode = GLB_SYS_FSCAN;
			// 强制切换为搜索
			g_GLB_stPara.nStatus = GLB_STATUS_FSCAN;
			//清空算法输出
			GLB_InitOutputs();
			break;

			// 无人值守模式（演示样机状态）
		case GLB_SYS_AUTO:
			g_GLB_stPara.nSysMode = GLB_SYS_AUTO;
			// 已在跟踪状态则忽略
			if (g_GLB_stPara.nStatus != GLB_STATUS_TRACK)
			{
				g_GLB_stPara.nStatus = GLB_STATUS_SEARCH;
				//清空算法输出
				GLB_InitOutputs();
			}
			break;
		case GLB_SYS_STARE:
			g_GLB_stPara.nSysMode = GLB_SYS_STARE;

			// 强制切换为搜索
			g_GLB_stPara.nStatus = GLB_STATUS_SEARCH;
			//清空算法输出
			GLB_InitOutputs();

			break;
		
		case GLB_SYS_SCAN:
			g_GLB_stPara.nSysMode = GLB_SYS_SCAN;
			// 强制切换为搜索
			g_GLB_stPara.nStatus = GLB_STATUS_SCAN;
			//清空算法输出
			GLB_InitOutputs();
			break;

		default:
			break;
		}

	}

	// 场景模式切换
	if (g_GLB_stCommand.ubScenCmd != g_GLB_stPara.nWorkScene && g_GLB_stCommand.ubScenCmd!=GLB_SCEN_NONE)
	{
		g_GLB_stPara.nWorkScene = g_GLB_stCommand.ubScenCmd;

		m_SceneType = g_GLB_stCommand.ubScenCmd;

		//清空算法输出
		GLB_InitOutputs();
		// 清空锁定指令
		g_GLB_stPara.stLockCtrl = LOCK_NONE;
		g_GLB_stPara.bEnForceGuide = false;
		// 如果在跟踪模式，直接切回搜索
		// 否则当前帧的跟踪信息已经清空了无法正常到下一帧
		if (g_GLB_stPara.nStatus == GLB_STATUS_TRACK ||
		g_GLB_stPara.nStatus == GLB_STATUS_MOTRACK)
		{
			g_GLB_stPara.nStatus = GLB_STATUS_SEARCH;
		}

	}

	//++++++++++++++2.响应跟踪器工作状态指令+++++++++++++++++++
	//锁定/解锁：01目标锁定/修正攻击点，02目标解锁。
	switch (g_GLB_stCommand.ubCmd)
	{
		//目标锁定/修正攻击点
	case LockMode::LOCK_POINT:
		g_GLB_stPara.bEnForceGuide = TRUE;
		g_GLB_stCommand.bGuideUpDate = TRUE;
		CMD_OnTrack(LockMode::LOCK_POINT);
		break;
		// 框选（强制）
	case LockMode::LOCK_RECT:
		CMD_OnTrack(LockMode::LOCK_RECT);
		break;
		//目标解锁
	case LockMode::LOCK_UNLOCK:
		CMD_OnCancelTrack();
		g_GLB_stPara.bEnForceGuide = FALSE;
		break;

	//	引导 ON
	// case GLB_CMD_GUIDE_ON:
	// 	g_GLB_stPara.bEnRadarGuide = TRUE;
	// 	break;

	// //	引导 OFF
	// case GLB_CMD_GUIDE_OFF:
	// 	g_GLB_stPara.bEnRadarGuide = FALSE;
	// 	break;

		//其它
	default:

		// 
		break;
	}

	// 外引导多目标处理
	if (g_GLB_stPara.bEnRadarGuide)
	{
		// 在导引有效期内暂时切换为自动锁定
		g_GLB_stPara.bSelectObjManual = false;

		// 引导更新处理
		if (g_GLB_stCommand.bGuideUpDate)
		{
			g_GLB_stPara.bGuideUpDate = true;
			g_GLB_stPara.nGuideLastTime = 500;  //导引信息有效期，在此期间会不断尝试自动锁定导引区域目标

			// 引导更新自动下发 LockMode::LOCK_AUTO 锁定模式
			g_GLB_stPara.stLockCtrl = LockMode::LOCK_AUTO;

			// 读取外部引导的多目标
			memcpy(g_GLB_stPara.targetGuide, g_GLB_stCommand.target, sizeof(TargetGuide)* GUIDE_TARGET_NUM);
		}
		else
		{
			g_GLB_stPara.bGuideUpDate = false;
			g_GLB_stPara.nGuideLastTime--;
		}
		if (g_GLB_stPara.nGuideLastTime < 0)
		{
			//清空目标检测结果


			//关闭导引
			g_GLB_stPara.bEnRadarGuide = false;

			// 关闭自动锁定
			g_GLB_stPara.bSelectObjManual = true;

			// 自动锁定清除锁定标记
			if (g_GLB_stPara.stLockCtrl == LockMode::LOCK_AUTO)
			{
				g_GLB_stPara.stLockCtrl = LockMode::LOCK_NONE;
			}
			
		}


		// 接受导引期间收到锁定指令，直接响应锁定，不再等待自动锁定，否则显得很呆//           by wcw04046 @ 2021/11/25
		if (g_GLB_stCommand.unSetLockPosX != 0 && g_GLB_stCommand.unSetLockPosY != 0)
		{
			g_GLB_stPara.bSelectObjManual = true;
		}

		// 更新引导信息在当前帧图像信息
		for (size_t i = 0; i < GUIDE_TARGET_NUM; i++)
		{
			TargetGuideInner* pGuideInfo = &g_GLB_stPara.targetGuide[i];

			// 批号0为无效目标
			if (pGuideInfo->ID == 0)
			{
				memset(pGuideInfo, 0, sizeof(TargetGuide));//清除目标
				continue;
			}

			// 稳定系到图像坐标系
			POINT32F imgPos = getImagePosFromStablePole(pGuideInfo->stTargetPole, g_GLB_stInput.stCamera, g_GLB_stInput.servoInfo, g_GLB_stInput.afPlatformRPY, g_GLB_stInput.setupErr);
			pGuideInfo->ptGuidePOS.x = (int)imgPos.x;
			pGuideInfo->ptGuidePOS.y = (int)imgPos.y;

			////判断雷达引导位置是否在视场内
			if (!IMGO_IsPoint32FOutImg(g_GLB_stInput.nImageWidth, g_GLB_stInput.nImageHeight, imgPos))
			{
				pGuideInfo->bGuidePosInFOV = true;
			}
			else
			{
				pGuideInfo->bGuidePosInFOV = false;
			}

		}

	}
	else
	{
		// 关闭自动锁定
		g_GLB_stPara.bSelectObjManual = true;
	}

	// 强制提取
	if (g_GLB_stPara.bEnForceGuide)
	{
		// 引导更新处理
		if (g_GLB_stCommand.bGuideUpDate)
		{
			g_GLB_stPara.bGuideUpDate = true;
			g_GLB_stPara.nGuideLastTime = 3 * GLB_FRM_FREQ;  //半自动截获有效期--3s
			g_GLB_stPara.stLockCtrl = LockMode::LOCK_POINT;
		} else
		{
			g_GLB_stPara.bGuideUpDate = false;
			g_GLB_stPara.stLockCtrl = LockMode::LOCK_POINT;
			g_GLB_stPara.nGuideLastTime--;
		}
		if (g_GLB_stPara.nGuideLastTime < 0)
		{
			//关闭导引
			g_GLB_stPara.bEnForceGuide = false;
			// 清除锁定标记
			if (g_GLB_stPara.stLockCtrl = LockMode::LOCK_POINT)
			{
				g_GLB_stPara.stLockCtrl = LockMode::LOCK_NONE;
			}
		}
		//g_GLB_stPara.ubLockID = g_GLB_stCommand.ubSetID;
		////实际锁定点坐标（实际锁定点X坐标、实际锁定点Y坐标）
		//g_GLB_stPara.ptLockPos.x = g_GLB_stCommand.unSetLockPosX;
		//g_GLB_stPara.ptLockPos.y = g_GLB_stCommand.unSetLockPosY;
		////锁定波门尺寸（宽度、高度）
		//g_GLB_stPara.snLockBoxSize.w = g_GLB_stCommand.unSetLockBoxW;
		//g_GLB_stPara.snLockBoxSize.h = g_GLB_stCommand.unSetLockBoxH;
		//g_GLB_stPara.snLockBoxSize.s = g_GLB_stCommand.unSetLockBoxW * g_GLB_stCommand.unSetLockBoxH;
	}

	// 指令响应完成后弹起
	CMD_CleanUpPCCommand();
}

/**********************************************************
* 函数名称：CMD_CleanUpPCCommand()
* 功能描述：初始化PC机到跟踪板的控制命令
* 输入参数：无
* 输出参数：无
* 返 回 值：无
* 调用关系：无
* 其它说明：无
**********************************************************/
void Arith_EOController::CMD_CleanUpPCCommand(void)
{
	memset(&g_GLB_stCommand, 0, sizeof(GLB_PCCOMMAND));
}


/**********************************************************
* 函数名称：CMD_OnTrack()
* 功能描述：按下锁定键: 开始跟踪，或修正跟踪
* 输入参数：是否需要强制锁定
* 输出参数：无
* 返 回 值：无
* 调用关系：无
* 其它说明：无
**********************************************************/
void Arith_EOController::CMD_OnTrack(LockMode mode)
{
	g_GLB_stPara.stLockCtrl = mode;

	g_GLB_stPara.ubLockID = g_GLB_stCommand.ubSetID;

	//实际锁定点坐标（实际锁定点X坐标、实际锁定点Y坐标）
	g_GLB_stPara.ptLockPos.x = g_GLB_stCommand.unSetLockPosX;
	g_GLB_stPara.ptLockPos.y = g_GLB_stCommand.unSetLockPosY;
	//锁定波门尺寸（宽度、高度）
	/*g_GLB_stPara.snLockBoxSize.w = g_GLB_stCommand.unSetLockBoxW;
	g_GLB_stPara.snLockBoxSize.h = g_GLB_stCommand.unSetLockBoxH;
	g_GLB_stPara.snLockBoxSize.s = g_GLB_stCommand.unSetLockBoxW * g_GLB_stCommand.unSetLockBoxH;*/
}


/**********************************************************
* 函数名称：CMD_OnCancelTrack()
* 功能描述：按下解锁键: 停止跟踪
* 输入参数：无
* 输出参数：无
* 返 回 值：无
* 调用关系：无
* 其它说明：无
**********************************************************/
void Arith_EOController::CMD_OnCancelTrack(void)
{
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//置解锁标志为真
	g_GLB_stPara.stLockCtrl = LOCK_NONE;

	// 释放所有跟踪器
	//GLB_Release_Trackers(g_GLB_stPara.nPipeMaxNum);
	LOG_DEBUG("CMD_OnCancelTrack:nStatus:{},unSetCancleMode:{}", g_GLB_stPara.nStatus, g_GLB_stCommand.unSetCancleMode);
	if (0 == g_GLB_stCommand.unSetCancleMode)
	{// 解锁对应点跟踪器
		POINT32S ptCanclePos = { g_GLB_stCommand.unSetCanclePosX ,g_GLB_stCommand.unSetCanclePosY };
		GLB_Cancle_Point(g_GLB_stPara.nPipeMaxNum,ptCanclePos);
		g_GLB_stPara.bEnRadarGuide = false;
	}
	if (1 == g_GLB_stCommand.unSetCancleMode)
	{
		// 释放所有跟踪器
		GLB_Release_Trackers(g_GLB_stPara.nPipeMaxNum);
		//清空算法输出结果
		//memset(&g_GLB_stOutput, 0, sizeof(GLB_OUTPUT));
	}
	//跟踪状态下，当接收到解锁指令后，下一帧跟踪器返回待命状态
	if (GLB_STATUS_TRACK == g_GLB_stPara.nStatus || GLB_STATUS_LOST == g_GLB_stPara.nStatus || (GLB_STATUS_MOTRACK == g_GLB_stPara.nStatus && 1 == g_GLB_stCommand.unSetCancleMode))
	{
		g_GLB_stPara.nStatus = GLB_STATUS_SEARCH;
	} else
	{
		return;  //非跟踪状态不响应
	}
	//清空导引信息
	g_GLB_stPara.bEnRadarGuide = false;
	memset(g_GLB_stPara.targetGuide, 0, sizeof(TargetGuide) * GUIDE_TARGET_NUM);


	//清空算法输出结果
	memset(&g_GLB_stOutput, 0, sizeof(GLB_OUTPUT));
}