N3_sub/ArduSub/UserCode.cpp

#include "Sub.h"

#ifdef USERHOOK_INIT
void Sub::userhook_init()
{
    // put your initialisation code here
    // this will be called once at start-up
}
#endif

#ifdef USERHOOK_FASTLOOP
void Sub::userhook_FastLoop()
{
    // put your 100Hz code here
}
#endif

#ifdef USERHOOK_50HZLOOP
extern mavlink_rov_control_t rov_control;

void Sub::userhook_50Hz()
{
    USBL_PowerSwitch();
	getgain();
	getyaw();
}
#endif

#ifdef USERHOOK_MEDIUMLOOP
void Sub::userhook_MediumLoop()
{
    // put your 10Hz code here
}
#endif

#ifdef USERHOOK_SLOWLOOP
void Sub::userhook_SlowLoop()
{
    // put your 3.3Hz code here
}
#endif

#ifdef USERHOOK_SUPERSLOWLOOP
void Sub::userhook_SuperSlowLoop()
{
    // put your 1Hz code here
}
#endif

void Sub::USBL_PowerSwitch(void)
{
		if (rov_control.floodlight == 1  && motors.armed())
	   {
		   lights = 1;
	   }else {
		   lights = 0;
	   }
		if(motors.armed() && rov_control.USBL_flag==1 ){
			usblpoweroff = 1;//usbl
		}else{
			usblpoweroff = 0;//usbl
		}

}
extern mavlink_rov_state_monitoring_t rov_message;

void Sub::getgain(void){
	//上位机设置油门
	//uint16_t _gain = SRV_Channels::srv_channel(9)->get_output_min();//上位机设置油门
	
	gain = 1.0;
	

	static int j = 0;
	 j++;
	 if(j>800)
	 {
		//gcs().send_text(MAV_SEVERITY_WARNING, " gain  %f\n",(float)gain);	 
		   j=0;
	}
}

void Sub::getyaw(void){
	if (!motors.armed()){
		yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
		 return;
	}else{
	
	}
}
void Sub::updat_Atom(void){

			uart2_read_Atom(hal.uartD);
			GetAngle();	  
}

void Sub::uart2_read_Atom(AP_HAL::UARTDriver *uart)
{
	if (uart == nullptr) {
		return;
	}
	uint32_t readtime=AP_HAL::micros(); 
	unsigned char received_char;
	unsigned char Xordata;
	unsigned char last_byte;
		  
	while(uart->available())//recive num
	{
		received_char = uart->read();
		ucRxBufferATOM[ucRxCnt_atom]=received_char;   


		//数据头是41 78 -----------------------  
		if(ucRxBufferATOM[0]!=0x41) {
			ucRxCnt_atom=0;
			// 不是头
			 continue;
		} else if(ucRxCnt_atom>0 && ucRxBufferATOM[1]!=0x78){
			ucRxCnt_atom=0;
			//不是头
			 continue;
		}
		//---------------------
		ucRxCnt_atom++;
		
		last_byte =ucRxBufferATOM[5]+7;//数据个数ucRxBufferATOM[5]     +  帧头6个：41 78 FF 06 81 35+校验1+尾6d
		if(ucRxCnt_atom>last_byte && ucRxBufferATOM[last_byte]==0x6D)
		{
			break;
		}
			if(AP_HAL::micros() - readtime >800)//时间溢出0.8ms
			{//接收数据超时
				ucRxCnt_atom=0;
				usart_state_atom =FALSE;//通信失败		
				memset(ucRxBufferATOM, 0, sizeof(unsigned char)*256);
				gcs().send_text(MAV_SEVERITY_INFO, "atom read timeout");
				return;

			}
				    
	 }
	if (ucRxCnt_atom==0 ){//没有接收到数据
			return;
	}else{
		  //异或校验
		Xordata =Atom_BBC(ucRxBufferATOM,ucRxCnt_atom-2);
		  
		if (Xordata != ucRxBufferATOM[ucRxCnt_atom-2])
		{//校验失败
			static int j = 0;
			j++;
			 if(j>400)
			{
					gcs().send_text(MAV_SEVERITY_INFO, " ATOM 400 times error \n");
					j=0;
			}
		  
			ucRxCnt_atom =0;
			usart_state_atom =FALSE;	//通信失败		  
			memset(ucRxBufferATOM, 0, sizeof(unsigned char)*256);
			return;
		}
		
					  
		usart_state_atom =TRUE;//通信成功
		ucRxCnt_atom =0;
	}
			  
}

