#include "TopDefine.h"
#include "FreeRTOS.h"
#include "userTask.h"
#include <cmsis_os2.h>
#include "gimbalTask.hpp"
#include "gimbal.hpp"
#include "main.h"
#include "remote_control.h"
#include "nuc.h"
Gimbal gimbal;
NUC_t nucData; // 用于存储从队列接收的数据
extern RC_ctrl_t rc_ctrl;

void FunctionGimbal(void *argument)
{
	(void)argument; /* 未使用argument，消除警告 */

    const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_GIMBAL;
	
	HAL_Delay(500);
	gimbal.gimbalInit();
	gimbal.gimbalZero();
	HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET);

	uint32_t tick = osKernelGetTickCount();

	while(1)
	{
   #ifdef DEBUG
		task_struct.stack_water_mark.gimbal = osThreadGetStackSpace(osThreadGetId());  
   #endif 		
		// 从消息队列接收视觉数据
		if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
		{
			// 使用接收到的视觉数据调整云台
			gimbal.gimbalVision(nucData);
		}

			gimbal.gimbalZero();
			gimbal.gimbalFlow();
		osDelay(1);

		tick += delay_tick; /* 计算下一个唤醒时刻 */
        osDelayUntil(tick);
	}
}