#include "Sub.h"
//const AP_HAL::HAL& hal = AP_HAL::get_HAL();
extern const AP_HAL::HAL& hal;
#define PI	3.141592653589793238462643383279502884


extern mavlink_rov_state_monitoring_t rov_message;


#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();

	 float v  = chan_adc->voltage_average();

	 
	  chan_adc->set_pin(14);
	 float temprature =0.0;
	 if (v>2.58)
	{
		 temprature = -44.017*v + 112.64;
	}else if(v>1.0){
		temprature =-28.635*v + 72.616;
	}else if(v>0.518){
		temprature =-48.078*v + 91.882;
	}else if(v>0.28){
		temprature =-90.07*v + 113.38;
	}else if(v>0.12){
		temprature = -195.98*v + 141.46;
	}
	else{
		temprature = 120;
	}

	rov_message.temp = temprature;



     
}
#endif

#ifdef USERHOOK_MEDIUMLOOP
void Sub::userhook_MediumLoop()
{
   //read_external_DVL(hal.uartD);//uart4
   //write_external_DVL(hal.uartD);		
}
#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)
{
	uint32_t tnow = AP_HAL::millis();
	RC_Channel* chan = RC_Channels::rc_channel(8);
	uint16_t min = chan->get_radio_min();
	uint16_t max = chan->get_radio_max();

	   	if(!motors.armed()){	
	   		lights1 = constrain_float(min, min, max);
	   	}
	   if (lights1>1000)
		  {
		   lights = 1;
		  	}else{
	   		lights =0;
		}
	   
		RC_Channels::set_override(8, lights1, tnow);
		if(motors.armed() && (rov_control.USBL_flag==1 || usblpoweroff == 1))
		{
			
			 RC_Channels::set_override(9, 20000, tnow);       // lights 2
		
			
		}else{
		 	
			   RC_Channels::set_override(9, 0, tnow);		// lights 1
			 
		}
		

}
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{
	
	}
}

float Sub::getgainf(float data){
	float temp =0;

	if (data>=-0.6 && data <-0.08)
	{
		temp = -0.2;
	}else if(data>=-0.8 && data<-0.6 )
	{
		temp = -0.5;
	}else if(data>=-1.0 && data<-0.8 )
	{
		temp = -0.8;
	}
	else if (data<=0.6 && data >0.08)
	{
		temp = 0.2;
	}else if(data<=0.8 && data>0.6 )
	{
		temp = 0.5;
	}else if(data<=1.0 && data>0.8 )
	{
		temp = 0.8;
	}else{
		temp = 0.0;
	}
	return temp;
}
float Sub::getgainfstatble(float data){
	float temp =0;

	if (data>=-0.5 && data <-0.08)
	{
		temp = -0.2;
	}else if(data>=-0.8 && data<-0.5 )
	{
		temp = -0.5;
	}else if(data>=-1.0 && data<-0.8 )
	{
		temp = -1.0;
	}
	else if (data<=0.5 && data >0.08)
	{
		temp = 0.2;
	}else if(data<=0.8 && data>0.5 )
	{
		temp = 0.5;
	}else if(data<=1.0 && data>0.8 )
	{
		temp = 1.0;
	}else{
		temp = 0.0;
	}
	return temp;
}