//原子九轴校验 异或校验
 unsigned char Sub::Atom_BBC(unsigned char *addr,uint16_t len)  
{
	unsigned char *DataPoint;
	DataPoint = addr;
	unsigned char XorData = 0;
	unsigned short DataLength=len;
	 while(DataLength--)
	{
		XorData = XorData ^ *DataPoint;
		DataPoint++;
	}
	return XorData;
}
 void Sub::GetAngle(void)
 {
				//添加数据
	 uint16_t data_cnt_sum ;
	 data_cnt_sum = ucRxBufferATOM[5]+6;//6个字节的帧头
	 uint32_t data_adress[127];
	 uint16_t data_cnt;
		   
	 uint16_t adress_index = 0;
	 uint16_t data_index=6;
	 uint16_t SaberID = 0x0000;
	 uint16_t i =0;

	 
	 if (usart_state_atom == TRUE )
	{
		 //收到的是命令
		 SaberID = ucRxBufferATOM[3]+(ucRxBufferATOM[4]<<8);
		
	}
	 if(usart_state_atom ==TRUE && (SaberID== RespAllStatusID ||SaberID == SystemResetAckID))
	 { //接收到的是命令反馈
	 	for(i=0;i<12;i++){
		
			SaberCommandRes[i] = ucRxBufferATOM[i];
			
	 	}
		usart_state_atom = FALSE;
		memset(ucRxBufferATOM, 0, sizeof(unsigned char)*256);
	 	
	 }else if(usart_state_atom ==TRUE &&SaberID == SaberDatapacketID){//接收到的是数据
		 while(data_index< data_cnt_sum)
		 {	//通信成功  数据包
			 data_adress[adress_index]= (ucRxBufferATOM[data_index+1]<<8)+ ucRxBufferATOM[data_index];
			   
					   
			 if (data_adress[adress_index] == 0x8C01)
			 {//读取陀螺仪角速度
				 GyroX = char_to_float(ucRxBufferATOM[data_index+3],ucRxBufferATOM[data_index+4],ucRxBufferATOM[data_index+5],ucRxBufferATOM[data_index+6]);
				 GyroY = char_to_float(ucRxBufferATOM[data_index+7],ucRxBufferATOM[data_index+8],ucRxBufferATOM[data_index+9],ucRxBufferATOM[data_index+10]);
				 GyroZ = char_to_float(ucRxBufferATOM[data_index+11],ucRxBufferATOM[data_index+12],ucRxBufferATOM[data_index+13],ucRxBufferATOM[data_index+14]);
				//转化成弧度
				 GyroX=GyroX*3.14/180.0;
				 GyroY=GyroY*3.14/180.0;
				 GyroZ=GyroZ*3.14/180.0;
				 
			 }
			 else if(data_adress[adress_index] ==0xB001)
			 {//读取角度
				 float yawbuf=0.0;	 
				
				 Roll = char_to_float(ucRxBufferATOM[data_index+3],ucRxBufferATOM[data_index+4],ucRxBufferATOM[data_index+5],ucRxBufferATOM[data_index+6]);
				 Pitch = char_to_float(ucRxBufferATOM[data_index+7],ucRxBufferATOM[data_index+8],ucRxBufferATOM[data_index+9],ucRxBufferATOM[data_index+10]);
				 yawbuf = char_to_float(ucRxBufferATOM[data_index+11],ucRxBufferATOM[data_index+12],ucRxBufferATOM[data_index+13],ucRxBufferATOM[data_index+14]);
				 //转化成弧度
				 Roll_Raian =Roll*3.14/180.0;
				 Pitch_Raian =Pitch*3.14/180.0;
				 //yaw转化成0-360
				 if(yawbuf	< 0.0)
				 { 
					   Yaw	= 360.0 + yawbuf ;
				 }  
				 else
				 {
					 Yaw = yawbuf;
				 }
				 
				 //------PITCH 90度安装-----------
				 /*Yaw_Angle = (360.0-Yaw);
				 Yaw_Raian = Yaw_Angle*3.14/180.0;
				 
				 attitude_control.update_ATOM_sensor(Roll_Raian,Pitch_Raian,Yaw_Raian,GyroY,GyroX,GyroZ);//// 注意原子九轴X是俯仰 Y是翻滚应当将XY反过来试一试
				 */
				 


				 //-------ROLL 90安装----------------------
				  float yawbufraian=0.0;
				 //原子的角速度方向和飞控相反
				 yawbufraian =(360.0-Yaw)*3.14/180.0+1.57;
				 Yaw_Angle = (360.0-Yaw)+90.0;
				 //限制角度在0-360之间
				 if (Yaw_Angle>360)
				 {
					 Yaw_Angle = Yaw_Angle-360;
				 }
				 
				 if (yawbufraian>6.28)
				 {
					 Yaw_Raian = yawbufraian-6.28;
				 }
				 else{
					 Yaw_Raian =yawbufraian;
				}

				 
				 
				 attitude_control.update_ATOM_sensor(Roll_Raian,Pitch_Raian,Yaw_Raian,GyroY,GyroX,-GyroZ);//// 注意原子九轴X是俯仰 Y是翻滚应当将XY反过来试一试

				 
				

			 }
				 data_cnt = ucRxBufferATOM[data_index+2];
				 data_index =data_index+data_cnt +3;
				 adress_index++;
		 }
		 usart_state_atom = FALSE;
		 memset(ucRxBufferATOM, 0, sizeof(unsigned char)*256);
	}
		   
 }

 //将char类型转换为float类型，输入为4个字节 小端在前，输出为float类型
