#include "ti_msp_dl_config.h"
#include "systick.h"
#include "string.h"
#include "bsp_eeprom.h"
#include "w25qxx.h"
#include "nclink.h"
#include "sensor.h"
#include "user_ctrl.h"
#include "pid.h"
#include "drv_expand.h"
#include "subtask_demo.h"

uint16_t flight_subtask_cnt[FLIGHT_SUBTASK_NUM]={0};//飞行任务子线程计数器，可以用于控制每个航点子线程的执行
uint32_t flight_global_cnt[FLIGHT_SUBTASK_NUM]={0}; //飞行任务子线全局计数器，可以结合位置偏差用于判断判断航点是否到达
uint32_t execute_time_ms[FLIGHT_SUBTASK_NUM]={0};		//飞行任务子线执行时间，可以用于设置某个子线程的执行时间
uint32_t flight_global_cnt2[FLIGHT_SUBTASK_NUM]={0};//飞行任务子线全局计数器2
#define flight_subtask_delta 5//5ms
vector3f base_position;//用于记录导航基准原点位置

void Reserved_Params_Init(void)
{
  float _param_value[RESERVED_PARAM_NUM];  
  #if eeprom_mode==0
  EEPROMRead((uint32_t *)(_param_value), RESERVED_PARAM * 4, RESERVED_PARAM_NUM);
  #elif eeprom_mode==1
  EEPROMRead_f((float *)(_param_value), RESERVED_PARAM * 4, RESERVED_PARAM_NUM);
	#else
	W25QXX_Read_f((float *)(_param_value), RESERVED_PARAM * 4, RESERVED_PARAM_NUM);
  #endif
  for(uint16_t i = 0; i < RESERVED_PARAM_NUM; i++)
  {
    if(isnan(_param_value[i]) == 0) param_value[i] = _param_value[i];
    else param_value[i] = 0;
  }
}



void flight_subtask_reset(void)
{
  for(uint16_t i = 0; i < FLIGHT_SUBTASK_NUM; i++)
  {
    flight_subtask_cnt[i] = 0;
    execute_time_ms[i] = 0;
    flight_global_cnt[i] = 0;
    flight_global_cnt2[i] = 0;
  }
}


//基本飞行保障必备子函数——定高、定点
void basic_auto_flight_support(void)
{
	Horizontal_Control_VIO(0);//SLAM定点控制
	Flight.yaw_ctrl_mode=ROTATE;//偏航控制为手动模式
	Flight.yaw_outer_control_output  =RC_YAW;
	Flight_Alt_Hold_Control(ALTHOLD_MANUAL_CTRL,NUL,NUL);//高度控制
}


/***********************************************************************************************************/
//顺时针转动90度，完成后降落
void flight_subtask_1(void)
{
	static uint8_t n=0;
	if(flight_subtask_cnt[n]==0)
	{
		Flight.yaw_ctrl_mode=CLOCKWISE;
		Flight.yaw_ctrl_start=1;
		Flight.yaw_outer_control_output  =90;//顺时针90度	
		Horizontal_Control_VIO(0);	
		Flight_Alt_Hold_Control(ALTHOLD_MANUAL_CTRL,NUL,NUL);//高度控制
		//偏航控制命令响应后才进行下一阶段任务
		if(Flight.yaw_ctrl_response==1)
		{
			flight_subtask_cnt[n]++;
			Flight.yaw_ctrl_start=0;
			Flight.yaw_ctrl_response=0;		
		}
	}
	else if(flight_subtask_cnt[n]==1)
	{
		Flight.yaw_ctrl_mode=CLOCKWISE;
		Flight.yaw_outer_control_output  =0;
		Horizontal_Control_VIO(0);	
		Flight_Alt_Hold_Control(ALTHOLD_MANUAL_CTRL,NUL,NUL);//高度控制
		
		if(Flight.yaw_ctrl_end==1)  flight_subtask_cnt[n]=2;//执行完毕后，切换到下一阶段	
	}
	else if(flight_subtask_cnt[n]==2)
	{
		Flight.yaw_ctrl_mode=ROTATE;
		Flight.yaw_outer_control_output  =RC_YAW;
		Horizontal_Control_VIO(0);
		Flight_Alt_Hold_Control(ALTHOLD_AUTO_VEL_CTRL,NUL,-30);//高度控制
	}
	else
	{
		basic_auto_flight_support();//基本飞行支持软件
	}
}