float Sub::sign_in(float f){
	return (f<0)? -1 :1;
}
float Sub::gaindelay(float gain1){

	if(throttle_continuous(gain1)){
		
		bool vel_stationary = velocity_z_filer > -5.0 && velocity_z_filer < 5.0;//cm
		float	result = (updowmgain-0.5)*2;

		if (vel_stationary){
			last_continuous = 1;
			 continuous_count++;
			 if (continuous_count>=MAIN_LOOP_RATE && continuous_count<=4*MAIN_LOOP_RATE){
				result = result + sign_in(result)*0.2*(continuous_count/MAIN_LOOP_RATE);//步长是0.2
				result = constrain_float(result,-1.0,1.0);
			}else if(continuous_count>4*MAIN_LOOP_RATE){
				result = result +sign_in(result)*0.2*(continuous_count/MAIN_LOOP_RATE);//步长是0.2
				result = constrain_float(result,-1.0,1.0);
				continuous_count = 4*MAIN_LOOP_RATE+1;
			}
		}else{
			if (last_continuous == 1)
			{
				if (continuous_countdec<MAIN_LOOP_RATE){
					continuous_countdec++;
					result = result + sign_in(result)*0.2*(continuous_count/MAIN_LOOP_RATE);//步长是0.2
					result = constrain_float(result,-1.0,1.0);
				}else{
					result = result + sign_in(result)*0.2*(continuous_count/MAIN_LOOP_RATE);//步长是0.2
					result = constrain_float(result,-1.0,1.0);
				}
			}else{
					result = (updowmgain-0.5)*2;
					continuous_count = 0;
					last_continuous = 0;
					continuous_countdec = 0;
			}
		
		}
		 static uint16_t count = 0;
	 count++;
	 if (count>400)
		 {
		 count = 0;
		 gcs().send_text(MAV_SEVERITY_INFO, "conti %d %d %d %f\n",(int)continuous_count,(int)last_continuous,(int)continuous_countdec,result);

	 }

		return constrain_float(result/2.0+0.5,0.0,1.0);
	}else{

		 	
			continuous_count = 0;
			last_continuous = 0;
			continuous_countdec = 0;
		return gain1;//有压力分级的时候返回压力分级 没有压力分级的时候返回按键
	}
    
}
bool Sub::throttle_continuous(float gain1){
	static float last_gain = updowmgain;
	if (fabsf(gain1 -0.5)<0.05)//没有压力分级
	{
		delaygain++;
		if (delaygain>400)//1秒
		{
			delaygain=401;
			updowmgain = 0.5;
			last_gain = updowmgain;
			return FALSE;//1秒内没有持续按键
		}else{
			
			if ((last_gain>0.5 && updowmgain<0.5) || (last_gain<0.5 && updowmgain>0.5))//防止突然反向
			{
				last_gain = updowmgain;
				return FALSE;
			}
			last_gain = updowmgain;
			return TRUE;//持续按键
		}
		
    }else{
    	delaygain=401;
    //有压力分级
    	updowmgain = 0.5;
		last_gain = updowmgain;
		continuous_count = 0;
		last_continuous = 0;
		continuous_countdec = 0;
    	return FALSE;//1秒内没有持续按键
	}
}

