更改了油门分段和灯光分级

ArduSub/Sub.cpp

@@ -46,6 +46,8 @@ Sub::Sub()
     // init sensor error logging flags
     sensor_health.baro = true;
     sensor_health.compass = true;
+	lights1 = 0;
+	updowmgain = 0.5;
 
 #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
     failsafe.pilot_input = true;

ArduSub/Sub.h

@@ -848,7 +848,11 @@ public:
 	PressNetLevel PressLevel;//压力分级枚举类型
 	float PressLevel_f;//压力分级float类型
 
+	float updowmgain;
+	float delaygain;
+
 	int16_t pitch_input_inc;//pitch给定
+	int16_t lights1;
 
 	 struct telemetry_info_t {
 	 	int16_t dutycycle;
@@ -879,6 +883,8 @@ public:
 	void clean_sidenet_run(void);
 	void track_pidcontrol(float _targethead,int16_t &_motor1,int16_t &_motor2);
 	float get_yaw_error(float yaw_heading);
+	float getgainf(float data);
+	float gaindelay(float gain1,float _gain);
 };
 
 extern const AP_HAL::HAL& hal;

ArduSub/UserCode.cpp

@@ -83,21 +83,24 @@ void Sub::userhook_SuperSlowLoop()
 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())
 	   {
-
-			
-			RC_Channels::set_override(8, 20000, tnow);       // lights 1
-  
-
-	   }
-		else {
-			
-			RC_Channels::set_override(8, 0, tnow);       // lights 1
-	   		
-
+	   		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))
 		{
 			
@@ -139,3 +142,65 @@ void Sub::getyaw(void){
 	}
 }
 
+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;
+    }
+
+}
+

ArduSub/control_clean.cpp

@@ -35,6 +35,8 @@ bool Sub::clean_init()
 
 }
 void Sub::track_reset(void){
+	updowmgain =0.5;
+
 	autoclean_command = FALSE;//自动刷网
 	clean_bottom_command =FALSE;
 	clean_bottom_flag = FALSE;	
@@ -77,6 +79,7 @@ void Sub::clean_run()
 			
 			PressLevel_f =5.0;//压力为0
 			PressLevel = no;
+			updowmgain =0.5;
 
 			//-------------
 			track_reset();

ArduSub/control_manual.cpp

@@ -9,10 +9,12 @@ bool Sub::manual_init()
     // attitude hold inputs become thrust inputs in manual mode
     // set to neutral to prevent chaotic behavior (esp. roll/pitch)
     set_neutral_controls();
+	updowmgain =0.5;
 
     return true;
 }
 
+
 extern mavlink_rov_state_monitoring_t rov_message;
 
 void Sub::manual_run()
@@ -24,6 +26,7 @@ void Sub::manual_run()
         attitude_control.relax_attitude_controllers();
 		PressLevel_f =5.0;
 		PressLevel = no;
+		updowmgain =0.5;
         return;
     }
 
@@ -35,15 +38,18 @@ void Sub::manual_run()
    
     motors.set_forward(channel_forward->norm_input());
     motors.set_lateral(channel_lateral->norm_input());
-	motors.set_throttle(1.0-(float)PressLevel_f*0.1);//0-0.5下压
+	//motors.set_throttle(1.0-(float)PressLevel_f*0.1);//0-0.5下压
+	motors.set_throttle(gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain));//0-0.5下压
 	static int j = 0;
+	float tt = gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain);
 	  j++;
-	  if(j>800)
+	  if(j>300)
 	  {
-		//  gcs().send_text(MAV_SEVERITY_INFO, " throttle %f\n",channel_throttle->norm_input());
-		 // gcs().send_text(MAV_SEVERITY_INFO, " yaw %f\n",channel_yaw->norm_input());
-		 // gcs().send_text(MAV_SEVERITY_INFO, " forward %f\n",channel_forward->norm_input());
-		 // gcs().send_text(MAV_SEVERITY_INFO, " lateral %f\n",channel_lateral->norm_input());
+	  	gcs().send_text(MAV_SEVERITY_INFO, " upgain %f\n",tt);//0~1.0
+		  //gcs().send_text(MAV_SEVERITY_INFO, " throttle %f\n",getgainf(channel_throttle->norm_input()));//0~1.0
+		 // gcs().send_text(MAV_SEVERITY_INFO, " yaw %f\n",getgainf(channel_yaw->norm_input()));//-1.0~1.0
+		 // gcs().send_text(MAV_SEVERITY_INFO, " forward %f\n",getgainf(channel_forward->norm_input()));//-1.0~1.0
+		 // gcs().send_text(MAV_SEVERITY_INFO, " lateral %f\n",getgainf(channel_lateral->norm_input()));//-1.0~1.0
 	    	
 		
 		 // gcs().send_text(MAV_SEVERITY_INFO, " forward  %d %d %d %d\n",(int16_t)channel_forward->radio_in,(int16_t)channel_forward->radio_min,(int16_t)channel_forward->radio_max,(int16_t)channel_forward->radio_trim);
@@ -54,3 +60,5 @@ void Sub::manual_run()
 	  }
 	rov_message.pressure_level = (int)PressLevel;
 }
