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


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()
{
    // 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)
{
	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();
	static uint8_t last_light = 0;

		if (rov_control.floodlight == 1  && motors.armed())
	   {
	   		last_light = 1;
			lights1 = constrain_float(max, min, max);
	   }
		else if(!motors.armed()){
			last_light = 0;
			lights1 = constrain_float(min, min, max);	
	   }else if(last_light == 1){
	   		last_light = 0;
	   		lights1 = constrain_float(min, min, max);
	   	}
		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.0)
		{
		temp = sqrtf(data);
		}
	else if(data<0.0){
		temp = -sqrtf(-data);
		}
	else{
		temp = 0.0;
		}
	if (temp>0.3)
	{
		temp = constrain_float(temp,0.4,1.0);
	}else if(temp<-0.3){
		temp = constrain_float(temp,-1.0,-0.4);
	}else{
		temp=0.0;
		}*/
	if (data>=-0.6 && data <-0.08)
	{
		temp = -0.3;
	}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.3;
	}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::gaindelay(float gain1,float _gain){
	
	if (fabsf(gain1 -0.5)<0.05)
	{
		delaygain++;
		if (delaygain>400)//1秒
		{
			delaygain=0;
			_gain = 0.5;
			updowmgain = 0.5;
		}
		return _gain;
    }else{
		return gain1;
    }

}