/***********************************************************************************************************/
//逆时针转动90度，完成后降落
void flight_subtask_2(void)
{
	static uint8_t n=1;
	if(flight_subtask_cnt[n]==0)
	{
		Flight.yaw_ctrl_mode=ANTI_CLOCKWISE;
		Flight.yaw_ctrl_start=1;
		Flight.yaw_outer_control_output  =90;//逆时针90度	
		Horizontal_Control_VIO(0);	
		Flight_Alt_Hold_Control(ALTHOLD_MANUAL_CTRL,NUL,NUL);//高度控制
		//偏航控制命令响应后才进行下一阶段任务
		if(Flight.yaw_ctrl_response==1)
		{
			flight_subtask_cnt[n]++;
			Flight.yaw_ctrl_start=0;
			Flight.yaw_ctrl_response=0;		
		}
	}
	else if(flight_subtask_cnt[n]==1)
	{
		Flight.yaw_ctrl_mode=ANTI_CLOCKWISE;
		Flight.yaw_outer_control_output  =0;
		Horizontal_Control_VIO(0);	
		Flight_Alt_Hold_Control(ALTHOLD_MANUAL_CTRL,NUL,NUL);//高度控制		
		if(Flight.yaw_ctrl_end==1)  flight_subtask_cnt[n]=2;//执行完毕后，切换到下一阶段		
	}
	else if(flight_subtask_cnt[n]==2)
	{
		Flight.yaw_ctrl_mode=ROTATE;
		Flight.yaw_outer_control_output  =RC_YAW;
		Horizontal_Control_VIO(0);
		Flight_Alt_Hold_Control(ALTHOLD_AUTO_VEL_CTRL,NUL,-30);//高度控制
	}
	else
	{
		basic_auto_flight_support();//基本飞行支持软件
	}
}





/***********************************************************************************************************/
//以10deg/s的角速度顺时针转动10000ms，完成后降落
void flight_subtask_3(void)
{
	static uint8_t n=2;
	if(flight_subtask_cnt[n]==0)
	{		
		Flight.yaw_ctrl_mode=CLOCKWISE_TURN;
		Flight.yaw_ctrl_start=1;
		Flight.yaw_outer_control_output  =10;//以10deg/s的角速度顺时针转动10000ms
		Flight.execution_time_ms=10000;//执行时间
		Horizontal_Control_VIO(0);	
		Flight_Alt_Hold_Control(ALTHOLD_MANUAL_CTRL,NUL,NUL);//高度控制
		
		//偏航控制命令响应后才进行下一阶段任务
		if(Flight.yaw_ctrl_response==1)
		{
			flight_subtask_cnt[n]++;
			Flight.yaw_ctrl_start=0;
			Flight.yaw_ctrl_response=0;		
		}	
	}
	else if(flight_subtask_cnt[n]==1)
	{
		Flight.yaw_ctrl_mode=CLOCKWISE_TURN;
		Flight.yaw_outer_control_output  =0;
		Horizontal_Control_VIO(0);	
		Flight_Alt_Hold_Control(ALTHOLD_MANUAL_CTRL,NUL,NUL);//高度控制
		
		if(Flight.yaw_ctrl_end==1)  flight_subtask_cnt[n]=2;//执行完毕后，切换到下一阶段		
	}
	else if(flight_subtask_cnt[n]==2)
	{
		
		Flight.yaw_ctrl_mode=ROTATE;
		Flight.yaw_outer_control_output  =RC_YAW;

		Horizontal_Control_VIO(0);
		Flight_Alt_Hold_Control(ALTHOLD_AUTO_VEL_CTRL,NUL,-30);//高度控制
	}
	else
	{
		basic_auto_flight_support();//基本飞行支持软件
	}
}





/***********************************************************************************************************/
//以10deg/s的角速度逆时针转动10000ms，完成后降落
void flight_subtask_4(void)
{
	static uint8_t n=3;
	if(flight_subtask_cnt[n]==0)
	{	
		Flight.yaw_ctrl_mode=ANTI_CLOCKWISE_TURN;
		Flight.yaw_ctrl_start=1;
		Flight.yaw_outer_control_output  =10;//以10deg/s的角速度顺时针转动10000ms
		Flight.execution_time_ms=10000;//执行时间
		Horizontal_Control_VIO(0);	
		Flight_Alt_Hold_Control(ALTHOLD_MANUAL_CTRL,NUL,NUL);//高度控制
		//偏航控制命令响应后才进行下一阶段任务
		if(Flight.yaw_ctrl_response==1)
		{
			flight_subtask_cnt[n]++;
			Flight.yaw_ctrl_start=0;
			Flight.yaw_ctrl_response=0;		
		}	
	}
	else if(flight_subtask_cnt[n]==1)
	{
		Flight.yaw_ctrl_mode=ANTI_CLOCKWISE_TURN;
		Flight.yaw_outer_control_output  =0;
		Horizontal_Control_VIO(0);	
		Flight_Alt_Hold_Control(ALTHOLD_MANUAL_CTRL,NUL,NUL);//高度控制
		
		if(Flight.yaw_ctrl_end==1)  flight_subtask_cnt[n]=2;//执行完毕后，切换到下一阶段		
	}
	else if(flight_subtask_cnt[n]==2)
	{
		Flight.yaw_ctrl_mode=ROTATE;
		Flight.yaw_outer_control_output  =RC_YAW;

		Horizontal_Control_VIO(0);
		Flight_Alt_Hold_Control(ALTHOLD_AUTO_VEL_CTRL,NUL,-30);//高度控制
	}
	else
	{
		basic_auto_flight_support();//基本飞行支持软件
	}
}