+
+

ArduSub/control_sport.cpp

@@ -37,14 +37,14 @@ bool Sub::sport_init()
     last_roll = 0;
     last_pitch = 0;
     
+	updowmgain =0.5;
 
     last_yaw = ahrs.yaw_sensor;
     last_input_ms = AP_HAL::millis();
 	last_input_ms_stable = AP_HAL::millis();
 	
 	
-	gcs().send_text(MAV_SEVERITY_INFO, " init Z target %f %f",(float)pos_control.get_pos_target().z,barometer.get_altitude()*100);
-
+	
     return true;
 }
 
@@ -75,9 +75,11 @@ void Sub::handle_attitude_sport(){
         // If we don't have a mavlink attitude target, we use the pilot's input instead
         //get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
 		//get_pilot_desired_lean_angles((int16_t)(channel_lateral->norm_input()*5700), (int16_t)((0.5-channel_throttle->norm_input())*5700*2), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
-		get_pilot_desired_lean_angles(0, (int16_t)((0.5-channel_throttle->norm_input())*5700*2), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
+		//get_pilot_desired_lean_angles(0, (int16_t)((0.5-channel_throttle->norm_input())*5700*2), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
+		get_pilot_desired_lean_angles(0, (int16_t)((0.5-channel_throttle->norm_input())*5700), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
 
-		target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+		target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()*0.3);//*0.6 变慢
+		
         if (abs(target_roll) > 300 || abs(target_pitch) > 300) {
             //last_roll = ahrs.roll_sensor;
             //last_pitch = ahrs.pitch_sensor;
@@ -111,15 +113,12 @@ void Sub::handle_attitude_sport(){
 // should be called at 100hz or more
 void Sub::sport_run()
 {
-	static int f2 = 201;
-	static int f3 = 201;
+
 	
 
     // When unarmed, disable motors and stabilization
     if (!motors.armed()) {
-			f2 = 201;
-	        f3 = 201;
-	       
+
         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
         // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
         attitude_control.set_throttle_out(0.5 ,true, g.throttle_filt);
@@ -127,21 +126,14 @@ void Sub::sport_run()
         pos_control.relax_alt_hold_controllers();
 		
 		//pos_control.set_alt_target(barometer.get_altitude()*100);//当前深度
-		
+		updowmgain =0.5;
         last_roll = 0;
         last_pitch = 0;
         last_yaw = ahrs.yaw_sensor;
         holding_depth = false;
 		ahrs.get_quat_body_to_ned(attitude_desired_quat_);//12.19
 		
-		static int f = 0;
-		f++;
-		if(f>800)
-		{
-		
-		   gcs().send_text(MAV_SEVERITY_INFO, " disarm Z target %f %f",(float)pos_control.get_pos_target().z,barometer.get_altitude()*100);
-			f=0;
-		}
+
         return;
     }
 
@@ -161,14 +153,17 @@ void Sub::sport_run()
 
     //TODO: scale throttle with the ammount of thrusters in the given direction
    // motors.set_throttle(0.5+throttle_vehicle_frame.z + channel_throttle->norm_input()-0.5);
-    motors.set_throttle(throttle_vehicle_frame.z + 1.0-(float)PressLevel_f*0.1);
+   // motors.set_throttle(throttle_vehicle_frame.z + 1.0-(float)PressLevel_f*0.1);
+   
+   motors.set_throttle(throttle_vehicle_frame.z + gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain));
 	rov_message.pressure_level = int(PressLevel);
-    motors.set_forward(-throttle_vehicle_frame.x + channel_forward->norm_input());
-    motors.set_lateral(-throttle_vehicle_frame.y + channel_lateral->norm_input());
+    motors.set_forward(constrain_float(-throttle_vehicle_frame.x + getgainf(channel_forward->norm_input()),-0.8,0.8));//*0.6 变慢
+    motors.set_lateral(constrain_float(-throttle_vehicle_frame.y + getgainf(channel_lateral->norm_input()),-0.8,0.8));//*0.6 变慢
 
     // We rotate the RC inputs to the earth frame to check if the user is giving an input that would change the depth.
     //Vector3f earth_frame_rc_inputs = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), channel_lateral->norm_input(), (2.0f*(-0.5f+channel_throttle->norm_input())));
-	Vector3f earth_frame_rc_inputs = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), channel_lateral->norm_input(), (2.0*(0.5-PressLevel_f*0.1)));
+	//Vector3f earth_frame_rc_inputs = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), channel_lateral->norm_input(), (2.0*(0.5-PressLevel_f*0.1)));
+	Vector3f earth_frame_rc_inputs = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), 0.0, (2.0*(0.5-PressLevel_f*0.1)));//去掉侧移影响
 
     if (fabsf(earth_frame_rc_inputs.z) > 0.05f) { // Throttle input  above 5%
        // reset z targets to current values
@@ -176,65 +171,59 @@ void Sub::sport_run()
         pos_control.relax_alt_hold_controllers();
         //pos_control.set_alt_target(barometer.get_altitude()*100);//当前深度
 		
-		static int f = 0;
-		f2 = 201;
-	    f3 = 201;
-		f++;
-		if(f>400)
-		{
-		
-		    gcs().send_text(MAV_SEVERITY_INFO, " Z1 target %f %f ",(float)pos_control.get_pos_target().z,barometer.get_altitude()*100);
-			gcs().send_text(MAV_SEVERITY_INFO, " surface %d at_bottom %d ",ap.at_surface,ap.at_bottom);
-			f=0;
-		}
+
     } else { // hold z
         if (ap.at_surface) {	//最大油门不能向上动
             pos_control.set_alt_target(g.surface_depth - 10.0f); // set target to 5cm below surface level
             holding_depth = true;
 			
-			f2++;
-			if(f2>200)
-			{
-		
-		  		gcs().send_text(MAV_SEVERITY_INFO, " Zsurface target %f %f ",(float)pos_control.get_pos_target().z,barometer.get_altitude()*100);
-				gcs().send_text(MAV_SEVERITY_INFO, " surface %d at_bottom %d ",ap.at_surface,ap.at_bottom);
-				f2=0;
-			}
+
         } else if (ap.at_bottom) {//最大油门不能向下动
             //pos_control.set_alt_target(inertial_nav.get_altitude() + 20.0f); // set target to 10 cm above bottom
             pos_control.set_alt_target(barometer.get_altitude()*100 +20);//cm
             holding_depth = true;
 			
-			f3++;
-			if(f3>200)
-			{
-		
-		  		gcs().send_text(MAV_SEVERITY_INFO, " Zbottom target %f %f ",(float)pos_control.get_pos_target().z,barometer.get_altitude()*100);
-				gcs().send_text(MAV_SEVERITY_INFO, " surface %d at_bottom %d ",ap.at_surface,ap.at_bottom);
-				f3=0;
-			}
+
         } else if (!holding_depth) {
             //pos_control.set_target_to_stopping_point_z();//有对位置的付值
             pos_control.calc_leash_length_z();//刹车长度 12.19
             pos_control.set_alt_target(barometer.get_altitude()*100);//cm 当前深度
-            f2 = 201;
-	    	f3 = 201;
+
             holding_depth = true;
-		  	gcs().send_text(MAV_SEVERITY_INFO, " Z4 target %f %f ",(float)pos_control.get_pos_target().z,barometer.get_altitude()*100);
-			gcs().send_text(MAV_SEVERITY_INFO, " surface %d at_bottom %d ",ap.at_surface,ap.at_bottom);
-			
+		  
         }
     }