/*float Sub::gaindelay(float gain1){
	 float result=0.5;
	 static uint16_t count=0;
	 static uint16_t countdec=0;
	 static uint8_t last = 0;
	if (fabsf(gain1 -0.5)<0.05)
	{
		delaygain++;
		if (delaygain>300)//1秒
		{
			delaygain=0;
			
			result = 0.5;
			updowmgain = 0.5;
			count = 0;
		}else{
			float velocity_z = pos_control.alt_rate.get()/100;// m/s
		    bool vel_stationary = velocity_z > -0.05 && velocity_z < 0.05;
			if (vel_stationary && updowmgain>0.5)//上升
			{
				last = 1;
				countdec = 0;
				 count++;
				 if (count<MAIN_LOOP_RATE)
				{
					 result = updowmgain;
				}
				 else if (count>=MAIN_LOOP_RATE && count<=3*MAIN_LOOP_RATE)
				{
					 result = updowmgain+0.1*(count/MAIN_LOOP_RATE);
					 result = constrain_float(result,-1.0,1.0);
				}else if(count>3*MAIN_LOOP_RATE){
					result = 1.0;
					count = 3*MAIN_LOOP_RATE+1;
				}
				
			}else{
				if (last == 1)
				{
					if (countdec<MAIN_LOOP_RATE){
						countdec++;
						result = updowmgain+0.1*(count/MAIN_LOOP_RATE);
					    result = constrain_float(result,-1.0,1.0);
					}else{
						last = 0;
						result = updowmgain;
						count = 0;
						countdec = 0;
					}
				}else{
					result = updowmgain;
					count = 0;
					last = 0;
					countdec = 0;
				}
			
			}
		}
		return result;
    }else{
		count = 0;
		updowmgain = 0.5;
		last = 0;
		countdec = 0;
		return gain1;//有压力分级的时候返回压力分级 没有压力分级的时候返回按键
    } 
}*/
//得到手柄的轴
void Sub::get_heading_to_ned(Matrix3f &mat_half,Matrix3f mat_body){

	// Matrix3f mat_body = ahrs.get_rotation_body_to_ned();
	float beta = 0.0;  
	float dot=0.0;
	
	if(mat_body.c.x<0.98 && mat_body.c.x>-0.98){//大约78.5度认为Z轴指向方向，小于78.5进入X轴指向方向
		Vector3f Xbx = mat_body*Vector3f(1,0,0);
		Xbx.z = 0.0;
		Vector3f Xex = Vector3f(1,0,0);
	
	   dot = Xbx * Xex/Xbx.length();
		beta = acosf(dot );
		if (Xbx.y<0)
	   {
		   beta = -beta;
	   }
		Vector3f Xbz = mat_body*Vector3f(0,0,1);
		Vector3f Xez = Vector3f(0,0,1);
	   float dotz = Xbz * Xez/Xbz.length();
		if (dotz<0)
	   {
			beta = beta-PI;
			if (beta<-PI)
		   {
				beta = beta+2*PI;
		   }
	   }
	
		
		mat_half.a.x= cosf(beta);
		mat_half.a.y=-sinf(beta);
		mat_half.a.z=0.0;  
		mat_half.b.x= sinf(beta);
		mat_half.b.y= cosf(beta);
		mat_half.b.z=0.0;
		mat_half.c.x= 0.0;
		mat_half.c.y= 0.0;
		mat_half.c.z=1.0;
	
	
	   }
	else{
	   
	   Vector3f Xbz = mat_body*Vector3f(0,0,1);
	   Xbz.z = 0.0;
	   Vector3f Xex = Vector3f(1,0,0);
	   dot = Xbz * Xex/Xbz.length();
	   beta =  acosf(dot );
	   if (Xbz.y<0)
	   {
		   beta = -beta;
	   }
		if (mat_body.c.x>0)//头朝下
	   {
			beta = beta-PI;
			if (beta<-PI)
		   {
				beta = beta+2*PI;
		   }
	   }
		mat_half.a.x= cosf(beta);
		mat_half.a.y=-sinf(beta);
		mat_half.a.z=0.0;  
		mat_half.b.x= sinf(beta);
		mat_half.b.y= cosf(beta);
		mat_half.b.z=0.0;
		mat_half.c.x= 0.0;
		mat_half.c.y= 0.0;
		mat_half.c.z=1.0;
	
	
	}

		static uint16_t countx = 0;
	   countx++;
	   if (countx>400)
		  {
		   countx = 0;
		   
		  	//   gcs().send_text(MAV_SEVERITY_INFO, "ang %d %d  \n",(int)(beta*180/PI),(int)ahrs.yaw_sensor/100);

	   }

}
float Sub::get_pitch_to_ned(Matrix3f mat_body){

	float pitch_ang = acosf(mat_body.c.x);
	if (mat_body.c.z<0)
	{
		pitch_ang = -pitch_ang;
	}
	pitch_ang =pitch_ang-PI/2;
	if (pitch_ang<-PI)
	{
		pitch_ang =pitch_ang+2*PI;
	}
	return pitch_ang;
}
void Sub::charArrtobyteArr(unsigned char *charArr ,uint8_t * dec,int16_t len){
	int16_t i = 0;
	for(i = 0 ;i<len;i++){
		dec[i] = (uint8_t)charArr[i] ;
	}	
}