//本demo适用于激光雷达SLAM定位条件下，普通光流（LC307、LC302）定位条件下无效
//机体坐标系下相对位移
//右前上分别对应XYZ正方向
void flight_subtask_5(void)
{		
	static uint8_t n=4;
	if(flight_subtask_cnt[n]==0)
	{
		basic_auto_flight_support();//基本飞行支持软件
		flight_subtask_cnt[n]=1;
		execute_time_ms[n]=10000/flight_subtask_delta;//子任务执行时间
		//向前移动100cm
		Horizontal_Navigation(0,100,0,RELATIVE_MODE,BODY_FRAME);
	}
	else if(flight_subtask_cnt[n]==1)
	{
		basic_auto_flight_support();//基本飞行支持软件
		if(execute_time_ms[n]>0) execute_time_ms[n]--;
		if(execute_time_ms[n]==0) 
		{
			flight_subtask_cnt[n]=2;
			execute_time_ms[n]=10000/flight_subtask_delta;//子任务执行时间
			//向右移动100cm
			Horizontal_Navigation(100,0,0,RELATIVE_MODE,BODY_FRAME);
		}
	}
	else if(flight_subtask_cnt[n]==2)
	{
		basic_auto_flight_support();//基本飞行支持软件
		if(execute_time_ms[n]>0) execute_time_ms[n]--;
		if(execute_time_ms[n]==0) 
		{
			flight_subtask_cnt[n]=3;
			execute_time_ms[n]=10000/flight_subtask_delta;//子任务执行时间
			//向后移动100cm
			Horizontal_Navigation(0,-100,0,RELATIVE_MODE,BODY_FRAME);
		}
	}
	else if(flight_subtask_cnt[n]==3)
	{
		basic_auto_flight_support();//基本飞行支持软件
		if(execute_time_ms[n]>0) execute_time_ms[n]--;
		if(execute_time_ms[n]==0) 
		{
			flight_subtask_cnt[n]=4;
			execute_time_ms[n]=10000/flight_subtask_delta;//子任务执行时间
			//向左移动100cm
			Horizontal_Navigation(-100,0,0,RELATIVE_MODE,BODY_FRAME);
		}
	}
	else if(flight_subtask_cnt[n]==4)
	{
		basic_auto_flight_support();//基本飞行支持软件
		if(execute_time_ms[n]>0) execute_time_ms[n]--;
		if(execute_time_ms[n]==0) 
		{
			flight_subtask_cnt[n]=5;
			execute_time_ms[n]=10000/flight_subtask_delta;//子任务执行时间
		  
			//以下还可以继续增加子任务，比如向上移动50cm
			//Horizontal_Navigation(0,0,50,RELATIVE_FRAME,MAP_FRAME);
		}
	}
	else
	{
		basic_auto_flight_support();//基本飞行支持软件
	}
}

//导航坐标系下相对位移
//东北天分别对应XYZ正方向
void flight_subtask_6(void)
{		
	static uint8_t n=5;
	if(flight_subtask_cnt[n]==0)
	{
		basic_auto_flight_support();//基本飞行支持软件
		flight_subtask_cnt[n]=1;
		execute_time_ms[n]=10000/flight_subtask_delta;//子任务执行时间
		//向前移动100cm
		Horizontal_Navigation(0,100,0,RELATIVE_MODE,MAP_FRAME);
	}
	else if(flight_subtask_cnt[n]==1)
	{
		basic_auto_flight_support();//基本飞行支持软件
		if(execute_time_ms[n]>0) execute_time_ms[n]--;
		if(execute_time_ms[n]==0) 
		{
			flight_subtask_cnt[n]=2;
			execute_time_ms[n]=10000/flight_subtask_delta;//子任务执行时间
			//向右移动100cm
			Horizontal_Navigation(100,0,0,RELATIVE_MODE,MAP_FRAME);
		}
	}
	else if(flight_subtask_cnt[n]==2)
	{
		basic_auto_flight_support();//基本飞行支持软件
		if(execute_time_ms[n]>0) execute_time_ms[n]--;
		if(execute_time_ms[n]==0) 
		{
			flight_subtask_cnt[n]=3;
			execute_time_ms[n]=10000/flight_subtask_delta;//子任务执行时间
			//向后移动100cm
			Horizontal_Navigation(0,-100,0,RELATIVE_MODE,MAP_FRAME);
		}
	}
	else if(flight_subtask_cnt[n]==3)
	{
		basic_auto_flight_support();//基本飞行支持软件
		if(execute_time_ms[n]>0) execute_time_ms[n]--;
		if(execute_time_ms[n]==0) 
		{
			flight_subtask_cnt[n]=4;
			execute_time_ms[n]=10000/flight_subtask_delta;//子任务执行时间
			//向左移动100cm
			Horizontal_Navigation(-100,0,0,RELATIVE_MODE,MAP_FRAME);
		}
	}
	else if(flight_subtask_cnt[n]==4)
	{
		basic_auto_flight_support();//基本飞行支持软件
		if(execute_time_ms[n]>0) execute_time_ms[n]--;
		if(execute_time_ms[n]==0) 
		{
			flight_subtask_cnt[n]=5;
			execute_time_ms[n]=10000/flight_subtask_delta;//子任务执行时间
		  
			//以下还可以继续增加子任务，比如向上移动50cm
			//Horizontal_Navigation(0,0,50,RELATIVE_FRAME,MAP_FRAME);
		}
	}
	else
	{
		basic_auto_flight_support();//基本飞行支持软件
	}
}