-			static int f5 = 0;
-			f5++;
-			if(f5>500)
-			{
-		
-		  		 gcs().send_text(MAV_SEVERITY_INFO, " Z00 target %f %f ",(float)pos_control.get_pos_target().z,barometer.get_altitude()*100);
 
-				gcs().send_text(MAV_SEVERITY_INFO, " surface %d at_bottom %d ",ap.at_surface,ap.at_bottom);
-				f5=0;
-			}
 
 	
 }
+/*
+float rmpforward(float _gain){
+	static uint16_t rmpcounter = 0;
+	float get_forward_gain = motors.get_forward();
+	float temp=0;
+	rmpcounter++;
+	if (rmpcounter>20)
+	{
+		rmpcounter =0;
+		if(get_forward_gain - _gain>0.01){
+			temp = get_forward_gain-0.01;
+		
+	 	}else if(get_forward_gain- _gain<-0.01){
+			temp = get_forward_gain+0.01;
+		}
+		else{
+			temp = _gain;
+		}
+	}
+	if (_gain>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;
+		}
+	
+
+}*/

ArduSub/control_stabilize.cpp

@@ -8,7 +8,7 @@ bool Sub::stabilize_init()
     
     last_roll_s = 0;
     last_pitch_s = 0;
-    
+    updowmgain =0.5;
     last_yaw_s = ahrs.yaw_sensor;
     //last_input_ms = AP_HAL::millis();
 	last_input_ms_stable = AP_HAL::millis();