float Sub::char_to_float(unsigned char u1,unsigned char u2,unsigned char u3,unsigned char u4)
{
	data_floatfromchar.u[0] = u1;
	data_floatfromchar.u[1] = u2;
	data_floatfromchar.u[2] = u3;
	data_floatfromchar.u[3] = u4;
	return data_floatfromchar.f; 
		  
}

#define maxangle 55.0

//机器人物理空间运动状态的水平竖直切换
void Sub::direction0_90(void)
{
	static int16_t cnt=0;
	static int16_t cnt2=0;
	static int16_t cnt_backzero1=0;
	static int16_t cnt_backzero2=0;
	/*
		控制区间的变化 抬头或者低头大于60度进入正负竖直状态；
		进入正负竖直状态之后 外置传感器的低头抬头要保证在机器人（不是任何传感器）在空间中60到150以及-60到-150之间，保持竖直状态标志为1，超出这个范围恢复水平状态
		侧歪若超过45度，说明控制不了，回到水平状态  即三个姿态均为0度
		水平状态下 roll不能过大 最大控制为70度，如果超过80度 说明控制不稳定回到水平状态
	*/
	/*ahrs.pitch_sensor：
			*（90）					*（>60）
				*				*------	*
					*--------*				*
						*（<-60）					*（-90）
	*/
	if ((ahrs.pitch_sensor >= attitude_control.change_angle*100) && (fabsf(Pitch)<maxangle))
	{//大于60 侧歪<45
			agl_sec = section90;//正竖直
	}
	else if((ahrs.pitch_sensor <= -9000-attitude_control.Offset_ver*100) && (fabsf(Pitch)<maxangle))//Offset_ver -30
	{//小于-60 侧歪<45
			agl_sec = section_90;//负竖直
	}
	else if(agl_sec == section90)
	{//正竖直
		if (fabsf(Pitch)>=maxangle ||(Roll>-120.0 && Roll<-60)){
			//侧歪>45 || 外置九轴大于-120 小于-60（机器人空间姿态150 - 210度）
			cnt_backzero1++;
			if(cnt_backzero1 >50){
				cnt_backzero1 = 0;
				attitude_control.roll_pitch_back_orign = 1;//复位回到水平
			
			}
		}else{
			cnt_backzero1 = 0;	
		}
		if ((Roll<150.0 && Roll>30))
		{//外置九轴30到150, 机器人空间姿态-60 - 60度
			cnt++;
			if (cnt>=50)
			{
				cnt = 0;
				agl_sec = section0;//水平
			}
		}
	}
	else if(agl_sec == section_90)
	{
		if (fabsf(Pitch)>=maxangle ||(Roll>-120.0 && Roll<-60)){
			//侧歪>45 || 外置九轴大于-120 小于-60（机器人空间姿态150 - 210度）
			cnt_backzero2++;
			if(cnt_backzero2 >50){
				cnt_backzero2 = 0;
				attitude_control.roll_pitch_back_orign = 1;//复位水平
			}
		}else{
			cnt_backzero2 = 0;	
		}
		if (Roll>30 && Roll< 150)//Offset_ver -30
		{//外置九轴30到150, 机器人空间姿态-60 - 60度
			cnt2++;
			if (cnt2>=50)
			{
				cnt2 = 0;
				agl_sec = section0;//水平
			}
		}

	}
	else if(fabsf(Pitch)>=maxangle){
		//20210624---------------
		if (control_mode == STABILIZE)
		{
		attitude_control.roll_pitch_back_orign = 1;//复位回到水平
		
		}
		agl_sec = section0;//水平
		
	}
	//--------------------------------------------
/* 机器人 飞控 外置九轴的空间姿态对照表
	
	机器人pitch	飞控	pitch  九轴	roll			计算	
	0	   			 0	    90	   			    90-飞控
	60				60		30					90-飞控
	90				90		0					90-飞控
	120 			60		-30 				飞控-90
	150 			30		-60 				飞控-90
	180 			0		-90 				飞控-90
	270 			-90 	-180				飞控-90
	360 			0		90					90-飞控
	这里目前机器人的角度限制在-90到+90 也就是90-飞控的模式下
	*/
	if (attitude_control.roll_pitch_back_orign ==1){
			if (fabsf(ahrs.pitch_sensor)<3000 &&fabsf(ahrs.roll_sensor)<3000)
			{//	确保已经回到水平状态
				attitude_control.roll_pitch_back_orign =0;
			}
	}
	if (agl_sec == section0)
	{//水平状态
		/*if (attitude_control.roll_pitch_back_orign ==1){
			if (fabsf(ahrs.pitch_sensor)<3000 &&fabsf(ahrs.roll_sensor)<3000)
			{//	确保已经回到水平状态
				attitude_control.roll_pitch_back_orign =0;
			}
		}*/
		agl_act =action_Hor;
		
	}
	else if(agl_sec == section90)
	{//竖直
			agl_act =action_Ver_postive;
			//外置九轴PID的pitch给定
			attitude_control.deta_angle_901 =90.0-attitude_control.angle_geiding/100.0;//注意这里angle_geiding的范围是-90到90 也就是机器人本体物理角度的-90到正90度，参照上述说明

	}
	else if(agl_sec == section_90)
		{//负向竖直
			agl_act =action_Ver_negtive;
			//外置九轴PID的pitch给定

			attitude_control.deta_angle_901 =90.0-attitude_control.angle_geiding/100.0;
				//外置九轴的roll范围是-180到+180
				if(attitude_control.deta_angle_901<-180.0)
				{
					attitude_control.deta_angle_901 = attitude_control.deta_angle_901 +360.0;
				}

		 		
		}
		if(agl_act ==action_Hor)
		{//相当于外置九轴的pitch 不能超过70度，超过非常不稳定
			if((ahrs.roll_sensor>7000&&ahrs.roll_sensor<8000) ||(ahrs.roll_sensor<-7000 && ahrs.roll_sensor>-8000))
				{
				//回到水平状态
				attitude_control.roll_pitch_back_orign =1;
				}
			else if(ahrs.roll_sensor>=8000 || ahrs.roll_sensor<=-8000)
				{//超过80 disarm
					//arming.disarm();//海检测试抗流性的时候侧着安装不能启动
					attitude_control.roll_pitch_back_orign =1;
				}
		}
		if (!motors.armed())
		{
				
				attitude_control.roll_pitch_back_orign = 0;
		}
		

	  attitude_control.update_Hor_Ver_Choose(agl_act);//将状态传递到姿态控制类
		static int j = 0;
		j++;
		if(j>500)
		{
			// gcs().send_text(MAV_SEVERITY_WARNING, " roll_pitch_back_orign %d %d\n",(int)agl_act,(int)attitude_control.roll_pitch_back_orign);	
			 j=0;
		}



}