//导航坐标系下，基于初始点的绝对坐标位移
void flight_subtask_7(void)
{		
	static uint8_t n=6;
	vector3f target_position;
	float x=0,y=0,z=0;
	if(flight_subtask_cnt[n]==0)
	{
		basic_auto_flight_support();//基本飞行支持软件
		//记录下初始起点位置，实际项目中可设置为某一基准原点
		base_position.x=uav_ins.position[_EAST];
		base_position.y=uav_ins.position[_NORTH];
		base_position.z=uav_ins.position[_UP];
		
		flight_subtask_cnt[n]=1;
		execute_time_ms[n]=10000/flight_subtask_delta;//子任务执行时间
		//向前移动100cm
		x=0;y=100;z=0;
		target_position.x=base_position.x+x;
		target_position.y=base_position.y+y;
		target_position.z=base_position.z+z;
		Horizontal_Navigation(target_position.x,
												  target_position.y,
												  target_position.z,
												  GLOBAL_MODE,
												  MAP_FRAME);
	}
	else if(flight_subtask_cnt[n]==1)
	{
		basic_auto_flight_support();//基本飞行支持软件
		if(execute_time_ms[n]>0) execute_time_ms[n]--;
		if(execute_time_ms[n]==0) 
		{
			flight_subtask_cnt[n]=2;
			execute_time_ms[n]=10000/flight_subtask_delta;//子任务执行时间
			//向右移动100cm
			x=100;y=100;z=0;
			target_position.x=base_position.x+x;
			target_position.y=base_position.y+y;
			target_position.z=base_position.z+z;
			Horizontal_Navigation(target_position.x,
														target_position.y,
														target_position.z,
														GLOBAL_MODE,
														MAP_FRAME);
		}
	}
	else if(flight_subtask_cnt[n]==2)
	{
		basic_auto_flight_support();//基本飞行支持软件
		if(execute_time_ms[n]>0) execute_time_ms[n]--;
		if(execute_time_ms[n]==0) 
		{
			flight_subtask_cnt[n]=3;
			execute_time_ms[n]=10000/flight_subtask_delta;//子任务执行时间
			//向后移动100cm
			x=100;y=0;z=0;
			target_position.x=base_position.x+x;
			target_position.y=base_position.y+y;
			target_position.z=base_position.z+z;
			Horizontal_Navigation(target_position.x,
														target_position.y,
														target_position.z,
														GLOBAL_MODE,
														MAP_FRAME);
		}
	}
	else if(flight_subtask_cnt[n]==3)
	{
		basic_auto_flight_support();//基本飞行支持软件
		if(execute_time_ms[n]>0) execute_time_ms[n]--;
		if(execute_time_ms[n]==0) 
		{
			flight_subtask_cnt[n]=4;
			execute_time_ms[n]=10000/flight_subtask_delta;//子任务执行时间
			//向左移动100cm
			x=0;y=0;z=0;
			target_position.x=base_position.x+x;
			target_position.y=base_position.y+y;
			target_position.z=base_position.z+z;
			Horizontal_Navigation(target_position.x,
														target_position.y,
														target_position.z,
														GLOBAL_MODE,
														MAP_FRAME);
		}
	}
	else if(flight_subtask_cnt[n]==4)
	{
		basic_auto_flight_support();//基本飞行支持软件
		if(execute_time_ms[n]>0) execute_time_ms[n]--;
		if(execute_time_ms[n]==0) 
		{
			flight_subtask_cnt[n]=5;
			execute_time_ms[n]=10000/flight_subtask_delta;//子任务执行时间
		  
			//以下还可以继续增加子任务，比如向上移动50cm
			//Horizontal_Navigation(0,0,50,RELATIVE_FRAME,MAP_FRAME);
		}
	}
	else
	{
		basic_auto_flight_support();//基本飞行支持软件
	}
}