@@ -61,11 +61,13 @@ void Sub::handle_attitude_stablize(){
 		   pitch_input = 0;
 		   target_roll = 0;
 		   last_roll_s = 0;
-		   target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+		   target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()*0.4);
+
+		   
 	   }else if(prepare_state == Vertical)
 		{
 		   pitch_input = 8000;
-		   target_roll = ((float)channel_yaw->get_control_in()*0.5);
+		   target_roll = ((float)channel_yaw->get_control_in()*0.4);
 		   
 		   target_yaw = 0;
 	   }
@@ -121,6 +123,7 @@ void Sub::stabilize_run()
         attitude_control.relax_attitude_controllers();
         last_roll_s = 0;
         last_pitch_s = 0;
+		updowmgain =0.5;
         last_yaw_s = ahrs.yaw_sensor;
 
 		PressLevel_f =5.0;//压力分级复位
@@ -134,9 +137,11 @@ void Sub::stabilize_run()
 
     handle_attitude_stablize();
 
-    motors.set_forward(channel_forward->norm_input());
-    motors.set_lateral(channel_lateral->norm_input());
+    motors.set_forward(constrain_float(getgainf(channel_forward->norm_input()),-0.8,0.8));
+    motors.set_lateral(constrain_float(getgainf(channel_lateral->norm_input()),-0.8,0.8));
+	
+		attitude_control.set_throttle_out(gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain), false, g.throttle_filt);//压力分级
 
-	attitude_control.set_throttle_out(1.0-(float)PressLevel_f*0.1, false, g.throttle_filt);//压力分级
+	//attitude_control.set_throttle_out(1.0-(float)PressLevel_f*0.1, false, g.throttle_filt);//压力分级
 	rov_message.pressure_level = int(PressLevel);
 }

ArduSub/joystick.cpp

@@ -7,7 +7,7 @@
 namespace {
 float cam_tilt = 1500.0;
 float cam_pan = 1500.0;
-int16_t lights1 = 0;
+
 int16_t lights2 = 0;
 int16_t rollTrim = 0;
 int16_t pitchTrim = 0;
@@ -50,6 +50,7 @@ void Sub::init_joystick()
 
     gain = constrain_float(gain, 0.1, 1.0);
 }
+extern mavlink_rov_control_t rov_control;
 
 void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons)
 {
@@ -133,7 +134,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
   //  RC_Channels::set_override(9, lights2, tnow);       // lights 2
     RC_Channels::set_override(10, video_switch, tnow); // video switch
     
-	lights1 =constrain_int16(lights1, 0, 20000);
+	//lights1 =constrain_int16(lights1, 0, 20000);
 	lights2 =constrain_int16(lights2, 0, 20000);
 
     // Store old x, y, z values for use in input hold logic
@@ -245,15 +246,16 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
         break;
     case JSButton::button_function_t::k_lights1_cycle:
          if (!held) {
-			if(lights1 ==0){
-				lights1 = 20000;
-				//sub.lights = 1;
-				
-			}
-			else{
-				lights1 = 0;
-				//sub.lights = 0;
-			}
+		 	RC_Channel* chan = RC_Channels::rc_channel(8);
+		 	uint16_t min = chan->get_radio_min();
+            uint16_t max = chan->get_radio_max();
+			 if(lights1 ==min){
+			    lights1 = constrain_float(max, min, max);
+       		 }
+		   else{
+			   lights1 = constrain_float(min, min, max);
+		   }
+			 gcs().send_text(MAV_SEVERITY_INFO,"min %d %d",rov_control.floodlight,lights1);
          }
         break;
     case JSButton::button_function_t::k_lights1_brighter:
@@ -285,6 +287,9 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
 			   lights2 = 0;
 			   sub.usblpoweroff = 0;
 		   }