void Sub::charArrtodecArr(unsigned char *charArr ,uint8_t * dec,int16_t len){
	
	int16_t i = 0;
	uint8_t j=0;
	uint8_t k=0;
	char charArrtemp[20]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
	

	static uint8_t cnt = 0;
	cnt = 0;
	int typeflag = 0;
	for(i = 4 ;i<len;i++){
		
		if (charArr[i]!=',' && charArr[i]!=';'&& charArr[i]!='*')//&&charArr[i]!='y'&&charArr[i]!='n')//分割数据
		{
			charArrtemp[cnt++] = charArr[i];
			if (charArr[i] == '.')//不是整形
			{
				typeflag = 1;
			}
			
		}else{
			charArrtemp[cnt] = '\0';
			if (charArr[i-1]=='y' ||charArr[i-1]=='n')
			{
				arryf[j++] = charArr[i-1];
			}
			else if(typeflag == 0 && cnt>11)//长长整型 只有时间戳才会有这样的长整形
			{
				int64_t l = atoll (charArrtemp);
				timearry[k++] = l;
			}else if(cnt>11){//双精度 转换的话会丢掉精度  因此变成整形不会丢失精度 
				int il, jl,kl=0;
				for (il = 0, jl = 0; charArrtemp[il] != '\0'; il++) {
					if (charArrtemp[il] != '.') {
						charArrtemp[jl++] = charArrtemp[il];
					}else{
					kl = il;//找出.的位置
					}
				}
				charArrtemp[jl] = '\0';
				DVLdata_wrp.time_Mulfactor = jl-kl;
				int64_t l = atoll (charArrtemp);
				timearry[2] = l;
				//gcs().send_text(MAV_SEVERITY_INFO, "kl %02x %lld",DVLdata_wrp.time_Mulfactor,timearry[2]);
			}
			else{
				float f = atof(charArrtemp);
				arryf[j++] = f;
			}
			
			memset(charArrtemp, 0, sizeof(unsigned char)*20);
			cnt = 0;
			typeflag = 0;

		}
	}

	
}
void Sub:: write_external_DVL(AP_HAL::UARTDriver *uart){
	if (uart == nullptr) {
			return;
		}
	//unsigned char send_sucess = 0;
	/*if (DVLSet.type == 'D')
	{
		//send_sucess = uart->write(DVLSet.data,DVLSet.len);
		uart->write(DVLSet.data,DVLSet.len);
		DVLSet.type = '0';

	}*/
	static int jsn = 0;
	jsn++;
	if(jsn>100)
	{
	//	gcs().send_text(MAV_SEVERITY_INFO, " send %c%c%c%c%c%c%c%c%c%c%c\n",DVLSet.data[0],DVLSet.data[1],DVLSet.data[2],DVLSet.data[3],DVLSet.data[4],DVLSet.data[5],DVLSet.data[6],DVLSet.data[7],DVLSet.data[8],DVLSet.data[9],DVLSet.data[10]);
	//	gcs().send_text(MAV_SEVERITY_INFO, " send %d\n",(int)send_sucess);
		jsn=0;
	}	

}
void Sub::read_external_DVL(AP_HAL::UARTDriver *uart){
	if (uart == nullptr) {
			return;
		}
	
	uint32_t readtime=AP_HAL::micros();//微秒
	static uint32_t DVLvalidtime=AP_HAL::millis();//毫秒
	unsigned char received_char;
	static bool exitLoop = FALSE;
	exitLoop = FALSE;
	while(uart->available()&&!exitLoop)//recive num
	{
		received_char = uart->read();

		if (received_char == 'w')// 开始
		{
			RxDVL[0] = received_char;
		}
		if (RxDVL[0] == 'w' && received_char != '\r' && received_char != '\n')//得到开头 并且不是 尾  后开始存数据  '\r' 回车 '\n' 换行
		{
			RxDVL[DVL_RxCnt++]=received_char;//保存数据
		}
		if ((received_char == '\r') || (received_char == '\n'))//到达帧尾
		{
			if(RxDVL[0] == 'w' && RxDVL[1] == 'r'&&(RxDVL[2] == 'z'||RxDVL[2] == 'p'||RxDVL[2] == 'a'||RxDVL[2] == 'n'||RxDVL[2] == '?')){
				//开始校验
				uint8_t stringobj[DVL_RxCnt];
				charArrtobyteArr(RxDVL,stringobj,DVL_RxCnt);//转换成byte数组
				uint8_t crcsum = crc_crc8(stringobj, DVL_RxCnt-3);//使用模拟测试字符数组
				uint8_t dh = chartodec(RxDVL[DVL_RxCnt-2]);
				uint8_t dl = chartodec(RxDVL[DVL_RxCnt-1]);
				 for (int gcsi2 = 0; gcsi2 < DVL_RxCnt/32 + 1; gcsi2++)
				{
					 gcs().send_text(MAV_SEVERITY_INFO, "org %c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",RxDVL[gcsi2*32+0],RxDVL[gcsi2*32+1],RxDVL[gcsi2*32+2],RxDVL[gcsi2*32+3],RxDVL[gcsi2*32+4],RxDVL[gcsi2*32+5],RxDVL[gcsi2*32+6],RxDVL[gcsi2*32+7],
																							   RxDVL[gcsi2*32+8],RxDVL[gcsi2*32+9],RxDVL[gcsi2*32+10],RxDVL[gcsi2*32+11],RxDVL[gcsi2*32+12],RxDVL[gcsi2*32+13],RxDVL[gcsi2*32+14],RxDVL[gcsi2*32+15],
																							   RxDVL[gcsi2*32+16],RxDVL[gcsi2*32+17],RxDVL[gcsi2*32+18],RxDVL[gcsi2*32+19],RxDVL[gcsi2*32+20],RxDVL[gcsi2*32+21],RxDVL[gcsi2*32+22],RxDVL[gcsi2*32+23],
																							   RxDVL[gcsi2*32+24],RxDVL[gcsi2*32+25],RxDVL[gcsi2*32+26],RxDVL[gcsi2*32+27],RxDVL[gcsi2*32+28],RxDVL[gcsi2*32+29],RxDVL[gcsi2*32+30],RxDVL[gcsi2*32+31]);
				 }

				
				if (crcsum == dh*16+dl)
				{//校验通过
					charArrtodecArr(RxDVL,stringobj,DVL_RxCnt-2);//string 类型转为数据类型 计算数据
					switch (RxDVL[2]) {
					case 'z'://Velocity report
						//Velocity report wrz, [vx],[vy],[vz],[valid],[altitude],[fom],[covariance],
						//[time_of_validity],[time_of_transmission],[time],[status]
						DVLdata_wrz.Vx = arryf[0];
						DVLdata_wrz.Vy = arryf[1];
						DVLdata_wrz.Vz = arryf[2];
						DVLdata_wrz.valid = arryf[3];//  'y'/'n'
						DVLdata_wrz.altitude = arryf[4];
						DVLdata_wrz.fom = arryf[5];
						DVLdata_wrz.covariancearr[0] = arryf[6];
						DVLdata_wrz.covariancearr[1] = arryf[7];
						DVLdata_wrz.covariancearr[2] = arryf[8];
						DVLdata_wrz.covariancearr[3] = arryf[9];
						DVLdata_wrz.covariancearr[4] = arryf[10];
						DVLdata_wrz.covariancearr[5] = arryf[11];
						DVLdata_wrz.covariancearr[6] = arryf[12];
						DVLdata_wrz.covariancearr[7] = arryf[13];
						DVLdata_wrz.covariancearr[8] = arryf[14];
						DVLdata_wrz.timeofvalidity = timearry[0];
						DVLdata_wrz.timeoftransmission = timearry[1];
						DVLdata_wrz.time = arryf[15];
						DVLdata_wrz.status = (int)arryf[16];
						//树莓派源码中使用的是这一帧数据而不是wrp
						gcs().send_text(MAV_SEVERITY_INFO, "z %d %c %.3f %.3f %.3f %.3f",DVLdata_wrz.status,DVLdata_wrz.valid,DVLdata_wrz.time,DVLdata_wrz.Vx,DVLdata_wrz.Vy,DVLdata_wrz.Vz) ;
						gcs().send_text(MAV_SEVERITY_INFO, "z %lld %lld %.3f",DVLdata_wrz.timeofvalidity,DVLdata_wrz.timeoftransmission,DVLdata_wrz.time) ;

						if(DVLdata_wrz.status == 0){//0正常  1 故障
							//说明数据有效
							if (DVLdata_wrz.valid == 'y')//说明高度和速度有效
							{
								//transfer_position_delta_msg();
							
								DVLvalidtime=AP_HAL::millis();
								gcs().send_text(MAV_SEVERITY_INFO, "wrz %ld",DVLvalidtime) ;
							}
                			  
						}	
						exitLoop = TRUE;//退出循环
						break;
					case 'u'://Transducer report
							// wru, [id],[velocity],[distance],[rssi],[nsd]
							DVLdata_wru.id = (int)arryf[0];
							DVLdata_wru.velocity = arryf[1];
							DVLdata_wru.distance= arryf[2];
							DVLdata_wru.rssi = (int)arryf[3];
							DVLdata_wru.nsd = (int)arryf[4];
							gcs().send_text(MAV_SEVERITY_INFO, "wru %d %f %f %d",DVLdata_wru.id,DVLdata_wru.velocity,DVLdata_wru.distance,DVLdata_wru.rssi) ;
							exitLoop = TRUE;//退出循环
						break;
					case 'p'://Dead reckoning report
						// wrp, [time_stamp],[x],[y],[z],[pos_std],[roll],[pitch],[yaw],[status]
							DVLdata_wrp.time_stamp=timearry[2];//s  不能准确得到或者说不能准确打印出来
							DVLdata_wrp.Dx = arryf[0];
							DVLdata_wrp.Dy = arryf[1];
							DVLdata_wrp.Dz = arryf[2];
							DVLdata_wrp.pos_std = arryf[3];
							DVLdata_wrp.roll = arryf[4];
							DVLdata_wrp.pitch = arryf[5];
							DVLdata_wrp.yaw = arryf[6];
							DVLdata_wrp.status = (int)arryf[7];
							gcs().send_text(MAV_SEVERITY_INFO, "wrp %f %f %f %f",DVLdata_wrp.Dx,DVLdata_wrp.Dy,DVLdata_wrp.Dz,DVLdata_wrp.pos_std) ;

							exitLoop = TRUE;//退出循环
							break;
						//Dead reckoning can be reset by issuing the wcr command. The reply will be an ack ( wra ) if
						//the reset is successful, and a nak ( wrn ) if not.
					case 'a'://下发命令反馈 Successfully
							/*memset(DVLreturn.data, 0, 32);
							DVLreturn.type = 'V';
							DVLreturn.len = 3;
							DVLreturn.data[0] = 'w';
							DVLreturn.data[1] = 'r';
							DVLreturn.data[2] = 'a';*/
							gcs().send_text(MAV_SEVERITY_INFO, "wra");
							exitLoop = TRUE;//退出循环
						break;	
					case ('n'||'?')://下发反馈 失败
							/*memset(DVLreturn.data, 0, 32);
							DVLreturn.type = 'V';
							DVLreturn.len = 3;
							DVLreturn.data[0] = 'w';
							DVLreturn.data[1] = 'r';
							DVLreturn.data[2] = 'n';*/
							gcs().send_text(MAV_SEVERITY_INFO, "wrn");
							exitLoop = TRUE;//退出循环
						break;
					case 'c'://读配置成功
							/*memset(DVLreturn.data, 0, 32);
							DVLreturn.type = 'V';
							DVLreturn.len = DVL_RxCnt;
							for (int copyi = 0;  copyi < DVL_RxCnt; copyi++)
							{
								DVLreturn.data[copyi] = RxDVL[copyi];
							}*/
							gcs().send_text(MAV_SEVERITY_INFO, "wrc");
							exitLoop = TRUE;//退出循环
						break;	
							
					}
					DVL_RxCnt =0;//数据处理完了
					exitLoop = TRUE;//退出while
					memset(RxDVL, 0, sizeof(unsigned char)*240);
				}
				else{
					DVL_RxCnt =0;//校验错误
					//exitLoop = TRUE;//退出while
					memset(RxDVL, 0, sizeof(unsigned char)*240);
				}
			}
			else{
				DVL_RxCnt =0;
				//exitLoop = TRUE;//退出while
				//不是要接收数据的数据帧
				memset(RxDVL, 0, sizeof(unsigned char)*240);
			}
				
		}
		if(AP_HAL::micros() - readtime >800)//时间溢出0.8ms micros是微秒
		{//接收数据超时
				DVL_RxCnt=0;
				memset(RxDVL, 0, sizeof(unsigned char)*240);
				gcs().send_text(MAV_SEVERITY_INFO, " E read timeout");
				break;//退出while

		}
		
	}
	if (AP_HAL::millis() - DVLvalidtime>3000)//超过3秒没有可靠数据 millis是毫秒
		{
		DVLvalidtime = AP_HAL::micros();
		//gcs().send_text(MAV_SEVERITY_WARNING, " DVL lost valid data");
		}

	
}
int Sub::chartodec(unsigned char _ch){
	int dl = 0;
	if(_ch>='a' && _ch<='f'){
		dl = _ch-87;
	}else if(_ch>='0' && _ch<='9'){
		dl = _ch-48;
	}else{
		//不是数字
	}
	return dl;

}