//轨迹圆
#define Waypoint_Radius_CM     50 									//轨迹点半径，单位为cm   100
#define Waypoint_Num           60				 						//轨迹点数量						 60
//改变轨迹圆参数，一般情况下只需要调整半径Waypoint_Radius_CM即可
#define Waypoint_Angle_Delta   (360/Waypoint_Num)		//航点角度增量，单位为deg
#define Waypoint_Ideal_Speed	 20										//理想的巡航速度，用于推算航点发布时间，单位为cm/s
#define Waypoint_Time_Gap			 1000*(2*3.14*Waypoint_Radius_CM/Waypoint_Ideal_Speed)/Waypoint_Num		//航点发布时间间隔，单位为ms
float waypoint_x[Waypoint_Num+1]={0},waypoint_y[Waypoint_Num+1]={0};
void Circle_Waypoint_Generate(uint16_t num,float radius)
{
	for(uint16_t i=0;i<num+1;i++)
	{
		waypoint_x[i]=radius-radius*FastCos(Waypoint_Angle_Delta*i*DEG2RAD);
		waypoint_y[i]=radius*FastSin(Waypoint_Angle_Delta*i*DEG2RAD);
	}
}


void flight_subtask_8(void)
{		
	static uint8_t n=7;
	vector3f target_position;
	basic_auto_flight_support();//基本飞行支持软件
	if(flight_subtask_cnt[n]==0)
	{
		//Circle_Waypoint_Generate(Waypoint_Num,Waypoint_Radius_CM);
		//记录下初始起点位置，实际项目中可设置为某一基准原点
		base_position.x=uav_ins.position[_EAST];
		base_position.y=uav_ins.position[_NORTH];
		base_position.z=uav_ins.position[_UP];
		
		execute_time_ms[n]=Waypoint_Time_Gap/flight_subtask_delta;//子任务执行时间
		target_position.x=base_position.x+waypoint_x[0];
		target_position.y=base_position.y+waypoint_y[0];
		target_position.z=base_position.z;
		Horizontal_Navigation(target_position.x,
													target_position.y,
													target_position.z,
													GLOBAL_MODE,
													MAP_FRAME);
		flight_subtask_cnt[n]=1;
		laser_light_1.reset=1;
		laser_light_1.times=50000;//闪速1次
		laser_light_1.period=40;
		laser_light_1.light_on_percent=0.5;
	}
	else if(flight_subtask_cnt[n]<Waypoint_Num+1)
	{
		if(execute_time_ms[n]>0) execute_time_ms[n]--;
		if(execute_time_ms[n]==0) 
		{
			execute_time_ms[n]=Waypoint_Time_Gap/flight_subtask_delta;//子任务执行时间
			
			target_position.x=base_position.x+waypoint_x[flight_subtask_cnt[n]];
			target_position.y=base_position.y+waypoint_y[flight_subtask_cnt[n]];
			target_position.z=base_position.z;
			Horizontal_Navigation(target_position.x,
														target_position.y,
														target_position.z,
														GLOBAL_MODE,
														MAP_FRAME);
			flight_subtask_cnt[n]++;
		}
	}
	else//一直重复圆形轨迹，只运行一次可以注释掉下方赋值行
	{
		flight_subtask_cnt[n]=1;
	}
}



//自动起飞到某一高度
uint8_t Auto_Takeoff(float target)
{
	static uint8_t n=11;
	vector3f _target_position;
	basic_auto_flight_support();//基本飞行支持软件	
	if(flight_subtask_cnt[n]==0)
	{
		//不加此行代码，当后续全程无油门上下动作后，飞机最后自动降落到地面不会自动上锁
		
		//记录下初始起点位置，实际项目中可设置为某一基准原点
		base_position.x=uav_ins.position[_EAST];
		base_position.y=uav_ins.position[_NORTH];
		base_position.z=uav_ins.position[_UP];
		
		_target_position.x=base_position.x;
		_target_position.y=base_position.y;
		_target_position.z=base_position.z+target;
		Horizontal_Navigation(_target_position.x,
													_target_position.y,
													_target_position.z,
													GLOBAL_MODE,
													MAP_FRAME);
		flight_subtask_cnt[n]=1;
	  return	0;		
	}
	else if(flight_subtask_cnt[n]==1)
	{
		//判断是否起飞到目标高度
		if(flight_global_cnt[n]<400)//持续400*5ms满足
		{
			if(ABS(Total_Controller.Height_Position_Control.Err)<=10.0f)	flight_global_cnt[n]++;
			else flight_global_cnt[n]/=2;
			return	0;
		}
		else//持续200*5ms满足，表示到达目标高度
		{
			return	1;
		}
	}
	return	0;	
}