+
+
+		   
 		}
 
         break;
@@ -320,53 +325,20 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
         break;
     case JSButton::button_function_t::k_gain_inc:
 		{
-        if (!held) {
-			if(control_mode == CLEAN)
-			{
-					
-				turn_angle =-16.0;
-				gcs().send_text(MAV_SEVERITY_INFO,"%f turn_angle",(float)turn_angle);		
-			}else if(control_mode == STABILIZE){
-				if (!motors.armed()){
-						//gcs().send_text(MAV_SEVERITY_INFO,"disarm used by current yaw is %f",(float)yaw_press);
-					}
-				else{
-						
-						//yaw_press +=3;
-						//if(yaw_press>=360){
-						//	yaw_press-=360;
-						//}
-						//gcs().send_text(MAV_SEVERITY_INFO,"%d %f  yaw++",(int)yaw_press,(float)yaw_press);
-					}
-	
-			}
-
+        {
+			
+			updowmgain=0.3;//下
+			delaygain =0;
+			
         }
         break;
 		}
     case JSButton::button_function_t::k_gain_dec:
 		{
 		
-        if (!held) {
-			if(control_mode == CLEAN)
-			{
-					
-				turn_angle =16.0;
-				gcs().send_text(MAV_SEVERITY_INFO,"%f turn_angle",(float)turn_angle);		
-			}else if(control_mode == STABILIZE){
-				if (!motors.armed()){
-					//	gcs().send_text(MAV_SEVERITY_INFO,"disarm used by current yaw is %f",(float)yaw_press);
-					}
-				else{
-						
-					//	yaw_press -=3;
-					//	if(yaw_press<0){
-					//		yaw_press+=360;
-					//	}
-					//	gcs().send_text(MAV_SEVERITY_INFO,"%d %f  yaw--",(int)yaw_press,(float)yaw_press);
-					}
-
-			}
+        {
+			updowmgain=0.7;
+			delaygain =0;
 
         }
         break;

ArduSub/surface_bottom_detector.cpp

@@ -26,16 +26,7 @@ void Sub::update_surface_and_bottom_detector()
     
 	float vel = AC_AttitudeControl::sqrt_controller(cur_alt-last_alt, pos_control._p_pos_z.kP(), pos_control._accel_z_cms/100, pos_control._dt);
 	velocity.z = vel;//12.20
-	static int f = 0;
 
-	f++;
-	if(f>600)
-	{
-	
-		
-		gcs().send_text(MAV_SEVERITY_INFO, " velocity.z %f %f %f ",cur_alt,last_alt,velocity.z);
-		f=0;
-	}
 
 	static uint16_t i=0;
 	uint16_t timecnt = (uint16_t)(0.25/MAIN_LOOP_SECONDS);

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -617,18 +617,7 @@ void AC_PosControl::run_z_controller()
     // send throttle to attitude controller with angle boost
     _attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);
 
-	static int f = 401;
 
-	f++;
-	if(f>400)
-	{
-
-		gcs().send_text(MAV_SEVERITY_INFO, " dt  %f %f  %f",curr_alt,last_alt,_dt);
-		gcs().send_text(MAV_SEVERITY_INFO, " alt  %f %f  %f",_pos_target.z,curr_alt, _pos_error.z);
-		gcs().send_text(MAV_SEVERITY_INFO, " vel  %f  %f  %f",_vel_target.z,curr_vel.z,_vel_error.z);
-		gcs().send_text(MAV_SEVERITY_INFO, " AC  %f  %f  %f  ",_accel_target.z,z_accel_meas,thr_out);
-		f=0;
-	}
 }
 
 ///

libraries/AP_HAL/AP_HAL_Macros.h

@@ -1,7 +1,7 @@
 #pragma once
 
 #include <AP_HAL/AP_HAL_Boards.h>
-
+#include <math.h>
 /*
   macros to allow code to build on multiple platforms more easily
  */