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

		if (rov_control.floodlight == 1  && motors.armed())
	   {

			
			RC_Channels::set_override(8, 20000, tnow);       // lights 1
  

	   }
		else {
			
			RC_Channels::set_override(8, 0, tnow);       // lights 1
	   		

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