/********************************************************************************************************************/
#define nav_param_base (51-1)      //预留参数数组用于存放航点信息的起始量
#define user_setpoint_max 28   //最大航点数，可根据实际项目需要自己定义:显示屏航点设置页面为4页，每页有7个航点，共28个
#define user_setpoint_fixed_2d_cm  5.0f //5cm
#define user_setpoint_fixed_3d_cm  10.0f//10cm
#define user_setpoint_fixed_times  5//满足次数
int32_t nav_setpoint[user_setpoint_max][3]={0,0,0};//航点坐标数组 0x200066D8 
uint8_t user_setpoint_valid_flag[user_setpoint_max]={0};//航点有效标志位
void user_setpoint_generate(void)
{
	memset(user_setpoint_valid_flag,0,sizeof(char)*user_setpoint_max);
	for(uint16_t i=0;i<user_setpoint_max;i++)
	{
		if((param_value[nav_param_base+3*i+0]==0
		  &&param_value[nav_param_base+3*i+1]==0
		  &&param_value[nav_param_base+3*i+2]==0)!=1)//通过判断参数组内数据均非0，来判定航点数据是否有效
		{
			user_setpoint_valid_flag[i]=true;
			nav_setpoint[i][0]=param_value[nav_param_base+3*i+0];
			nav_setpoint[i][1]=param_value[nav_param_base+3*i+1];
			nav_setpoint[i][2]=param_value[nav_param_base+3*i+2];
		}
		else user_setpoint_valid_flag[i]=false;
	}
}


#define First_Working_Height 150
//用户通过按键自定义输入三维的航点位置，无人机依次遍历各个航点，最多支持28个航点
void Navigation_User_Setpoint(void)
{
	static uint8_t n=13;
	vector3f target_position;
	float x=0,y=0,z=0;	
	if(flight_subtask_cnt[n]==0)//起飞点作为第一个悬停点
	{
		basic_auto_flight_support();//基本飞行支持软件
		user_setpoint_generate();//生成航点
			
		//记录下初始起点位置，实际项目中可设置为某一基准原点
		//base_position.x=VIO_SINS.Position[_EAST];
		//base_position.y=VIO_SINS.Position[_NORTH];
		base_position.z=First_Working_Height;//第一作业高度
		
		x=base_position.x;
		y=base_position.y;
		z=First_Working_Height;
		target_position.x=x;
		target_position.y=y;
		target_position.z=z;
		Horizontal_Navigation(target_position.x,
													target_position.y,
													target_position.z,
													GLOBAL_MODE,
													MAP_FRAME);
		flight_subtask_cnt[n]=1;
		flight_global_cnt[n]=0;
		flight_global_cnt2[n]=0;
		execute_time_ms[n]=1000/flight_subtask_delta;//子任务执行时间
	}
	else if(flight_subtask_cnt[n]==1)//起飞之后原定悬停1S后再执行航点任务
	{
		basic_auto_flight_support();//基本飞行支持软件
		if(execute_time_ms[n]>0) execute_time_ms[n]--;
		if(execute_time_ms[n]==0)//悬停时间计数器归零，悬停任务执行完毕 
		{
			//判断所有航点均执行完毕，执行下一任务
			if(flight_global_cnt2[n]>=user_setpoint_max)
			{
				flight_subtask_cnt[n]=3;//结束航点遍历
				flight_global_cnt[n]=0;
				execute_time_ms[n]=0;
				//航点计数器清0
				flight_global_cnt2[n]=0;
				return ;				
			}
				
			uint16_t current_num=constrain_int32(flight_global_cnt2[n],0,user_setpoint_max-1);//限幅防溢出
			if(user_setpoint_valid_flag[current_num]==true)//如果当前的航点有效，就设置目标航点进入下一线程
			{
				x=nav_setpoint[current_num][0];
				y=nav_setpoint[current_num][1];
				z=nav_setpoint[current_num][2];
				target_position.x=base_position.x+x;//水平位置期望为起飞后基准位置+位置X偏移
				target_position.y=base_position.y+y;//水平位置期望为起飞后基准位置+位置Y偏移
				target_position.z=z;
				Horizontal_Navigation(target_position.x,
															target_position.y,
															target_position.z,
															GLOBAL_MODE,
															MAP_FRAME);
				flight_subtask_cnt[n]=2;
				flight_global_cnt[n]=0;
				execute_time_ms[n]=0;				
			}
			else//如果当前的航点无效，跳过当前航点,继续设置下一航点
			{
				//航点计数器自加
				flight_global_cnt2[n]++;			
			}
		}		
	}
	else if(flight_subtask_cnt[n]==2)//检测起飞点悬停完毕后，飞向下一个目标点
	{
		basic_auto_flight_support();//基本飞行支持软件		
		//判断是否到达目标航点位置
		if(flight_global_cnt[n]<user_setpoint_fixed_times)//持续10*5ms=0.05s满足
		{
			float dis_cm=pythagorous3(OpticalFlow_Pos_Ctrl_Err.x,OpticalFlow_Pos_Ctrl_Err.y,Total_Controller.Height_Position_Control.Err);
			if(dis_cm<=user_setpoint_fixed_3d_cm)	flight_global_cnt[n]++;
			else flight_global_cnt[n]/=2;
		}
		else//持续10*5ms满足，表示到达目标航点位置，后降低目标高度
		{
			flight_subtask_cnt[n]=1;
			flight_global_cnt[n]=0;
			execute_time_ms[n]=0;	
			//航点计数器自加
			flight_global_cnt2[n]++;
		}
		//判断所有航点均执行完毕，执行下一任务		
		if(flight_global_cnt2[n]>=user_setpoint_max)
		{
			flight_subtask_cnt[n]=3;
			flight_global_cnt[n]=0;
			execute_time_ms[n]=0;
			//航点计数器清0
			flight_global_cnt2[n]=0;			
		}
	}
	else if(flight_subtask_cnt[n]==3)//执行航点完毕后，原地下降，可以根据实际需要，自写更多的飞行任务
	{
		Flight.yaw_ctrl_mode=ROTATE;
		Flight.yaw_outer_control_output  =RC_YAW;
		Horizontal_Control_VIO(0);
		Flight_Alt_Hold_Control(ALTHOLD_AUTO_VEL_CTRL,NUL,-30);//高度控制		
	}
	else
	{
		basic_auto_flight_support();//基本飞行支持软件
	}
}

//2023年TI电赛G题目——空地协同智能消防系统
/************************************************
	D  E  F
	A  B  C
U	G	
************************************************/
#define block_navpoint_num_basic 6//基础部分航点数量
const int16_t block_center_coordinate[block_navpoint_num_basic][2]={
	{100,155},//A
	{255,155},//B
	{410,125},//C
	{410,305},//F
	{270,305},//E
	{115,305},//D
};
const int16_t firetruck_home_center_coordinate[2]={135,25};//G
const int16_t uav_home_center_coordinate[2]={35,35};//U
#define Patrol_Height         180//巡逻高度180cm
#define patrol_fixed_3d_30cm  30.0f//30cm
#define patrol_fixed_3d_20cm  20.0f//20cm
#define patrol_fixed_3d_10cm  10.0f//10cm
#define patrol_fixed_3d_5cm   5.0f//5cm
#define patrol_fixed_times    3//满足次数
#define patrol_fixed_2d_5cm   5.0f//5cm
#define patrol_fixed_2d_times 5
void Air_Ground_Extinguish_Fire_System_Basic(void)
{
	static uint8_t n=14;
	vector3f target_position;
	float x=0,y=0,z=0;	
	if(flight_subtask_cnt[n]==0)//起飞点作为第一个悬停点
	{
		basic_auto_flight_support();//基本飞行支持软件
		//激光笔点亮
		laser_light_1.period=100;//200*5ms
		laser_light_1.light_on_percent=1.0f;
		laser_light_1.reset=1;
		laser_light_1.times=10000;
		
		//记录下初始起点位置，实际项目中可设置为某一基准原点
		//base_position.x=VIO_SINS.Position[_EAST];
		//base_position.y=VIO_SINS.Position[_NORTH];
		base_position.z=Patrol_Height;//巡逻高度
		
		x=base_position.x;
		y=base_position.y;
		z=Patrol_Height;
		target_position.x=x;
		target_position.y=y;
		target_position.z=z;
		Horizontal_Navigation(target_position.x,target_position.y,target_position.z,GLOBAL_MODE,MAP_FRAME);
		
		flight_subtask_cnt[n]=1;
		flight_global_cnt[n]=0;
		flight_global_cnt2[n]=0;
		execute_time_ms[n]=1000/flight_subtask_delta;//子任务执行时间
	}
	else if(flight_subtask_cnt[n]==1)//起飞之后原定悬停1S后再执行航点任务
	{
		basic_auto_flight_support();//基本飞行支持软件
		
		//判断所有航点均执行完毕，执行下一任务
		if(flight_global_cnt2[n]>=block_navpoint_num_basic)
		{
			flight_subtask_cnt[n]=3;//结束航点遍历
			flight_global_cnt[n]=0;
			execute_time_ms[n]=0;
			//航点计数器清0
			flight_global_cnt2[n]=0;

						//继续执行返航任务航点任务
			target_position.x=base_position.x;
			target_position.y=base_position.y;
			target_position.z=Patrol_Height;
			Horizontal_Navigation(target_position.x,target_position.y,target_position.z,GLOBAL_MODE,MAP_FRAME);
			
			return ;				
		}
		
		uint16_t current_num=constrain_int32(flight_global_cnt2[n],0,block_navpoint_num_basic-1);//限幅防溢出		
		if(execute_time_ms[n]>0) execute_time_ms[n]--;
		if(execute_time_ms[n]==0)//悬停时间计数器归零，悬停任务执行完毕 
		{				
			//设置A航点任务
			x=block_center_coordinate[current_num][0]-uav_home_center_coordinate[0];
			y=block_center_coordinate[current_num][1]-uav_home_center_coordinate[1];
			z=Patrol_Height;
			target_position.x=base_position.x+x;//水平位置期望为起飞后基准位置+位置X偏移
			target_position.y=base_position.y+y;//水平位置期望为起飞后基准位置+位置Y偏移
			target_position.z=z;
			Horizontal_Navigation(target_position.x,target_position.y,target_position.z,GLOBAL_MODE,MAP_FRAME);
			
			flight_subtask_cnt[n]=2;
			flight_global_cnt[n]=0;
			execute_time_ms[n]=0;				
		}		
	}
	else if(flight_subtask_cnt[n]==2)//检测起飞点悬停完毕后，飞向下一个目标点
	{
		basic_auto_flight_support();//基本飞行支持软件		
		//判断是否到达目标航点位置
		if(flight_global_cnt[n]<patrol_fixed_times)//持续10*5ms=0.05s满足
		{
			float dis_cm=pythagorous3(OpticalFlow_Pos_Ctrl_Err.x,OpticalFlow_Pos_Ctrl_Err.y,Total_Controller.Height_Position_Control.Err);
			//距离误差约束20cm，针对基础赛题可以加大此处阈值，从而实现更快速遍历
			if(dis_cm<=patrol_fixed_3d_20cm)	flight_global_cnt[n]++;
			else flight_global_cnt[n]/=2;
		}
		else//持续10*5ms满足，表示到达目标航点位置
		{
			flight_subtask_cnt[n]=1;
			flight_global_cnt[n]=0;
			execute_time_ms[n]=500/flight_subtask_delta;;//更改此值可以对每个航点的悬停时间进行设置	
			//航点计数器自加
			flight_global_cnt2[n]++;
		}
		
		//判断所有航点均执行完毕，执行下一任务
		if(flight_global_cnt2[n]>=block_navpoint_num_basic)
		{
			flight_subtask_cnt[n]=3;//结束航点遍历
			flight_global_cnt[n]=0;
			execute_time_ms[n]=0;
			//航点计数器清0
			flight_global_cnt2[n]=0;
			
			//继续执行返航任务航点任务
			target_position.x=base_position.x;
			target_position.y=base_position.y;
			target_position.z=Patrol_Height;
			Horizontal_Navigation(target_position.x,target_position.y,target_position.z,GLOBAL_MODE,MAP_FRAME);
			return ;				
		}
	}
	else if(flight_subtask_cnt[n]==3)//飞向起飞点正上方
	{
		basic_auto_flight_support();//基本飞行支持软件
		//判断是否到达目标航点位置
		if(flight_global_cnt[n]<patrol_fixed_2d_times)//持续10*5ms=0.05s满足
		{
			float dis_cm=pythagorous2(OpticalFlow_Pos_Ctrl_Err.x,OpticalFlow_Pos_Ctrl_Err.y);
			if(dis_cm<=patrol_fixed_2d_5cm)	flight_global_cnt[n]++;
			else flight_global_cnt[n]/=2;
		}
		else//持续10*5ms满足，表示到达目标航点位置
		{
			flight_subtask_cnt[n]++;
			flight_global_cnt[n]=0;
			execute_time_ms[n]=0;
			
			//以下复位操作的作用：空中到地面不同高度，环境分布陈设变化造成的误差
			//不同高度梯度上陈设变化不大时，以下特殊处理部分可以去掉
			//特殊处理开始
			/*
			send_check_back=4;//重置slam
			VIO_SINS.Position[_EAST] = 0;
			VIO_SINS.Position[_NORTH]= 0;
		  OpticalFlow_Pos_Ctrl_Expect.x=0;
		  OpticalFlow_Pos_Ctrl_Expect.y=0;
			*/
			//特殊处理结束	
		}
	}	
	else if(flight_subtask_cnt[n]==4)//原地下降
	{
		Flight.yaw_ctrl_mode=ROTATE;
		Flight.yaw_outer_control_output=RC_YAW;
		Horizontal_Control_VIO(0);
		Flight_Alt_Hold_Control(ALTHOLD_AUTO_VEL_CTRL,NUL,-30);//高度控制		
	}
	else
	{
		basic_auto_flight_support();//基本飞行支持软件
	}
}