更改了 位置速度计算 上升下潜 stablize有深度保持 前进后退改为地球坐标系 pitch速度提高

ArduSub/ArduSub.cpp

@@ -111,6 +111,7 @@ void Sub::fast_loop()
         // run low level rate controllers that only require IMU data
         attitude_control.rate_controller_run();
     }
+	
 
     // send outputs to the motors library
     motors_output();
@@ -118,8 +119,6 @@ void Sub::fast_loop()
     // run EKF state estimator (expensive)
     // --------------------
     read_AHRS();
-	
-
 
     // Inertial Nav
     // --------------------
@@ -129,6 +128,8 @@ void Sub::fast_loop()
     check_ekf_yaw_reset();
 
     // run the attitude controllers
+    
+
     update_flight_mode();
 
 	//motor_toCan();
@@ -339,6 +340,7 @@ void Sub::update_altitude()
 {
     // read in baro altitude
     read_barometer();
+	pos_control.calculate_rate(0.1);
 
     if (should_log(MASK_LOG_CTUN)) {
         Log_Write_Control_Tuning();

ArduSub/Attitude.cpp

@@ -29,6 +29,7 @@ void Sub::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &ro
     // return
     roll_out = roll_in;
     pitch_out = pitch_in;
+
 }
 
 // get_pilot_desired_heading - transform pilot's yaw input into a

ArduSub/Sub.cpp

@@ -48,6 +48,14 @@ Sub::Sub()
     sensor_health.compass = true;
 	lights1 = 0;
 	updowmgain = 0.5;
+	stable_alt_control = TRUE;
+	rotyaw.identity();
+	rotpitch.identity();
+	delaygain = 401;
+	velocity_z_filer =0.0;
+	continuous_count=0;
+	continuous_countdec=0;
+	last_continuous = 0;
 
 #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
     failsafe.pilot_input = true;

ArduSub/Sub.h

@@ -114,6 +114,7 @@
 #define backward  2
 #define Speedmax 60
 
+#define STARTCONT 2000
 
 //压力等级 包含下压和上浮总共10级 第五级为1500 即中值
 enum PressNetLevel {
@@ -853,6 +854,18 @@ public:
 
 	int16_t pitch_input_inc;//pitch给定
 	int16_t lights1;
+	bool  stable_alt_control;
+	uint16_t continuous_count;
+	uint16_t continuous_countdec;
+	uint8_t last_continuous;
+	float velocity_z_filer;
+
+
+	bool  bottom_set;
+
+	Matrix3f rotpitch;
+	Matrix3f rotyaw;
+	
 
 	 struct telemetry_info_t {
 	 	int16_t dutycycle;
@@ -885,7 +898,18 @@ public:
 	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);
+	bool throttle_continuous(float gain1);
+	float surface_depth_use(void);
+
+	float gaindelay(float gain1);
+	void altcontrol_ff();
+	void altcontrol();
+	void ratecontrol();
+	float bottom_detector(float z);
+	void bottom_detectorgain(void);
+	void get_heading_to_ned(Matrix3f &mat_half,Matrix3f mat_body);
+	float get_pitch_to_ned(Matrix3f mat_body);
+	float sign_in(float f);
 };
 
 extern const AP_HAL::HAL& hal;

ArduSub/UserCode.cpp

@@ -1,6 +1,7 @@
 #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;
@@ -144,27 +145,10 @@ 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;
+		temp = -0.2;
 	}else if(data>=-0.8 && data<-0.6 )
 	{
 		temp = -0.5;
@@ -174,7 +158,7 @@ float Sub::getgainf(float data){
 	}
 	else if (data<=0.6 && data >0.08)
 	{
-		temp = 0.3;
+		temp = 0.2;
 	}else if(data<=0.8 && data>0.6 )
 	{
 		temp = 0.5;
@@ -186,21 +170,267 @@ float Sub::getgainf(float data){
 	}
 	return temp;
 }
-float Sub::gaindelay(float gain1,float _gain){
-	
-	if (fabsf(gain1 -0.5)<0.05)
+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 \n",(int)continuous_count,(int)last_continuous,(int)continuous_countdec);
+
+	 }
+
+		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;
-			_gain = 0.5;
+			
+			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 _gain;
+		return result;
     }else{
-		return gain1;
-    }
+		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;
+}
+
+
 

ArduSub/control_manual.cpp

@@ -39,9 +39,10 @@ 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(gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain));//0-0.5下压
+	float tt = gaindelay(1.0-(float)PressLevel_f*0.1);
+	motors.set_throttle(tt);//0-0.5下压
 	static int j = 0;
-	float tt = gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain);
+	
 	  j++;
 	  if(j>300)
 	  {

ArduSub/control_sport .cpp

@@ -0,0 +1,392 @@
+#include "Sub.h"
+//#include "/AP_Math/AP_Math.h"
+#define PI	3.141592653589793238462643383279502884
+
+extern mavlink_rov_state_monitoring_t rov_message;
+Quaternion attitude_desired_quat_;
+//Matrix3f last_desired;
+
+
+/*
+ * control_althold.pde - init and run calls for althold, flight mode
+ */
+
+// althold_init - initialise althold controller
+bool Sub::sport_init()
+{
+
+	attitude_control.set_throttle_out(0.5 ,true, g.throttle_filt);
+	//holding_depth = false;
+	//检查是否有深度计存在
+    if(!control_check_barometer()) {
+        return false;
+    }
+
+    // initialize vertical speeds and leash lengths
+    // sets the maximum speed up and down returned by position controller
+    //get_pilot_speed_dn QGC PILOT 设置的值
+    pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);//(-500,500)设置最大最小速度 和刹车长度
+    pos_control.set_max_accel_z(g.pilot_accel_z);//设置最大加速度值 和刹车长度
+    pos_control.relax_alt_hold_controllers();//目标高度为当前高度，目标速度和上次速度 当前速度
+	pos_control.reset_I();
+    //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);//当前深度12.19
+    holding_depth = true;
+	//ahrs.get_quat_body_to_ned(attitude_desired_quat_);//12.19 使用roll pitch =0的姿态
+
+    
+	attitude_desired_quat_.from_euler(0.0, 0.0,ahrs.yaw);//初始化
+	rotpitch.identity();
+	 attitude_desired_quat_.rotation_matrix(rotyaw);
+	
+    last_roll = 0;
+    last_pitch = 0;
+    
+	updowmgain =0.5;
+	delaygain = 401;
+	
+	continuous_count=0;
+	continuous_countdec=0;
+	last_continuous = 0;
+
+
+
+    last_yaw = ahrs.yaw_sensor;
+    last_input_ms = AP_HAL::millis();
+	last_input_ms_stable = AP_HAL::millis();
+	//updown_gain = 0.0;
+	
+	
+    return true;
+}
+
+void Sub::handle_attitude_sport(){
+	uint32_t tnow = AP_HAL::millis();
+
+    // get pilot desired lean angles
+    float target_roll, target_pitch, target_yaw;
+
+    // Check if set_attitude_target_no_gps is valid
+    if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
+        Quaternion(
+            set_attitude_target_no_gps.packet.q
+        ).to_euler(
+            target_roll,
+            target_pitch,
+            target_yaw
+        );
+        target_roll = 100 * degrees(target_roll);
+        target_pitch = 100 * degrees(target_pitch);
+        target_yaw = 100 * degrees(target_yaw);
+        last_roll = target_roll;
+        last_pitch = target_pitch;
+        last_yaw = target_yaw;
+        
+        attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, target_yaw, true);
+    } else {
+        float throttle = 2*(0.5-channel_throttle->norm_input());
+		float yaw = channel_yaw->norm_input();
+		if (fabsf(throttle)>=fabsf(yaw))
+		{
+			yaw = 0.0;
+		}else{
+			throttle = 0.0;
+			yaw = channel_yaw->get_control_in()*0.35;
+		}
+		get_pilot_desired_lean_angles(0, (int16_t)(throttle/1.5*5700), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
+
+		target_yaw = get_pilot_desired_yaw_rate(yaw);//*0.6 变慢
+		
+        if ( abs(target_pitch) > 300) {
+         
+            last_input_ms = tnow;
+            attitude_control.input_rate_bf_roll_pitch_yaw(0, target_pitch, 0);
+			
+			//ahrs.get_quat_body_to_ned(attitude_desired_quat_);
+			
+		
+			Matrix3f mat_body = ahrs.get_rotation_body_to_ned();//03.17 
+			float pitch_ang = get_pitch_to_ned(mat_body);
+			
+			rotpitch.from_euler(0,pitch_ang,0);//pitch 旋转矩阵
+			
+
+			
+        } else if (abs(target_yaw) > 300) {
+			
+            // if only yaw is being controlled, don't update pitch and roll
+            attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, target_yaw);
+			
+			Matrix3f mat_body = ahrs.get_rotation_body_to_ned();//03.17
+			get_heading_to_ned(rotyaw,mat_body);//yaw旋转矩阵
+			
+			
+			
+			//ahrs.get_quat_body_to_ned(attitude_desired_quat_);
+            
+            last_input_ms = tnow;
+        } else if (tnow < last_input_ms + 250) {
+			
+            // just brake for a few mooments so we don't bounce
+            Matrix3f mat_body = ahrs.get_rotation_body_to_ned();//03.17
+			get_heading_to_ned(rotyaw,mat_body);//yaw旋转矩阵
+			
+            attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, 0);
+        } else {
+            // Lock attitude
+			
+			float depth = barometer.get_altitude()*100; // cm
+			if(fabsf(depth)<50){//水面附近
+				float maxsinpitch = constrain_float(fabsf(depth)/50.0,0.0,1.0);//40深度传感器到体长+10 余量 可以允许的最大pitch角的绝对值
+				float nowsinpitch = rotpitch.a.z;//当前的pitch角
+				Matrix3f rotpitch_limit;
+				if (fabsf(nowsinpitch)>maxsinpitch)//说明比允许的大 姿态应该是恢复水平一些
+				{
+					float pitch_ang = get_pitch_to_ned(rotpitch);//目前的pith
+					if (fabsf(pitch_ang)<PI/2)
+					{
+						pitch_ang = asinf(maxsinpitch);
+					}else{
+						pitch_ang = (PI- asinf(maxsinpitch))*sign_in(pitch_ang);
+					}
+					rotpitch_limit .from_euler(0,pitch_ang,0);//pitch 旋转矩阵
+				}else{//保持原来的姿态
+					rotpitch_limit = rotpitch;//pitch 旋转矩阵
+				}
+
+
+				attitude_desired_quat_.from_rotation_matrix(rotyaw*rotpitch_limit);
+				static uint16_t count = 0;
+				 count++;
+				 if (count>400)
+				{
+					 count = 0;
+					 gcs().send_text(MAV_SEVERITY_INFO, "ahr %f %f %f \n",nowsinpitch,maxsinpitch,get_pitch_to_ned(rotpitch_limit));
+				 }
+			}else{
+				attitude_desired_quat_.from_rotation_matrix(rotyaw*rotpitch);
+			}
+			attitude_control.input_quaternion(attitude_desired_quat_);
+
+			//attitude_desired_quat_.from_rotation_matrix(rotyaw*rotpitch);
+			//attitude_control.input_quaternion(attitude_desired_quat_);
+
+        }
+
+    }
+
+
+}
+
+void Sub::sport_run_alt(void){
+	 
+	
+	 // When unarmed, disable motors and stabilization
+	 if (!motors.armed()) {
+	
+		 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);
+		 attitude_control.relax_attitude_controllers();
+		 pos_control.relax_alt_hold_controllers();
+		 pos_control.reset_I();
+		 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;
+		 
+		 rotpitch.identity();
+		attitude_desired_quat_.from_euler(0.0, 0.0,ahrs.yaw);//初始化
+		attitude_desired_quat_.rotation_matrix(rotyaw);
+
+		delaygain = 401;
+	
+		continuous_count=0;
+		continuous_countdec=0;
+		last_continuous = 0;
+	
+		 return;
+	 }
+
+	 motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); 
+	 handle_attitude_sport();
+	 //pos_control.update_z_controller();//输出了set_throttle_out
+	Matrix3f mat_body = ahrs.get_rotation_body_to_ned();
+	 Matrix3f mat_half;
+	 get_heading_to_ned(mat_half,mat_body);//得到half状态
+	// mat 地到half状态
+	 Matrix3f rc_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed()*mat_half;//rc_vehicle_frame（half）到body
+	 //防止手柄交叉
+	   float forward = channel_forward->norm_input();
+	   float lateral =channel_lateral->norm_input();
+	   if (fabsf(forward)>=fabsf(lateral))
+		 {
+		   lateral = 0.0;
+		  }else{
+			forward = 0.0;
+		  }
+		  //手柄转到机体系------------------------
+	 float joystick_z = 2*(gaindelay(1.0-(float)PressLevel_f*0.1)-0.5);//-1~1
+	
+	  Vector3f rc_throttle_forward = Vector3f(getgainf(forward),0, 0);//手柄的值
+	  Vector3f rc_throttle_lateral = Vector3f(0, getgainf(lateral), 0);//手柄的值
+	  Vector3f rc_throttle = Vector3f(0, 0, joystick_z);//手柄的值
+	  
+
+	  
+	   Vector3f rc_throttle_vehicle_frame1 = rc_vehicle_frame*rc_throttle_forward;//手柄从half系到body系
+	   Vector3f rc_throttle_vehicle_frame2 = rc_vehicle_frame*rc_throttle_lateral;//手柄从half系到body系
+	   Vector3f rc_throttle_vehicle_frame3 = rc_vehicle_frame*rc_throttle;//手柄从half系到body系
+	  
+	   Vector3f rc_throttle_vehicle_frame;
+	   rc_throttle_vehicle_frame.x = rc_throttle_vehicle_frame1.x + rc_throttle_vehicle_frame2.x - rc_throttle_vehicle_frame3.x;
+	   rc_throttle_vehicle_frame.y = rc_throttle_vehicle_frame1.y + rc_throttle_vehicle_frame2.y - rc_throttle_vehicle_frame3.y;
+	   rc_throttle_vehicle_frame.z = -rc_throttle_vehicle_frame1.z - rc_throttle_vehicle_frame2.z + rc_throttle_vehicle_frame3.z;
+//----------------------------------------------------
+
+	 // Read the output of the z controller and rotate it so it always points up
+	 //深度保持控制
+	 float z_throttle = pos_control.update_z_controller_f();//输出了垂直变化量
+//将深度保持控制的对地系转到机体系
+	 Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, z_throttle);//变到机体坐标系
+
+	
+	//手柄控制和深度保持的Z轴输出的叠加  
+
+	 motors.set_forward(-throttle_vehicle_frame.x +rc_throttle_vehicle_frame.x);
+	 motors.set_lateral(-throttle_vehicle_frame.y +rc_throttle_vehicle_frame.y);
+	 motors.set_throttle(throttle_vehicle_frame.z +rc_throttle_vehicle_frame.z/2+0.5);
+
+	 
+
+	 rov_message.pressure_level = int(PressLevel);
+ 
+	 Vector3f earth_frame_rc_inputs = Vector3f(0.0, 0.0, joystick_z);//仅仅与竖直有关 手柄是在half系 Z轴
+	  
+
+	 bottom_detectorgain();
+
+	
+	 if (fabsf(earth_frame_rc_inputs.z) > 0.05f) { // Throttle input  above 5%
+		// reset z targets to current values
+		 holding_depth = false;
+		 pos_control.relax_alt_hold_controllers();
+		 
+		 
+	
+	 } else { // hold z
+		 if (ap.at_surface) {	 //最大油门不能向上动
+			 
+			 pos_control.set_alt_target(surface_depth_use()*100 - 10.0f); // set target to 5cm below surface level
+			 
+			 holding_depth = true;
+			 
+	
+		 } else if (ap.at_bottom||bottom_set) {//最大油门不能向下动
+			 //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 +10);//cm
+			 holding_depth = true;
+			 
+	
+		 } else if (!holding_depth) {
+			
+			 pos_control.calc_leash_length_z();//刹车长度 12.19
+			// pos_control.set_target_to_stopping_point_z();//有对位置的付值
+			 pos_control.set_alt_target(barometer.get_altitude()*100);//cm 当前深度
+	
+			 holding_depth = true;
+		   
+		 }
+	 }
+	 	static bool lastdepth = holding_depth;
+	if(lastdepth !=holding_depth){
+		lastdepth = holding_depth;
+		
+		
+		gcs().send_text(MAV_SEVERITY_INFO, "get_alt %d %f %f\n",(int)holding_depth,pos_control.get_alt_target(),barometer.get_altitude()*100);
+	
+	}
+	 static uint16_t count = 0;
+	 count++;
+	 if (count>400)
+		 {
+		 count = 0;
+		 gcs().send_text(MAV_SEVERITY_INFO, "surf %d %d %d %f\n",(int)ap.at_surface,(int)ap.at_bottom,(int)motors.limit.throttle_upper,(float)motors.get_throttle());
+		  
+		 gcs().send_text(MAV_SEVERITY_INFO, "alt %f %f %f %f\n",pos_control.get_alt_target(),barometer.get_altitude()*100,pos_control.alt_rate.get(),joystick_z);
+
+	 }
+
+}
+
+void Sub::altcontrol(){//相对机体系的手柄控制
+	//深度控制
+	 float z_throttle = pos_control.update_z_controller_f();//输出了set_throttle_out
+	//变到机体坐标系
+	 Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, z_throttle);	
+	// Output the Z controller + pilot input to all motors.
+	float z_throttle_joystic = gaindelay(1.0-(float)PressLevel_f*0.1);//0~1
+	 motors.set_throttle(throttle_vehicle_frame.z + z_throttle_joystic);//Z轴油门
+	
+	   rov_message.pressure_level = int(PressLevel);
+	  //防止交叉
+	  float forward = channel_forward->norm_input();
+	  float lateral =channel_lateral->norm_input();
+	  if (fabsf(forward)>=fabsf(lateral))
+		{
+		  lateral = 0.0;
+		 }else{
+		   forward = 0.0;
+		 }
+	 //变换到集体轴
+	   motors.set_forward(constrain_float(-throttle_vehicle_frame.x + getgainf(forward),-0.8,0.8));//*0.6 变慢
+	   motors.set_lateral(constrain_float(-throttle_vehicle_frame.y + getgainf(lateral),-0.8,0.8));//*0.6 变慢
+	
+	   Vector3f earth_frame_rc_inputs1 = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), 0.0, (z_throttle_joystic-0.5)*2);//去掉侧移影响
+	   bottom_detectorgain();
+	
+	   if (fabsf(earth_frame_rc_inputs1.z) > 0.05f) { // Throttle input  above 5%
+		  // reset z targets to current values
+		   holding_depth = false;
+		   pos_control.relax_alt_hold_controllers();
+		   
+	   } else { // hold z
+		  	if (ap.at_surface) {    //最大油门不能向上动
+
+			 	pos_control.set_alt_target(surface_depth_use()*100 - 10.0f); // set target to 5cm below surface level
+				 
+			   	holding_depth = true;
+	  		} else if (ap.at_bottom||bottom_set) {//最大油门不能向下动
+			   pos_control.set_alt_target(barometer.get_altitude()*100+10);//cm
+			   holding_depth = true;
+			   
+		   	} else if (!holding_depth) {
+				pos_control.calc_leash_length_z();//刹车长度 12.19
+				pos_control.set_alt_target(barometer.get_altitude()*100);//cm 当前深度
+				 holding_depth = true;
+		   	}
+	   }
+	   static bool lastdepth = holding_depth;
+	   if(lastdepth !=holding_depth){
+		   lastdepth = holding_depth;
+		   gcs().send_text(MAV_SEVERITY_INFO, " get_alt_target %d  %f  \n",(int)holding_depth,motors.get_throttle());
+	   }
+	
+	   static uint16_t count = 0;
+	   count++;
+	   if (count>300)
+		   {
+		    count = 0;
+		     gcs().send_text(MAV_SEVERITY_INFO, " surface %d %d %d\n",(int)ap.at_surface,(int)ap.at_bottom,(int)motors.limit.throttle_upper);
+		     gcs().send_text(MAV_SEVERITY_INFO, " altitude %f %f %f\n",pos_control.get_alt_target(),barometer.get_altitude()*100,motors.get_throttle());
+	
+		   }
+
+}
+
+
+

ArduSub/control_sport.cak

@@ -0,0 +1,576 @@
+#include "Sub.h"
+//#include "/AP_Math/AP_Math.h"
+
+extern mavlink_rov_state_monitoring_t rov_message;
+Quaternion attitude_desired_quat_;
+
+
+/*
+ * control_althold.pde - init and run calls for althold, flight mode
+ */
+
+// althold_init - initialise althold controller
+bool Sub::sport_init()
+{
+
+	attitude_control.set_throttle_out(0.5 ,true, g.throttle_filt);
+	//holding_depth = false;
+	//检查是否有深度计存在
+    if(!control_check_barometer()) {
+        return false;
+    }
+
+    // initialize vertical speeds and leash lengths
+    // sets the maximum speed up and down returned by position controller
+    //get_pilot_speed_dn QGC PILOT 设置的值
+    pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);//(-500,500)设置最大最小速度 和刹车长度
+    pos_control.set_max_accel_z(g.pilot_accel_z);//设置最大加速度值 和刹车长度
+    pos_control.relax_alt_hold_controllers();//目标高度为当前高度，目标速度和上次速度 当前速度
+	//
+    //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);//当前深度12.19
+    holding_depth = true;
+	ahrs.get_quat_body_to_ned(attitude_desired_quat_);//12.19
+
+    
+    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();
+	
+	
+	
+    return true;
+}
+
+void Sub::handle_attitude_sport(){
+	uint32_t tnow = AP_HAL::millis();
+
+    // get pilot desired lean angles
+    float target_roll, target_pitch, target_yaw;
+
+    // Check if set_attitude_target_no_gps is valid
+    if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
+        Quaternion(
+            set_attitude_target_no_gps.packet.q
+        ).to_euler(
+            target_roll,
+            target_pitch,
+            target_yaw
+        );
+        target_roll = 100 * degrees(target_roll);
+        target_pitch = 100 * degrees(target_pitch);
+        target_yaw = 100 * degrees(target_yaw);
+        last_roll = target_roll;
+        last_pitch = target_pitch;
+        last_yaw = target_yaw;
+        
+        attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, target_yaw, true);
+    } else {
+        // 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());
+		float throttle = 2*(0.5-channel_throttle->norm_input());
+		float yaw = channel_yaw->norm_input();
+		if (fabsf(throttle)>=fabsf(yaw))
+		{
+			yaw = 0.0;
+		}else{
+			throttle = 0.0;
+			yaw = channel_yaw->get_control_in()*0.3;
+		}
+		get_pilot_desired_lean_angles(0, (int16_t)(throttle/2.0*5700), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
+
+		target_yaw = get_pilot_desired_yaw_rate(yaw);//*0.6 变慢
+		
+        if (abs(target_roll) > 300 || abs(target_pitch) > 300) {
+            //last_roll = ahrs.roll_sensor;
+            //last_pitch = ahrs.pitch_sensor;
+            //last_yaw = ahrs.yaw_sensor;
+             ahrs.get_quat_body_to_ned(attitude_desired_quat_);
+            last_input_ms = tnow;
+            attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
+        } else if (abs(target_yaw) > 300) {
+            // if only yaw is being controlled, don't update pitch and roll
+            attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, target_yaw);
+            //last_yaw = ahrs.yaw_sensor;
+            ahrs.get_quat_body_to_ned(attitude_desired_quat_);
+            last_input_ms = tnow;
+        } else if (tnow < last_input_ms + 250) {
+            // just brake for a few mooments so we don't bounce
+            last_yaw = ahrs.yaw_sensor;
+            attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, 0);
+        } else {
+            // Lock attitude
+            //attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_yaw, true);
+            attitude_control.input_quaternion(attitude_desired_quat_);
+        }
+
+    }
+
+
+}
+
+void Sub::sport_run_alt(void){
+	 
+	
+	 // When unarmed, disable motors and stabilization
+	 if (!motors.armed()) {
+	
+		 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);
+		 attitude_control.relax_attitude_controllers();
+		 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
+		 
+	
+		 return;
+	 }
+
+	 motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); 
+	 handle_attitude_sport();
+	 //pos_control.update_z_controller();//输出了set_throttle_out
+	 Matrix3f mat_body = ahrs.get_rotation_body_to_ned();
+	 Vector3f Zb = mat_body*Vector3f(0,0,1);
+	 Vector3f Ze = Vector3f(0,0,1);
+	 float thetaz = Zb*Ze;//计算有没有翻过来
+	 Matrix3f mat_half;
+
+	 if(mat_body.c.x<0.98 && mat_body.c.x>-0.98){//大约78.5度认为Z轴指向方向，小于78.5进入X轴指向方向
+
+		 float beta = 0.0;	
+		 beta = safe_sqrt(1.0-mat_body.c.x*mat_body.c.x);
+		 if (thetaz<0)//翻过来
+		{
+			 beta =-beta;
+		}
+		 mat_half.a.x= mat_body.a.x/beta;
+		 mat_half.a.y=-mat_body.b.x/beta;
+		 mat_half.a.z=0.0;	
+		 mat_half.b.x= mat_body.b.x/beta;
+		 mat_half.b.y= mat_body.a.x/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 Xb = mat_body*Vector3f(0,0,1);
+		Xb.z = 0.0;
+		Vector3f Xe = Vector3f(1,0,0);
+		float alphacos = Xb * Xe/Xb.length();
+		float beta = safe_sqrt(1.0-alphacos*alphacos);
+		 if (Xb.y<0)
+		{
+			 beta =-beta;
+		}
+		 mat_half.a.x= alphacos;
+		 mat_half.a.y=-beta;
+		 mat_half.a.z=0.0;	
+		 mat_half.b.x= beta;
+		 mat_half.b.y=alphacos;
+		 mat_half.b.z=0.0;
+		 mat_half.c.x= 0.0;
+		 mat_half.c.y= 0.0;
+		 mat_half.c.z=1.0;
+	 }
+	// mat 地到half状态
+	
+	 Matrix3f rc_vehicle_frame = mat_half*ahrs.get_rotation_body_to_ned().transposed();//rc_vehicle_frame（half）到body
+
+	
+	 
+		   float forward = channel_forward->norm_input();
+		   float lateral =channel_lateral->norm_input();
+		   if (fabsf(forward)>=fabsf(lateral))
+			 {
+			   lateral = 0.0;
+			  }else{
+				forward = 0.0;
+			  }
+	 
+	Vector3f rc_throttle = Vector3f(getgainf(forward), getgainf(lateral), 2*(gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5));//手柄的值
+	 
+
+	 Vector3f rc_throttle_vehicle_frame = rc_vehicle_frame*rc_throttle;//手柄从half系到body系
+
+	 static uint16_t count2 = 0;
+	 count2++;
+	 if (count2>200)
+		{
+		 count2 = 0;
+		  gcs().send_text(MAV_SEVERITY_INFO, " rc_throttle %f %f %f\n",rc_throttle.x,rc_throttle.y,rc_throttle.z);
+	// gcs().send_text(MAV_SEVERITY_INFO, " mata %f %f %f\n",mat_half.a.x,mat_half.a.y,mat_half.a.z);
+//		 gcs().send_text(MAV_SEVERITY_INFO, " matb %f %f %f\n",mat_half.b.x,mat_half.b.y,mat_half.b.z);
+//		 gcs().send_text(MAV_SEVERITY_INFO, " matc %f %f %f\n",mat_half.c.x,mat_half.c.y,mat_half.c.z);
+//		 Matrix3f matx = ahrs.get_rotation_body_to_ned();
+
+//		 gcs().send_text(MAV_SEVERITY_INFO, " ahrsa %f %f %f\n",matx.a.x,matx.a.y,matx.a.z);
+	//	 gcs().send_text(MAV_SEVERITY_INFO, " ahrsb %f %f %f\n",matx.b.x,matx.b.y,matx.b.z);
+	//	 gcs().send_text(MAV_SEVERITY_INFO, " ahrsc %f %f %f\n",matx.c.x,matx.c.y,matx.c.z);
+	 
+		 }	
+	 // Read the output of the z controller and rotate it so it always points up
+	 //将Z轴输出变到机体坐标系
+	 //Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, motors.get_throttle_in_bidirectional());
+	 float z_throttle = pos_control.update_z_controller_f();//输出了set_throttle_out
+
+	 Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, z_throttle);
+ 
+	
+	//手柄控制和Z轴输出的叠加  
+
+	 motors.set_forward(-throttle_vehicle_frame.x +rc_throttle_vehicle_frame.x);
+	 motors.set_lateral(-throttle_vehicle_frame.y +rc_throttle_vehicle_frame.y);//*0.6 变慢
+	 motors.set_throttle(throttle_vehicle_frame.z +rc_throttle_vehicle_frame.z/2+0.5);
+	 rov_message.pressure_level = int(PressLevel);
+	static uint16_t count3 = 0;
+	 count3++;
+	 if (count3>200)
+		{
+		 count3 = 0;
+		 gcs().send_text(MAV_SEVERITY_INFO, " rc_frame %f %f %f\n",z_throttle,throttle_vehicle_frame.z);
+		
+		// gcs().send_text(MAV_SEVERITY_INFO, " rc_frame %f %f %f\n",rc_vehicle_frame.a.x,rc_vehicle_frame.a.y,rc_vehicle_frame.a.z);
+		// gcs().send_text(MAV_SEVERITY_INFO, " rc_frame %f %f %f\n",rc_vehicle_frame.b.x,rc_vehicle_frame.b.y,rc_vehicle_frame.b.z);
+		// gcs().send_text(MAV_SEVERITY_INFO, " rc_frame %f %f %f\n",rc_vehicle_frame.c.x,rc_vehicle_frame.c.y,rc_vehicle_frame.c.z);
+		 //gcs().send_text(MAV_SEVERITY_INFO, " rc_throttle %f %f %f\n",rc_throttle.x,rc_throttle.y,rc_throttle.z);
+		// gcs().send_text(MAV_SEVERITY_INFO, " rc %f %f %f\n",rc_throttle_vehicle_frame.x,rc_throttle_vehicle_frame.y,rc_throttle_vehicle_frame.z);
+
+	 
+		 }	
+     Vector3f earth_frame_rc_inputs =  Vector3f(0.0, 0.0, 2*(gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5));//去掉侧移影响
+
+
+	
+	 if (fabsf(earth_frame_rc_inputs.z) > 0.05f) { // Throttle input  above 5%
+		// reset z targets to current values
+		 holding_depth = false;
+		 pos_control.relax_alt_hold_controllers();
+		 
+		 
+	
+	 } 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;
+			 
+	
+		 } 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;
+			 
+	
+		 } 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 当前深度
+	
+			 holding_depth = true;
+		   
+		 }
+	 }
+	 	static bool lastdepth = holding_depth;
+	if(lastdepth !=holding_depth){
+		lastdepth = holding_depth;
+		
+		
+		gcs().send_text(MAV_SEVERITY_INFO, " get_alt_target	%d	%f \n",(int)holding_depth,motors.get_throttle());
+	
+	}
+	 static uint16_t count = 0;
+	 count++;
+	 if (count>200)
+		 {
+		 count = 0;
+		 
+		 gcs().send_text(MAV_SEVERITY_INFO, " altitude %f %f %f \n",pos_control.get_alt_target(),barometer.get_altitude()*100,motors.get_throttle());
+	 	 gcs().send_text(MAV_SEVERITY_INFO, " surface %d %d\n",(int)ap.at_surface,(int)ap.at_bottom);
+		 }
+
+}
+void Sub::altcontrol_ff(){
+
+		float speedclimb =SRV_Channels::srv_channel(15)->get_output_min()/10;//test
+	
+		 float target_climb_rate = ((altdelay(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5)*speedclimb);
+	
+		target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
+	
+		Vector3f earth_frame_rc_inputs1 = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), 0.0, 2*(gaindelay2(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5));//去掉侧移影响
+		rov_message.pressure_level = int(PressLevel);
+		if (fabsf(earth_frame_rc_inputs1.z) > 0.05f) { // Throttle input  above 5%
+		   // reset z targets to current values
+			pos_control.relax_alt_hold_controllers();	
+		} else { 
+			if(ap.at_surface && target_climb_rate>0.0){
+				pos_control.relax_part_hold_controllers(); 
+				pos_control.set_alt_target(g.surface_depth - 10.0f); // set target to 10cm below surface level
+	
+			} else if (ap.at_bottom && target_climb_rate<0.0) {
+				pos_control.relax_part_hold_controllers(); // clear ff control
+				pos_control.set_alt_target(barometer.get_altitude()*100); // set target to current
+			} else {
+				pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);//前馈控制 生成目标深度
+			}
+		}
+		 float z_throttle = pos_control.update_z_controller_f();//输出了set_throttle_out
+		 Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, z_throttle);//转到机体系
+		 motors.set_throttle(throttle_vehicle_frame.z+gaindelay2(1.0-(float)PressLevel_f*0.1,updowmgain));//z轴付值
+		 static uint16_t count = 0;
+		count++;
+		if (count>300)
+		{
+			count = 0;
+			gcs().send_text(MAV_SEVERITY_INFO, "climb_rate %f %f %f %f\n",target_climb_rate,throttle_vehicle_frame.z,updowmgain,speedclimb);
+			gcs().send_text(MAV_SEVERITY_INFO, " altitude %f %f \n",pos_control.get_alt_target(),barometer.get_altitude()*100);
+	
+		}
+		 // 前进侧移付值
+		 float forward = channel_forward->norm_input();
+		 float lateral =channel_lateral->norm_input();
+		if (fabsf(forward)>=fabsf(lateral))
+		 {
+		   lateral = 0.0;
+		  }else{
+			forward = 0.0;
+		  }
+		motors.set_forward(constrain_float(-throttle_vehicle_frame.x + getgainf(forward),-0.8,0.8));//	*0.6 变慢
+		motors.set_lateral(constrain_float(-throttle_vehicle_frame.y + getgainf(lateral),-0.8,0.8));// *0.6 变慢
+
+}
+void Sub::altcontrol(){
+
+	 float z_throttle = pos_control.update_z_controller_f();//输出了set_throttle_out
+	   // Read the output of the z controller and rotate it so it always points up
+	   //变到机体坐标系
+	 Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, z_throttle);	
+	   // Output the Z controller + pilot input to all motors.
+	 motors.set_throttle(throttle_vehicle_frame.z + gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain));
+	
+		static uint16_t j=0;
+	  j++;
+	  if(j>300)
+	  {
+		gcs().send_text(MAV_SEVERITY_INFO, " sportthrottle %f %f  \n",z_throttle,throttle_vehicle_frame.z);
+		gcs().send_text(MAV_SEVERITY_INFO, " throttle %f %f\n",gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain),motors.get_throttle());
+	  
+		 j=0;
+	  }
+	   rov_message.pressure_level = int(PressLevel);
+	  float forward = channel_forward->norm_input();
+	  float lateral =channel_lateral->norm_input();
+	  if (fabsf(forward)>=fabsf(lateral))
+		{
+		  lateral = 0.0;
+		 }else{
+		   forward = 0.0;
+		 }
+	   motors.set_forward(constrain_float(-throttle_vehicle_frame.x + getgainf(forward),-0.8,0.8));//*0.6 变慢
+	   motors.set_lateral(constrain_float(-throttle_vehicle_frame.y + getgainf(lateral),-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_inputs1 = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), 0.0, 2*(gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5));//去掉侧移影响
+	   //Vector3f earth_frame_rc_inputs2 = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), 0.0, motors.get_throttle_bidirectional());//去掉侧移影响
+	
+	
+	   if (fabsf(earth_frame_rc_inputs1.z) > 0.05f) { // Throttle input  above 5%
+		  // reset z targets to current values
+		   holding_depth = false;
+		   pos_control.relax_alt_hold_controllers();
+		 
+		   //pos_control.set_alt_target(barometer.get_altitude()*100);//当前深度
+		   
+	   } 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;
+	  		} 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);//cm
+			   holding_depth = true;
+			   
+		   	} else if (!holding_depth) {
+				pos_control.calc_leash_length_z();//刹车长度 12.19
+				pos_control.set_alt_target(barometer.get_altitude()*100);//cm 当前深度
+				 holding_depth = true;
+		   	}
+	   }
+	   static bool lastdepth = holding_depth;
+	   if(lastdepth !=holding_depth){
+		   lastdepth = holding_depth;
+		   
+	   gcs().send_text(MAV_SEVERITY_INFO, " surface %d %d\n",(int)ap.at_surface,(int)ap.at_bottom);
+		   gcs().send_text(MAV_SEVERITY_INFO, " get_alt_target %d  %f %f \n",(int)holding_depth,gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain),motors.get_throttle());
+	   }
+	
+	   static uint16_t count = 0;
+	   count++;
+	   if (count>300)
+		   {
+		   count = 0;
+		   
+	
+		   gcs().send_text(MAV_SEVERITY_INFO, " altitude %f %f \n",pos_control.get_alt_target(),barometer.get_altitude()*100);
+	
+		   }
+
+}
+void Sub::ratecontrol(){
+
+	float speedclimb =SRV_Channels::srv_channel(15)->get_output_min()/10;//test
+	
+	 float target_climb_rate = ((altdelay(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5)*speedclimb);
+	
+	target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
+	
+	uint32_t tnow = AP_HAL::millis();
+	float alt = 0.0;
+	static uint32_t lasttime = tnow;
+	static uint8_t alt_flag = 0;
+	bool alt_flag2 = false;
+	Vector3f earth_frame_rc_inputs1 = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), 0.0, 2*(gaindelay2(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5));//压力分级
+	rov_message.pressure_level = int(PressLevel);
+	if (fabsf(earth_frame_rc_inputs1.z) > 0.05f) { // Throttle input  above 5%
+	   // reset z targets to current values
+		pos_control.relax_alt_hold_controllers();	
+	    holding_depth = false;
+		alt_flag = 0;
+	} else { //没有压力分级的时候
+		if(ap.at_surface ){
+			
+			pos_control.set_alt_target(g.surface_depth - 10.0f); // set target to 10cm below surface level
+			holding_depth = true;
+
+		} else if (ap.at_bottom ) {
+			
+			pos_control.set_alt_target(barometer.get_altitude()*100+10); // set target to current
+			holding_depth = true;
+		} else {
+			
+			if (!holding_depth)
+			{
+				holding_depth = true;
+				pos_control.set_alt_target(barometer.get_altitude()*100); // set target to current
+			}
+
+			
+		}
+		if (fabsf(target_climb_rate)>0.0 )
+		{
+			alt_flag = 1;
+			alt =pos_control.run_z_controller_wangdan(target_climb_rate);
+			lasttime = tnow;
+			alt_flag2 =true;
+		}else if(tnow-lasttime<300){
+			alt_flag2 =true;
+			alt =pos_control.run_z_controller_wangdan(0.0);
+		}else{
+			if (alt_flag ==1)
+			{
+				alt_flag = 0;
+				pos_control.set_alt_target(barometer.get_altitude()*100); // set target to current
+			}
+			
+				
+		}
+	}
+	if (!alt_flag2)
+	{
+		alt=pos_control.update_z_controller_f();
+	}
+	
+
+		
+	
+	Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, alt);   
+		 
+	motors.set_throttle(throttle_vehicle_frame.z + gaindelay2(1.0-(float)PressLevel_f*0.1,updowmgain));
+	 // 前进侧移付值
+	 float forward = channel_forward->norm_input();
+	 float lateral =channel_lateral->norm_input();
+	if (fabsf(forward)>=fabsf(lateral))
+	 {
+	   lateral = 0.0;
+	  }else{
+		forward = 0.0;
+	  }
+	motors.set_forward(constrain_float(-throttle_vehicle_frame.x + getgainf(forward),-0.8,0.8));//	*0.6 变慢
+	motors.set_lateral(constrain_float(-throttle_vehicle_frame.y + getgainf(lateral),-0.8,0.8));// *0.6 变慢
+	static uint16_t count = 0;
+	count++;
+	if (count>300)
+	{
+		count = 0;
+		gcs().send_text(MAV_SEVERITY_INFO, " altitude %f %f %f \n",pos_control.get_alt_target(),barometer.get_altitude()*100,throttle_vehicle_frame.z );
+		gcs().send_text(MAV_SEVERITY_INFO, " throttle_lower %d %f %f\n",(int)motors.limit.throttle_lower,motors.get_throttle_filter().get_cutoff_freq(),pos_control.alt_rate.get());
+	
+	}
+
+	
+
+}
+
+// althold_run - runs the althold controller
+// should be called at 100hz or more
+void Sub::sport_run()
+{
+
+	
+
+    // When unarmed, disable motors and stabilization
+    if (!motors.armed()) {
+
+        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);
+        attitude_control.relax_attitude_controllers();
+        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
+		
+
+        return;
+    }
+
+    // Vehicle is armed, motors are free to run
+    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+	
+	handle_attitude_sport();
+//----------------------------------------速度前馈--------------------------
+	//altcontrol_ff();
+    //-----------------------------------------位置控制----------------------------------------
+/*
+    altcontrol();
+*/
+//-----------------------速度给定位置保持
+	ratecontrol();
+
+	
+}
+

ArduSub/control_sport.dak

@@ -0,0 +1,431 @@
+#include "Sub.h"
+//#include "/AP_Math/AP_Math.h"
+#define PI	3.141592653589793238462643383279502884
+
+extern mavlink_rov_state_monitoring_t rov_message;
+Quaternion attitude_desired_quat_;
+
+
+/*
+ * control_althold.pde - init and run calls for althold, flight mode
+ */
+
+// althold_init - initialise althold controller
+bool Sub::sport_init()
+{
+
+	attitude_control.set_throttle_out(0.5 ,true, g.throttle_filt);
+	//holding_depth = false;
+	//检查是否有深度计存在
+    if(!control_check_barometer()) {
+        return false;
+    }
+
+    // initialize vertical speeds and leash lengths
+    // sets the maximum speed up and down returned by position controller
+    //get_pilot_speed_dn QGC PILOT 设置的值
+    pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);//(-500,500)设置最大最小速度 和刹车长度
+    pos_control.set_max_accel_z(g.pilot_accel_z);//设置最大加速度值 和刹车长度
+    pos_control.relax_alt_hold_controllers();//目标高度为当前高度，目标速度和上次速度 当前速度
+	//
+    //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);//当前深度12.19
+    holding_depth = true;
+	//ahrs.get_quat_body_to_ned(attitude_desired_quat_);//12.19 使用roll pitch =0的姿态
+
+    
+	attitude_desired_quat_.from_euler(0.0, 0.0,ahrs.yaw);
+    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();
+	
+	
+	
+    return true;
+}
+
+void Sub::handle_attitude_sport(){
+	uint32_t tnow = AP_HAL::millis();
+
+    // get pilot desired lean angles
+    float target_roll, target_pitch, target_yaw;
+
+    // Check if set_attitude_target_no_gps is valid
+    if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
+        Quaternion(
+            set_attitude_target_no_gps.packet.q
+        ).to_euler(
+            target_roll,
+            target_pitch,
+            target_yaw
+        );
+        target_roll = 100 * degrees(target_roll);
+        target_pitch = 100 * degrees(target_pitch);
+        target_yaw = 100 * degrees(target_yaw);
+        last_roll = target_roll;
+        last_pitch = target_pitch;
+        last_yaw = target_yaw;
+        
+        attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, target_yaw, true);
+    } else {
+        float throttle = 2*(0.5-channel_throttle->norm_input());
+		float yaw = channel_yaw->norm_input();
+		if (fabsf(throttle)>=fabsf(yaw))
+		{
+			yaw = 0.0;
+		}else{
+			throttle = 0.0;
+			yaw = channel_yaw->get_control_in()*0.3;
+		}
+		get_pilot_desired_lean_angles(0, (int16_t)(throttle/2.0*5700), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
+
+		target_yaw = get_pilot_desired_yaw_rate(yaw);//*0.6 变慢
+		
+        if (abs(target_roll) > 300 || abs(target_pitch) > 300) {
+            //last_roll = ahrs.roll_sensor;
+            //last_pitch = ahrs.pitch_sensor;
+            //last_yaw = ahrs.yaw_sensor;
+            
+            last_input_ms = tnow;
+            attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
+		    ahrs.get_quat_body_to_ned(attitude_desired_quat_);
+        } else if (abs(target_yaw) > 300) {
+            // if only yaw is being controlled, don't update pitch and roll
+            attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, target_yaw);
+            //last_yaw = ahrs.yaw_sensor;
+            ahrs.get_quat_body_to_ned(attitude_desired_quat_);
+            last_input_ms = tnow;
+        } else if (tnow < last_input_ms + 250) {
+            // just brake for a few mooments so we don't bounce
+            last_yaw = ahrs.yaw_sensor;
+            attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, 0);
+        } else {
+            // Lock attitude
+            //attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_yaw, true);
+            attitude_control.input_quaternion(attitude_desired_quat_);
+        }
+
+    }
+
+
+}
+
+void Sub::sport_run_alt(void){
+	 
+	
+	 // When unarmed, disable motors and stabilization
+	 if (!motors.armed()) {
+	
+		 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);
+		 attitude_control.relax_attitude_controllers();
+		 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
+		 attitude_desired_quat_.from_euler(0.0, 0.0,ahrs.yaw);
+	
+		 return;
+	 }
+
+	 motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); 
+	 handle_attitude_sport();
+	 //pos_control.update_z_controller();//输出了set_throttle_out
+	 Matrix3f mat_body = ahrs.get_rotation_body_to_ned();
+	 //Vector3f Zb = mat_body*Vector3f(0,0,1);
+	 //Vector3f Ze = Vector3f(0,0,1);
+	 //float thetaz = Zb*Ze;//计算有没有翻过来
+	 Matrix3f mat_half;
+	 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;
+
+
+	 }
+	// mat 地到half状态
+
+	 Matrix3f rc_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed()*mat_half;//rc_vehicle_frame（half）到body
+
+	
+	 
+		   float forward = channel_forward->norm_input();
+		   float lateral =channel_lateral->norm_input();
+		   if (fabsf(forward)>=fabsf(lateral))
+			 {
+			   lateral = 0.0;
+			  }else{
+				forward = 0.0;
+			  }
+	 
+	  Vector3f rc_throttle_forward = Vector3f(getgainf(forward),0, 0);//手柄的值
+	  Vector3f rc_throttle_lateral = Vector3f(0, getgainf(lateral), 0);//手柄的值
+	  Vector3f rc_throttle = Vector3f(0, 0, 2*(gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5));//手柄的值
+	   
+	  
+	   Vector3f rc_throttle_vehicle_frame1 = rc_vehicle_frame*rc_throttle_forward;//手柄从half系到body系
+	   Vector3f rc_throttle_vehicle_frame2 = rc_vehicle_frame*rc_throttle_lateral;//手柄从half系到body系
+	   Vector3f rc_throttle_vehicle_frame3 = rc_vehicle_frame*rc_throttle;//手柄从half系到body系
+	  
+	   Vector3f rc_throttle_vehicle_frame;
+	   rc_throttle_vehicle_frame.x = rc_throttle_vehicle_frame1.x + rc_throttle_vehicle_frame2.x - rc_throttle_vehicle_frame3.x;
+	   rc_throttle_vehicle_frame.y = rc_throttle_vehicle_frame1.y + rc_throttle_vehicle_frame2.y - rc_throttle_vehicle_frame3.y;
+	   rc_throttle_vehicle_frame.z = -rc_throttle_vehicle_frame1.z - rc_throttle_vehicle_frame2.z + rc_throttle_vehicle_frame3.z;
+
+
+
+	 // Read the output of the z controller and rotate it so it always points up
+	 //将Z轴输出变到机体坐标系
+	 //Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, motors.get_throttle_in_bidirectional());
+	 float z_throttle = pos_control.update_z_controller_f();//输出了set_throttle_out
+
+	 Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, z_throttle);
+ 
+	
+	//手柄控制和Z轴输出的叠加  
+
+	 motors.set_forward(-throttle_vehicle_frame.x +rc_throttle_vehicle_frame.x);
+	 motors.set_lateral(-throttle_vehicle_frame.y +rc_throttle_vehicle_frame.y);//*0.6 变慢
+	 motors.set_throttle(throttle_vehicle_frame.z +rc_throttle_vehicle_frame.z/2+0.5);
+	 rov_message.pressure_level = int(PressLevel);
+	
+     Vector3f earth_frame_rc_inputs =  Vector3f(0.0, 0.0, 2*(gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5));//去掉侧移影响
+
+
+	
+	 if (fabsf(earth_frame_rc_inputs.z) > 0.05f) { // Throttle input  above 5%
+		// reset z targets to current values
+		 holding_depth = false;
+		 pos_control.relax_alt_hold_controllers();
+		 
+		 
+	
+	 } 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;
+			 
+	
+		 } 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 +10);//cm
+			 holding_depth = true;
+			 
+	
+		 } else if (!holding_depth) {
+			
+			 pos_control.calc_leash_length_z();//刹车长度 12.19
+			// pos_control.set_target_to_stopping_point_z();//有对位置的付值
+			 pos_control.set_alt_target(barometer.get_altitude()*100);//cm 当前深度
+	
+			 holding_depth = true;
+		   
+		 }
+	 }
+	 	static bool lastdepth = holding_depth;
+	if(lastdepth !=holding_depth){
+		lastdepth = holding_depth;
+		
+		
+		gcs().send_text(MAV_SEVERITY_INFO, "get_alt_target	%d	%f %f\n",(int)holding_depth,pos_control.get_alt_target(),barometer.get_altitude()*100);
+	
+	}
+	 static uint16_t count = 0;
+	 count++;
+	 if (count>300)
+		 {
+		 count = 0;
+		 
+		 gcs().send_text(MAV_SEVERITY_INFO, "alt %f %f %f %f\n",pos_control.get_alt_target(),barometer.get_altitude()*100,pos_control.alt_rate.get(),motors.get_throttle());
+	 	 gcs().send_text(MAV_SEVERITY_INFO, "surf %d %d %f \n",(int)ap.at_surface,(int)ap.at_bottom,motors.get_throttle_thrust_max());
+		 }
+
+}
+
+void Sub::altcontrol(){
+
+	 float z_throttle = pos_control.update_z_controller_f();//输出了set_throttle_out
+	   // Read the output of the z controller and rotate it so it always points up
+	   //变到机体坐标系
+	 Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, z_throttle);	
+	   // Output the Z controller + pilot input to all motors.
+	 motors.set_throttle(throttle_vehicle_frame.z + gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain));
+	
+		static uint16_t j=0;
+	  j++;
+	  if(j>300)
+	  {
+		gcs().send_text(MAV_SEVERITY_INFO, " sportthrottle %f %f %f \n",z_throttle,throttle_vehicle_frame.z,motors.get_throttle());
+
+		 j=0;
+	  }
+	   rov_message.pressure_level = int(PressLevel);
+	  float forward = channel_forward->norm_input();
+	  float lateral =channel_lateral->norm_input();
+	  if (fabsf(forward)>=fabsf(lateral))
+		{
+		  lateral = 0.0;
+		 }else{
+		   forward = 0.0;
+		 }
+	   motors.set_forward(constrain_float(-throttle_vehicle_frame.x + getgainf(forward),-0.8,0.8));//*0.6 变慢
+	   motors.set_lateral(constrain_float(-throttle_vehicle_frame.y + getgainf(lateral),-0.8,0.8));//*0.6 变慢
+	
+	   Vector3f earth_frame_rc_inputs1 = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), 0.0, 2*(gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5));//去掉侧移影响
+	
+	
+	   if (fabsf(earth_frame_rc_inputs1.z) > 0.05f) { // Throttle input  above 5%
+		  // reset z targets to current values
+		   holding_depth = false;
+		   pos_control.relax_alt_hold_controllers();
+		 
+		   //pos_control.set_alt_target(barometer.get_altitude()*100);//当前深度
+		   
+	   } 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;
+	  		} 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+10);//cm
+			   holding_depth = true;
+			   
+		   	} else if (!holding_depth) {
+				pos_control.calc_leash_length_z();//刹车长度 12.19
+				pos_control.set_alt_target(barometer.get_altitude()*100);//cm 当前深度
+				 holding_depth = true;
+		   	}
+	   }
+	   static bool lastdepth = holding_depth;
+	   if(lastdepth !=holding_depth){
+		   lastdepth = holding_depth;
+		   gcs().send_text(MAV_SEVERITY_INFO, " get_alt_target %d  %f  \n",(int)holding_depth,motors.get_throttle());
+	   }
+	
+	   static uint16_t count = 0;
+	   count++;
+	   if (count>300)
+		   {
+		    count = 0;
+		     gcs().send_text(MAV_SEVERITY_INFO, " surface %d %d %d\n",(int)ap.at_surface,(int)ap.at_bottom,(int)motors.limit.throttle_lower);
+		     gcs().send_text(MAV_SEVERITY_INFO, " altitude %f %f \n",pos_control.get_alt_target(),barometer.get_altitude()*100);
+	
+		   }
+
+}
+
+
+// althold_run - runs the althold controller
+// should be called at 100hz or more
+void Sub::sport_run()
+{
+
+	
+
+    // When unarmed, disable motors and stabilization
+    if (!motors.armed()) {
+
+        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);
+        attitude_control.relax_attitude_controllers();
+        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
+		
+
+        return;
+    }
+
+    // Vehicle is armed, motors are free to run
+    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+	
+	handle_attitude_sport();
+
+    altcontrol();
+
+
+	
+}
+

ArduSub/control_stabilize.cpp

@@ -1,20 +1,26 @@
 #include "Sub.h"
+#define PI	3.141592653589793238462643383279502884
 
 // stabilize_init - initialise stabilize controller
 bool Sub::stabilize_init()
 {
     // set target altitude to zero for reporting
-    pos_control.set_alt_target(0);
+    pos_control.set_alt_target(-10);//03.15
     
     last_roll_s = 0;
     last_pitch_s = 0;
     updowmgain =0.5;
+	delaygain = 401;
+
+	continuous_count=0;
+	continuous_countdec=0;
+	last_continuous = 0;
     last_yaw_s = ahrs.yaw_sensor;
     //last_input_ms = AP_HAL::millis();
 	last_input_ms_stable = AP_HAL::millis();
 
 	pitch_input_inc=0;
-	//yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
+	
 	prepare_state = Horizontal;
     return true;
 }
@@ -51,8 +57,7 @@ void Sub::handle_attitude_stablize(){
 		attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, target_yaw, true);
 	} else {
 		
-		//int32_t yaw_input= (int32_t)((float)yaw_press*100);
-		//int32_t roll_input	= 0;
+
 		int32_t pitch_input   = 0;
 		
 
@@ -76,7 +81,7 @@ void Sub::handle_attitude_stablize(){
 		}
 	   pitch_input_inc = constrain_int32(pitch_input,-9000,9000);
 	   //-------------------------------------
-	  // target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+	 
 	  if (abs(target_roll) > 300 ){
 		attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, 0, 0);
 		last_yaw_s = ahrs.yaw_sensor;
@@ -97,15 +102,13 @@ void Sub::handle_attitude_stablize(){
 		} else {
 			// Lock attitude
 			attitude_control.input_euler_angle_roll_pitch_yaw(last_roll_s, (float)pitch_input_inc, last_yaw_s, true);
-			//attitude_control.input_euler_angle_roll_pitch_yaw(roll_input, (float)pitch_input_inc, last_yaw, true);
+			
 		}//--------------------------------------------------------------------------------------------------------------
-	   //attitude_control.input_euler_angle_roll_pitch_yaw(roll_input, (float)pitch_input_inc, yaw_input, true);//pitch九十度 竖直前进有可能翻车
-		//attitude_control.input_euler_angle_roll_pitch_yaw_quat_control((float)roll_input, (float)pitch_input_inc, (float)yaw_input, true);//四元数控制在pitch90度时的效果与欧拉角控制在80度时相差不多, 但是在360往0的YAW切换的时候会猛烈转动
-		static int f = 0;
+	   static int f = 0;
 		f++;
 		if(f>800)
 		{
-		//float cosruslt = sinf(radians(286)*0.5f);
+	
 
 			//gcs().send_text(MAV_SEVERITY_INFO, " stableinput %f %f,%f ",(float)last_roll_s,(float)pitch_input_inc,(float)last_yaw_s);
 			f=0;
@@ -124,6 +127,11 @@ void Sub::stabilize_run()
         last_roll_s = 0;
         last_pitch_s = 0;
 		updowmgain =0.5;
+		delaygain = 401;
+	
+		continuous_count=0;
+		continuous_countdec=0;
+		last_continuous = 0;
         last_yaw_s = ahrs.yaw_sensor;
 
 		PressLevel_f =5.0;//压力分级复位
@@ -137,11 +145,34 @@ void Sub::stabilize_run()
 
     handle_attitude_stablize();
 
-    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));
+
+    if(stable_alt_control){
+			altcontrol();
+	}
+	else{
+		//防止交叉
+		 float _forward = channel_forward->norm_input();
+		 float _lateral =channel_lateral->norm_input();
+		if (fabsf(_forward)>=fabsf(_lateral))
+		 {
+		   _lateral = 0.0;
+		  }else{
+			_forward = 0.0;
+		  }
+    	motors.set_forward(constrain_float(getgainf(_forward),-0.8,0.8));
+    	motors.set_lateral(constrain_float(getgainf(_lateral),-0.8,0.8));
+		
+		attitude_control.set_throttle_out(gaindelay(1.0-(float)PressLevel_f*0.1), false, g.throttle_filt);//压力分级
+		 static uint16_t count = 0;
+		count++;
+		if (count>400)
+		{
+			count = 0;
+			gcs().send_text(MAV_SEVERITY_INFO, "set_throttle_out %f %f \n",gaindelay(1.0-(float)PressLevel_f*0.1),motors.get_throttle());
 	
-		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);//压力分级
+	
 	rov_message.pressure_level = int(PressLevel);
+	}
 }

ArduSub/failsafe.cpp

@@ -140,7 +140,7 @@ void Sub::failsafe_ekf_check()
 
     AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE);
 
-    if (AP_HAL::millis() > failsafe.last_ekf_warn_ms + 20000) {
+    if (AP_HAL::millis() > failsafe.last_ekf_warn_ms + 25000) {
         failsafe.last_ekf_warn_ms = AP_HAL::millis();
         gcs().send_text(MAV_SEVERITY_WARNING, "EKF bad");
     }

ArduSub/joystick.cpp

@@ -74,6 +74,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
         }
     }
 
+
     // Act if button is pressed
     // Only act upon pressing button and ignore holding. This provides compatibility with Taranis as joystick.
     for (uint8_t i = 0 ; i < 16 ; i++) {
@@ -158,10 +159,10 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
     Vector3f localRoll = Vector3f(1, 0, 0);
 	if(button==15){
 		    if(control_mode == SPORT){
-				updowmgain=0.8;
+				updowmgain=0.7;
 				
         	}else{
-			updowmgain=0.7;
+			updowmgain=0.8;
         	}
 			delaygain =0;
 			
@@ -186,6 +187,8 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
 			motor1_speed_target =0;
 			motor2_speed_target =0;		
 			track_motor_arm=1;
+			delaygain = 401;
+			updowmgain = 0.5;
 		}
         arming.disarm();
         break;
@@ -275,7 +278,8 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
             uint16_t min = chan->get_radio_min();
             uint16_t max = chan->get_radio_max();
             uint16_t step = (max - min) / g.lights_steps;
-            lights1 = constrain_float(lights1 + step, min, max);
+            lights1 = constrain_float(lights1 + step, min, max);//新灯一级灯光会闪
+	
         }
         break;
     case JSButton::button_function_t::k_lights1_dimmer:
@@ -285,6 +289,7 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
             uint16_t max = chan->get_radio_max();
             uint16_t step = (max - min) / g.lights_steps;
             lights1 = constrain_float(lights1 - step, min, max);
+
         }
         break;
     case JSButton::button_function_t::k_lights2_cycle:
@@ -729,16 +734,26 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
     	{
     		if(control_mode == SPORT)
 			{
-			updowmgain=0.2;//下
+			updowmgain=0.3;//下
 			}
 			else{
-				updowmgain=0.3;//下
+				updowmgain=0.2;//下
 			}
 			delaygain =0;
     	}
         break;
     case JSButton::button_function_t::k_custom_6:
-    	{
+    	{ 
+	    	if (!held){
+				if(control_mode == STABILIZE){
+					stable_alt_control = !stable_alt_control;
+					}
+				else{
+					stable_alt_control = TRUE;
+					}
+				 gcs().send_text(MAV_SEVERITY_INFO,"stable_alt_control %d",(int)stable_alt_control);
+	    	}
+    	
     	}
         break;
     }

ArduSub/surface_bottom_detector.cpp

@@ -1,44 +1,163 @@
 // Jacob Walser: jacob@bluerobotics.com
 
 #include "Sub.h"
+uint8_t maxthree(float x1,float x2,float x3);
 
 // counter to verify contact with bottom
 static uint32_t bottom_detector_count = 0;
 static uint32_t surface_detector_count = 0;
 static float current_depth = 0;
-
+bool bottomdetect = FALSE;
+bool setZ = TRUE;
 // checks if we have have hit bottom or surface and updates the ap.at_bottom and ap.at_surface flags
 // called at MAIN_LOOP_RATE
 // ToDo: doesn't need to be called this fast
+uint8_t maxthree(float x1,float x2,float x3){
+	
+	if(fabsf(x1)>fabsf(x2) || fabsf(fabsf(x1)-fabsf(x2))<0.0000001){//x1>=x3
+		if(fabsf(x1)>fabsf(x3) || fabsf(fabsf(x1)-fabsf(x3))<0.0000001){//x1>=x3
+		
+			return 1;
+		}else{//x3>x1
+			return 3;
+		}
+	}
+	if(fabsf(x2)>fabsf(x1) || fabsf(fabsf(x2)-fabsf(x1))<0.0000001){//x2>=x1
+		if(fabsf(x2)>fabsf(x3) || fabsf(fabsf(x2)-fabsf(x3))<0.0000001){//x2>=x3
+		
+			return 2;
+		}else{//x3>=x2
+			return 3;
+		}
+	}
+	return 0;
+	
+
+}
+void Sub::bottom_detectorgain(void){
+	float velocity_z = pos_control.alt_rate.get()/100;// m/s
+	bool vel_stationary = velocity_z > -0.05 && velocity_z < 0.05;
+	Matrix3f mat_body = ahrs.get_rotation_body_to_ned();
+	 Vector3f Xbx = mat_body*Vector3f(1,0,0);
+	 Vector3f Xby = mat_body*Vector3f(0,1,0);
+	 Vector3f Xbz = mat_body*Vector3f(0,0,1);
+	 float dot1=Xbx*Vector3f(0,0,1);
+	 float dot2=Xby*Vector3f(0,0,1);
+	 float dot3=Xbz*Vector3f(0,0,1);
+	 const AP_Motors6DOF &motors6dof = AP::motors6dof();//6自由度电机计算出来的PWM
+	 static uint16_t countx=0;
+	 static uint16_t county=0;
+	 static uint16_t countz=0;
+	  Vector3f velocity;
+	velocity.z = pos_control.alt_rate.get()/100;//12.20 cm
+
+    // check that we are not moving up or down
+
+	if (vel_stationary)//速度很小
+	{
+	 if(maxthree(dot1,dot2,dot3)==1) {//x轴
+	   
+	 	 float x1 = (float)(motors6dof.motor_to_detect[0] - NETRULPWM)/(NETRULPWM-1000) ;
+		 float x2 = (float)(motors6dof.motor_to_detect[1] - NETRULPWM)/(NETRULPWM-1000) ;
+		 if (fabsf(x1)>0.95 && fabsf(x2)>0.95)
+		 {
+		 	  countz=0;
+	 		  county=0;
+			if (countx < 2*MAIN_LOOP_RATE) {
+                countx++;
+            } else{
+				bottom_set = TRUE;
+			}
+		   
+		 }else{
+			 bottom_set = FALSE;
+			 countx = 0;
+		 }
+	 }
+	 else if (maxthree(dot1,dot2,dot3)==2 ){//y轴
+	  	 float l = (float)(motors6dof.motor_to_detect[5] - NETRULPWM)/(NETRULPWM-1000) ;
+		 countx=0;
+	 	 countz=0;
+		 if (fabsf(l)>0.9 )
+		 {
+		 	if (county < 2*MAIN_LOOP_RATE) {
+                county++;
+            } else{
+				bottom_set = TRUE;
+			}
+		   
+		 }else{
+		 	 bottom_set = FALSE;
+			 county = 0;
+		 }
+	 }
+	 else if (maxthree(dot1,dot2,dot3)==3 ){//z轴
+	 	countx=0;
+	 	 county=0;
+	 	 float z1 = (float)(motors6dof.motor_to_detect[2] - NETRULPWM)/(NETRULPWM-1000) ;
+		 float z2 = (float)(motors6dof.motor_to_detect[3] - NETRULPWM)/(NETRULPWM-1000) ;
+	 	 //float z3 = (float)(motors6dof.motor_to_detect[4] - NETRULPWM)/(NETRULPWM-1000) ;
+		 if(fabsf(z1)>0.95 && fabsf(z2)>0.95){
+			 if (countz < 2*MAIN_LOOP_RATE) {
+					countz++;
+			} else{
+					bottom_set = TRUE;
+			}
+		 }else{
+		 	 bottom_set = FALSE;
+			 countz = 0;
+		 }
+
+		 
+	 }else{
+		
+	 	 countx=0;
+	     county=0;
+	     countz=0;
+		 bottom_set = FALSE; //这里忽略了三个或者2个相等的情况
+	 }
+	}
+	 else{
+		 countx=0;
+	     county=0;
+	     countz=0;
+		 bottom_set = FALSE; //速度比较大
+	 	}
+	
+	 
+
+}
+
+float Sub::surface_depth_use(void){
+
+	Matrix3f mat_body = ahrs.get_rotation_body_to_ned();
+	float surface_depth_use;
+	if (abs(mat_body.c.x) <0.2)
+		{
+		surface_depth_use = g.surface_depth/100.0;
+		}else if(fabsf(mat_body.c.x) <0.45){
+			surface_depth_use = -0.2;//m
+		}else if(fabsf(mat_body.c.x) <0.7){
+			surface_depth_use = -0.3;//m
+		}else{
+			surface_depth_use = -0.45;//m
+		}
+		return surface_depth_use;
+
+}
 void Sub::update_surface_and_bottom_detector()
 {
-	static float last_alt = 0.0;//12.20
+	
     if (!motors.armed()) { // only update when armed
         set_surfaced(false);
         set_bottomed(false);
-		last_alt =  barometer.get_altitude();
+		
         return;
     }
 
     Vector3f velocity;
-    //ahrs.get_velocity_NED(velocity);//12.20
-    float cur_alt = barometer.get_altitude(); // m
-    
-	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 uint16_t i=0;
-	uint16_t timecnt = (uint16_t)(0.25/MAIN_LOOP_SECONDS);
-	i++;
-	if (i>timecnt)
-	{
-		i = 0;
-		last_alt = cur_alt;//12.19
-	}
-
-	
-
+	//velocity.z = pos_control.alt_rate.get()/100;//12.20 cm
+	velocity.z = velocity_z_filer/100;//03.24 cm
 
     // check that we are not moving up or down
     bool vel_stationary = velocity.z > -0.05 && velocity.z < 0.05;
@@ -46,20 +165,21 @@ void Sub::update_surface_and_bottom_detector()
     if (ap.depth_sensor_present && sensor_health.depth) { // we can use the external pressure sensor for a very accurate and current measure of our z axis position
         current_depth = barometer.get_altitude(); // cm
 
-
+		 
         if (ap.at_surface) {
-            set_surfaced(current_depth > g.surface_depth/100.0 - 0.05); // add a 5cm buffer so it doesn't trigger too often
+            set_surfaced(current_depth > surface_depth_use() - 0.05); // add a 5cm buffer so it doesn't trigger too often
         } else {
-            set_surfaced(current_depth > g.surface_depth/100.0); // If we are above surface depth, we are surfaced
+            set_surfaced(current_depth > surface_depth_use()); // If we are above surface depth, we are surfaced
         }
 
 
-        if (motors.limit.throttle_lower && vel_stationary) {
+        if (motors.limit.throttle_lower && vel_stationary){
+        
             // bottom criteria met - increment the counter and check if we've triggered
             if (bottom_detector_count < ((float)BOTTOM_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
                 bottom_detector_count++;
             } else {
-               // set_bottomed(true);//03.02
+                set_bottomed(true);//03.02
             }
 
         } else {
@@ -78,11 +198,12 @@ void Sub::update_surface_and_bottom_detector()
             }
 
         } else if (motors.limit.throttle_lower) {
+       
             // bottom criteria met, increment counter and see if we've triggered
             if (bottom_detector_count < ((float)BOTTOM_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
                 bottom_detector_count++;
             } else {
-               // set_bottomed(true);//03.02
+                set_bottomed(true);//03.02
             }
 
         } else { // we're not at the limits of throttle, so reset both detectors
@@ -94,6 +215,7 @@ void Sub::update_surface_and_bottom_detector()
         set_surfaced(false);
         set_bottomed(false);
     }
+	
 }
 
 void Sub::set_surfaced(bool at_surface)

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -33,7 +33,7 @@
 
 #define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT      1.0f    // Time constant used to limit lean angle so that vehicle does not lose altitude
 #define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX    0.8f    // Max throttle used to limit lean angle so that vehicle does not lose altitude
-#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN             10.0f   // Min lean angle so that vehicle can maintain limited control
+#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN             15.0f   // Min lean angle so that vehicle can maintain limited control
 
 #define AC_ATTITUDE_CONTROL_MIN_DEFAULT                 0.1f    // minimum throttle mix default
 #define AC_ATTITUDE_CONTROL_MAN_DEFAULT                 0.1f    // manual throttle mix default

libraries/AC_AttitudeControl/AC_PosControl.bak

@@ -0,0 +1,1541 @@
+#include <AP_HAL/AP_HAL.h>
+#include "AC_PosControl.h"
+#include <AP_Math/AP_Math.h>
+#include <AP_Logger/AP_Logger.h>
+#include "../../ArduSub/Sub.h"
+
+extern const AP_HAL::HAL& hal;
+
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+ // default gains for Plane
+ # define POSCONTROL_POS_Z_P                    1.0f    // vertical position controller P gain default
+ # define POSCONTROL_VEL_Z_P                    5.0f    // vertical velocity controller P gain default
+ # define POSCONTROL_ACC_Z_P                    0.3f    // vertical acceleration controller P gain default
+ # define POSCONTROL_ACC_Z_I                    1.0f    // vertical acceleration controller I gain default
+ # define POSCONTROL_ACC_Z_D                    0.0f    // vertical acceleration controller D gain default
+ # define POSCONTROL_ACC_Z_IMAX                 800     // vertical acceleration controller IMAX gain default
+ # define POSCONTROL_ACC_Z_FILT_HZ              10.0f   // vertical acceleration controller input filter default
+ # define POSCONTROL_ACC_Z_DT                   0.02f   // vertical acceleration controller dt default
+ # define POSCONTROL_POS_XY_P                   1.0f    // horizontal position controller P gain default
+ # define POSCONTROL_VEL_XY_P                   1.4f    // horizontal velocity controller P gain default
+ # define POSCONTROL_VEL_XY_I                   0.7f    // horizontal velocity controller I gain default
+ # define POSCONTROL_VEL_XY_D                   0.35f   // horizontal velocity controller D gain default
+ # define POSCONTROL_VEL_XY_IMAX                1000.0f // horizontal velocity controller IMAX gain default
+ # define POSCONTROL_VEL_XY_FILT_HZ             5.0f    // horizontal velocity controller input filter
+ # define POSCONTROL_VEL_XY_FILT_D_HZ           5.0f    // horizontal velocity controller input filter for D
+#elif APM_BUILD_TYPE(APM_BUILD_ArduSub)
+ // default gains for Sub
+ # define POSCONTROL_POS_Z_P                    3.0f    // vertical position controller P gain default
+ # define POSCONTROL_VEL_Z_P                    8.0f    // vertical velocity controller P gain default
+ # define POSCONTROL_ACC_Z_P                    0.5f    // vertical acceleration controller P gain default
+ # define POSCONTROL_ACC_Z_I                    0.1f    // vertical acceleration controller I gain default
+ # define POSCONTROL_ACC_Z_D                    0.0f    // vertical acceleration controller D gain default
+ # define POSCONTROL_ACC_Z_IMAX                 100     // vertical acceleration controller IMAX gain default
+ # define POSCONTROL_ACC_Z_FILT_HZ              20.0f   // vertical acceleration controller input filter default
+ # define POSCONTROL_ACC_Z_DT                   0.0025f // vertical acceleration controller dt default
+ # define POSCONTROL_POS_XY_P                   1.0f    // horizontal position controller P gain default
+ # define POSCONTROL_VEL_XY_P                   1.0f    // horizontal velocity controller P gain default
+ # define POSCONTROL_VEL_XY_I                   0.5f    // horizontal velocity controller I gain default
+ # define POSCONTROL_VEL_XY_D                   0.0f    // horizontal velocity controller D gain default
+ # define POSCONTROL_VEL_XY_IMAX                1000.0f // horizontal velocity controller IMAX gain default
+ # define POSCONTROL_VEL_XY_FILT_HZ             5.0f    // horizontal velocity controller input filter
+ # define POSCONTROL_VEL_XY_FILT_D_HZ           5.0f    // horizontal velocity controller input filter for D
+#else
+ // default gains for Copter / TradHeli
+ # define POSCONTROL_POS_Z_P                    1.0f    // vertical position controller P gain default
+ # define POSCONTROL_VEL_Z_P                    5.0f    // vertical velocity controller P gain default
+ # define POSCONTROL_ACC_Z_P                    0.5f    // vertical acceleration controller P gain default
+ # define POSCONTROL_ACC_Z_I                    1.0f    // vertical acceleration controller I gain default
+ # define POSCONTROL_ACC_Z_D                    0.0f    // vertical acceleration controller D gain default
+ # define POSCONTROL_ACC_Z_IMAX                 800     // vertical acceleration controller IMAX gain default
+ # define POSCONTROL_ACC_Z_FILT_HZ              20.0f   // vertical acceleration controller input filter default
+ # define POSCONTROL_ACC_Z_DT                   0.0025f // vertical acceleration controller dt default
+ # define POSCONTROL_POS_XY_P                   1.0f    // horizontal position controller P gain default
+ # define POSCONTROL_VEL_XY_P                   2.0f    // horizontal velocity controller P gain default
+ # define POSCONTROL_VEL_XY_I                   1.0f    // horizontal velocity controller I gain default
+ # define POSCONTROL_VEL_XY_D                   0.5f    // horizontal velocity controller D gain default
+ # define POSCONTROL_VEL_XY_IMAX                1000.0f // horizontal velocity controller IMAX gain default
+ # define POSCONTROL_VEL_XY_FILT_HZ             5.0f    // horizontal velocity controller input filter
+ # define POSCONTROL_VEL_XY_FILT_D_HZ           5.0f    // horizontal velocity controller input filter for D
+#endif
+
+const AP_Param::GroupInfo AC_PosControl::var_info[] = {
+    // 0 was used for HOVER
+
+    // @Param: _ACC_XY_FILT
+    // @DisplayName: XY Acceleration filter cutoff frequency
+    // @Description: Lower values will slow the response of the navigation controller and reduce twitchiness
+    // @Units: Hz
+    // @Range: 0.5 5
+    // @Increment: 0.1
+    // @User: Advanced
+    AP_GROUPINFO("_ACC_XY_FILT", 1, AC_PosControl, _accel_xy_filt_hz, POSCONTROL_ACCEL_FILTER_HZ),
+
+    // @Param: _POSZ_P
+    // @DisplayName: Position (vertical) controller P gain
+    // @Description: Position (vertical) controller P gain.  Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller
+    // @Range: 1.000 3.000
+    // @User: Standard
+    AP_SUBGROUPINFO(_p_pos_z, "_POSZ_", 2, AC_PosControl, AC_P),
+
+    // @Param: _VELZ_P
+    // @DisplayName: Velocity (vertical) controller P gain
+    // @Description: Velocity (vertical) controller P gain.  Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller
+    // @Range: 1.000 8.000
+    // @User: Standard
+    AP_SUBGROUPINFO(_p_vel_z, "_VELZ_", 3, AC_PosControl, AC_P),
+
+    // @Param: _ACCZ_P
+    // @DisplayName: Acceleration (vertical) controller P gain
+    // @Description: Acceleration (vertical) controller P gain.  Converts the difference between desired vertical acceleration and actual acceleration into a motor output
+    // @Range: 0.500 1.500
+    // @Increment: 0.05
+    // @User: Standard
+
+    // @Param: _ACCZ_I
+    // @DisplayName: Acceleration (vertical) controller I gain
+    // @Description: Acceleration (vertical) controller I gain.  Corrects long-term difference in desired vertical acceleration and actual acceleration
+    // @Range: 0.000 3.000
+    // @User: Standard
+
+    // @Param: _ACCZ_IMAX
+    // @DisplayName: Acceleration (vertical) controller I gain maximum
+    // @Description: Acceleration (vertical) controller I gain maximum.  Constrains the maximum pwm that the I term will generate
+    // @Range: 0 1000
+    // @Units: d%
+    // @User: Standard
+
+    // @Param: _ACCZ_D
+    // @DisplayName: Acceleration (vertical) controller D gain
+    // @Description: Acceleration (vertical) controller D gain.  Compensates for short-term change in desired vertical acceleration vs actual acceleration
+    // @Range: 0.000 0.400
+    // @User: Standard
+
+    // @Param: _ACCZ_FILT
+    // @DisplayName: Acceleration (vertical) controller filter
+    // @Description: Filter applied to acceleration to reduce noise.  Lower values reduce noise but add delay.
+    // @Range: 1.000 100.000
+    // @Units: Hz
+    // @User: Standard
+    AP_SUBGROUPINFO(_pid_accel_z, "_ACCZ_", 4, AC_PosControl, AC_PID),
+
+    // @Param: _POSXY_P
+    // @DisplayName: Position (horizonal) controller P gain
+    // @Description: Position controller P gain.  Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller
+    // @Range: 0.500 2.000
+    // @User: Standard
+    AP_SUBGROUPINFO(_p_pos_xy, "_POSXY_", 5, AC_PosControl, AC_P),
+
+    // @Param: _VELXY_P
+    // @DisplayName: Velocity (horizontal) P gain
+    // @Description: Velocity (horizontal) P gain.  Converts the difference between desired velocity to a target acceleration
+    // @Range: 0.1 6.0
+    // @Increment: 0.1
+    // @User: Advanced
+
+    // @Param: _VELXY_I
+    // @DisplayName: Velocity (horizontal) I gain
+    // @Description: Velocity (horizontal) I gain.  Corrects long-term difference in desired velocity to a target acceleration
+    // @Range: 0.02 1.00
+    // @Increment: 0.01
+    // @User: Advanced
+
+    // @Param: _VELXY_D
+    // @DisplayName: Velocity (horizontal) D gain
+    // @Description: Velocity (horizontal) D gain.  Corrects short-term changes in velocity
+    // @Range: 0.00 1.00
+    // @Increment: 0.001
+    // @User: Advanced
+
+    // @Param: _VELXY_IMAX
+    // @DisplayName: Velocity (horizontal) integrator maximum
+    // @Description: Velocity (horizontal) integrator maximum.  Constrains the target acceleration that the I gain will output
+    // @Range: 0 4500
+    // @Increment: 10
+    // @Units: cm/s/s
+    // @User: Advanced
+
+    // @Param: _VELXY_FILT
+    // @DisplayName: Velocity (horizontal) input filter
+    // @Description: Velocity (horizontal) input filter.  This filter (in hz) is applied to the input for P and I terms
+    // @Range: 0 100
+    // @Units: Hz
+    // @User: Advanced
+
+    // @Param: _VELXY_D_FILT
+    // @DisplayName: Velocity (horizontal) input filter
+    // @Description: Velocity (horizontal) input filter.  This filter (in hz) is applied to the input for P and I terms
+    // @Range: 0 100
+    // @Units: Hz
+    // @User: Advanced
+    AP_SUBGROUPINFO(_pid_vel_xy, "_VELXY_", 6, AC_PosControl, AC_PID_2D),
+
+    // @Param: _ANGLE_MAX
+    // @DisplayName: Position Control Angle Max
+    // @Description: Maximum lean angle autopilot can request.  Set to zero to use ANGLE_MAX parameter value
+    // @Units: deg
+    // @Range: 0 45
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("_ANGLE_MAX", 7, AC_PosControl, _lean_angle_max, 0.0f),
+
+    AP_GROUPEND
+};
+
+// Default constructor.
+// Note that the Vector/Matrix constructors already implicitly zero
+// their values.
+//
+AC_PosControl::AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& inav,
+                             const AP_Motors& motors, AC_AttitudeControl& attitude_control) :
+    _ahrs(ahrs),
+    _inav(inav),
+    _motors(motors),
+    _attitude_control(attitude_control),
+    _p_pos_z(POSCONTROL_POS_Z_P),
+    _p_vel_z(POSCONTROL_VEL_Z_P),
+    _pid_accel_z(POSCONTROL_ACC_Z_P, POSCONTROL_ACC_Z_I, POSCONTROL_ACC_Z_D, 0.0f, POSCONTROL_ACC_Z_IMAX, 0.0f, POSCONTROL_ACC_Z_FILT_HZ, 0.0f, POSCONTROL_ACC_Z_DT),
+    _p_pos_xy(POSCONTROL_POS_XY_P),
+    _pid_vel_xy(POSCONTROL_VEL_XY_P, POSCONTROL_VEL_XY_I, POSCONTROL_VEL_XY_D, POSCONTROL_VEL_XY_IMAX, POSCONTROL_VEL_XY_FILT_HZ, POSCONTROL_VEL_XY_FILT_D_HZ, POSCONTROL_DT_50HZ),
+    _dt(POSCONTROL_DT_400HZ),
+    _speed_down_cms(POSCONTROL_SPEED_DOWN),
+    _speed_up_cms(POSCONTROL_SPEED_UP),
+    _speed_cms(POSCONTROL_SPEED),
+    _accel_z_cms(POSCONTROL_ACCEL_Z),
+    _accel_cms(POSCONTROL_ACCEL_XY),
+    _leash(POSCONTROL_LEASH_LENGTH_MIN),
+    _leash_down_z(POSCONTROL_LEASH_LENGTH_MIN),
+    _leash_up_z(POSCONTROL_LEASH_LENGTH_MIN),
+    _accel_target_filter(POSCONTROL_ACCEL_FILTER_HZ),
+    alt_rate(10.0)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+
+    // initialise flags
+    _flags.recalc_leash_z = true;
+    _flags.recalc_leash_xy = true;
+    _flags.reset_desired_vel_to_pos = true;
+    _flags.reset_accel_to_lean_xy = true;
+    _flags.reset_rate_to_accel_z = true;
+    _flags.freeze_ff_z = true;
+    _flags.use_desvel_ff_z = true;
+    _limit.pos_up = true;
+    _limit.pos_down = true;
+    _limit.vel_up = true;
+    _limit.vel_down = true;
+    _limit.accel_xy = true;
+	
+}
+
+///
+/// z-axis position controller
+///
+
+/// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
+void AC_PosControl::set_dt(float delta_sec)
+{
+    _dt = delta_sec;
+
+    // update PID controller dt
+    _pid_accel_z.set_dt(_dt);
+    _pid_vel_xy.set_dt(_dt);
+
+    // update rate z-axis velocity error and accel error filters
+    _vel_error_filter.set_cutoff_frequency(POSCONTROL_VEL_ERROR_CUTOFF_FREQ);
+}
+
+/// set_max_speed_z - set the maximum climb and descent rates
+/// To-Do: call this in the main code as part of flight mode initialisation
+void AC_PosControl::set_max_speed_z(float speed_down, float speed_up)
+{
+    // ensure speed_down is always negative
+    speed_down = -fabsf(speed_down);
+//默认_speed_down_cms -150  ,,_speed_up_cms 250
+    if ((fabsf(_speed_down_cms - speed_down) > 1.0f) || (fabsf(_speed_up_cms - speed_up) > 1.0f)) {
+        _speed_down_cms = speed_down;
+        _speed_up_cms = speed_up;
+        _flags.recalc_leash_z = true;
+        calc_leash_length_z();// 刹车长度
+    }
+}
+
+/// set_max_accel_z - set the maximum vertical acceleration in cm/s/s
+void AC_PosControl::set_max_accel_z(float accel_cmss)
+{
+    if (fabsf(_accel_z_cms - accel_cmss) > 1.0f) {
+        _accel_z_cms = accel_cmss;
+        _flags.recalc_leash_z = true;
+        calc_leash_length_z();
+    }
+}
+
+/// set_alt_target_with_slew - adjusts target towards a final altitude target
+///     should be called continuously (with dt set to be the expected time between calls)
+///     actual position target will be moved no faster than the speed_down and speed_up
+///     target will also be stopped if the motors hit their limits or leash length is exceeded
+void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt)
+{
+    float alt_change = alt_cm - _pos_target.z;
+
+    // do not use z-axis desired velocity feed forward
+    _flags.use_desvel_ff_z = false;
+
+    // adjust desired alt if motors have not hit their limits
+    if ((alt_change < 0 && !_motors.limit.throttle_lower) || (alt_change > 0 && !_motors.limit.throttle_upper)) {
+        if (!is_zero(dt)) {
+            float climb_rate_cms = constrain_float(alt_change / dt, _speed_down_cms, _speed_up_cms);
+            _pos_target.z += climb_rate_cms * dt;
+            _vel_desired.z = climb_rate_cms;  // recorded for reporting purposes
+        }
+    } else {
+        // recorded for reporting purposes
+        _vel_desired.z = 0.0f;
+    }
+
+    // do not let target get too far from current altitude
+    float curr_alt = _inav.get_altitude();
+    _pos_target.z = constrain_float(_pos_target.z, curr_alt - _leash_down_z, curr_alt + _leash_up_z);
+}
+
+/// set_alt_target_from_climb_rate - adjusts target up or down using a climb rate in cm/s
+///     should be called continuously (with dt set to be the expected time between calls)
+///     actual position target will be moved no faster than the speed_down and speed_up
+///     target will also be stopped if the motors hit their limits or leash length is exceeded
+void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend)
+{
+    // adjust desired alt if motors have not hit their limits
+    // To-Do: add check of _limit.pos_down?
+    if ((climb_rate_cms < 0 && (!_motors.limit.throttle_lower || force_descend)) || (climb_rate_cms > 0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {
+        _pos_target.z += climb_rate_cms * dt;
+    }
+
+    // do not use z-axis desired velocity feed forward
+    // vel_desired set to desired climb rate for reporting and land-detector
+    _flags.use_desvel_ff_z = false;
+    _vel_desired.z = climb_rate_cms;
+}
+
+/// set_alt_target_from_climb_rate_ff - adjusts target up or down using a climb rate in cm/s using feed-forward
+///     should be called continuously (with dt set to be the expected time between calls)
+///     actual position target will be moved no faster than the speed_down and speed_up
+///     target will also be stopped if the motors hit their limits or leash length is exceeded
+///     set force_descend to true during landing to allow target to move low enough to slow the motors
+void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)
+{
+    // calculated increased maximum acceleration if over speed
+    float accel_z_cms = _accel_z_cms;
+    if (_vel_desired.z < _speed_down_cms && !is_zero(_speed_down_cms)) {
+        accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_down_cms;
+    }
+    if (_vel_desired.z > _speed_up_cms && !is_zero(_speed_up_cms)) {
+        accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_up_cms;
+    }
+    accel_z_cms = constrain_float(accel_z_cms, 0.0f, 750.0f);
+
+    // jerk_z is calculated to reach full acceleration in 1000ms.
+    float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO;
+
+    float accel_z_max = MIN(accel_z_cms, safe_sqrt(2.0f * fabsf(_vel_desired.z - climb_rate_cms) * jerk_z));
+
+    _accel_last_z_cms += jerk_z * dt;
+    _accel_last_z_cms = MIN(accel_z_max, _accel_last_z_cms);
+
+    float vel_change_limit = _accel_last_z_cms * dt;
+    _vel_desired.z = constrain_float(climb_rate_cms, _vel_desired.z - vel_change_limit, _vel_desired.z + vel_change_limit);
+    _flags.use_desvel_ff_z = true;
+
+    // adjust desired alt if motors have not hit their limits
+    // To-Do: add check of _limit.pos_down?
+    if ((_vel_desired.z < 0 && (!_motors.limit.throttle_lower || force_descend)) || (_vel_desired.z > 0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {
+        _pos_target.z += _vel_desired.z * dt;
+    }
+}
+
+/// add_takeoff_climb_rate - adjusts alt target up or down using a climb rate in cm/s
+///     should be called continuously (with dt set to be the expected time between calls)
+///     almost no checks are performed on the input
+void AC_PosControl::add_takeoff_climb_rate(float climb_rate_cms, float dt)
+{
+    _pos_target.z += climb_rate_cms * dt;
+}
+
+/// shift altitude target (positive means move altitude up)
+void AC_PosControl::shift_alt_target(float z_cm)
+{
+    _pos_target.z += z_cm;
+
+    // freeze feedforward to avoid jump
+    if (!is_zero(z_cm)) {
+        freeze_ff_z();
+    }
+}
+
+/// relax_alt_hold_controllers - set all desired and targets to measured
+void AC_PosControl::relax_alt_hold_controllers(float throttle_setting)
+{
+    _pos_target.z = _inav.get_altitude();
+    _vel_desired.z = 0.0f;
+    _flags.use_desvel_ff_z = false;
+    _vel_target.z = _inav.get_velocity_z();
+    _vel_last.z = _inav.get_velocity_z();
+    _accel_desired.z = 0.0f;
+    _accel_last_z_cms = 0.0f;
+    _flags.reset_rate_to_accel_z = true;
+    _pid_accel_z.set_integrator((throttle_setting - _motors.get_throttle_hover()) * 1000.0f);
+    _accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
+    _pid_accel_z.reset_filter();
+}
+
+// get_alt_error - returns altitude error in cm
+float AC_PosControl::get_alt_error() const
+{
+    return (_pos_target.z - _inav.get_altitude());
+}
+
+/// set_target_to_stopping_point_z - returns reasonable stopping altitude in cm above home
+void AC_PosControl::set_target_to_stopping_point_z()
+{
+    // check if z leash needs to be recalculated
+    calc_leash_length_z();
+
+    get_stopping_point_z(_pos_target);
+}
+
+/// get_stopping_point_z - calculates stopping point based on current position, velocity, vehicle acceleration
+void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const
+{
+    const float curr_pos_z = _inav.get_altitude();
+    float curr_vel_z = _inav.get_velocity_z();
+
+    float linear_distance; // half the distance we swap between linear and sqrt and the distance we offset sqrt
+    float linear_velocity;  // the velocity we swap between linear and sqrt
+
+    // if position controller is active add current velocity error to avoid sudden jump in acceleration
+    if (is_active_z()) {
+        curr_vel_z += _vel_error.z;
+        if (_flags.use_desvel_ff_z) {
+            curr_vel_z -= _vel_desired.z;
+        }
+    }
+
+    // avoid divide by zero by using current position if kP is very low or acceleration is zero
+    if (_p_pos_z.kP() <= 0.0f || _accel_z_cms <= 0.0f) {
+        stopping_point.z = curr_pos_z;
+        return;
+    }
+
+    // calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
+    linear_velocity = _accel_z_cms / _p_pos_z.kP();
+
+    if (fabsf(curr_vel_z) < linear_velocity) {
+        // if our current velocity is below the cross-over point we use a linear function
+        stopping_point.z = curr_pos_z + curr_vel_z / _p_pos_z.kP();
+    } else {
+        linear_distance = _accel_z_cms / (2.0f * _p_pos_z.kP() * _p_pos_z.kP());
+        if (curr_vel_z > 0) {
+            stopping_point.z = curr_pos_z + (linear_distance + curr_vel_z * curr_vel_z / (2.0f * _accel_z_cms));
+        } else {
+            stopping_point.z = curr_pos_z - (linear_distance + curr_vel_z * curr_vel_z / (2.0f * _accel_z_cms));
+        }
+    }
+    stopping_point.z = constrain_float(stopping_point.z, curr_pos_z - POSCONTROL_STOPPING_DIST_DOWN_MAX, curr_pos_z + POSCONTROL_STOPPING_DIST_UP_MAX);
+}
+
+/// init_takeoff - initialises target altitude if we are taking off
+void AC_PosControl::init_takeoff()
+{
+    const Vector3f& curr_pos = _inav.get_position();
+
+    _pos_target.z = curr_pos.z;
+
+    // freeze feedforward to avoid jump
+    freeze_ff_z();
+
+    // shift difference between last motor out and hover throttle into accelerometer I
+    _pid_accel_z.set_integrator((_attitude_control.get_throttle_in() - _motors.get_throttle_hover()) * 1000.0f);
+
+    // initialise ekf reset handler
+    init_ekf_z_reset();
+}
+
+// is_active_z - returns true if the z-axis position controller has been run very recently
+bool AC_PosControl::is_active_z() const
+{
+    return ((AP_HAL::micros64() - _last_update_z_us) <= POSCONTROL_ACTIVE_TIMEOUT_US);
+}
+
+/// update_z_controller - fly to altitude in cm above home
+void AC_PosControl::update_z_controller()
+{
+    // check time since last cast
+    const uint64_t now_us = AP_HAL::micros64();
+    if (now_us - _last_update_z_us > POSCONTROL_ACTIVE_TIMEOUT_US) {
+        _flags.reset_rate_to_accel_z = true;
+        _pid_accel_z.set_integrator((_attitude_control.get_throttle_in() - _motors.get_throttle_hover()) * 1000.0f);
+        _accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
+        _pid_accel_z.reset_filter();
+    }
+    _last_update_z_us = now_us;
+
+    // check for ekf altitude reset
+    //check_for_ekf_z_reset();
+
+    // check if leash lengths need to be recalculated
+    calc_leash_length_z();
+
+    // call z-axis position controller
+   run_z_controller();
+}
+float AC_PosControl::update_z_controller_f()
+{
+    // check time since last cast
+    const uint64_t now_us = AP_HAL::micros64();
+    if (now_us - _last_update_z_us > POSCONTROL_ACTIVE_TIMEOUT_US) {
+        _flags.reset_rate_to_accel_z = true;
+        _pid_accel_z.set_integrator((_attitude_control.get_throttle_in() - _motors.get_throttle_hover()) * 1000.0f);
+        _accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
+        _pid_accel_z.reset_filter();
+    }
+    _last_update_z_us = now_us;
+
+    // check for ekf altitude reset
+    //check_for_ekf_z_reset();
+
+    // check if leash lengths need to be recalculated
+    calc_leash_length_z();
+
+    // call z-axis position controller
+   return run_z_controller_f();
+}
+
+
+/// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
+///     called by update_z_controller if z-axis speed or accelerations are changed
+void AC_PosControl::calc_leash_length_z()
+{
+    if (_flags.recalc_leash_z) {
+        _leash_up_z = calc_leash_length(_speed_up_cms, _accel_z_cms, _p_pos_z.kP());
+        _leash_down_z = calc_leash_length(-_speed_down_cms, _accel_z_cms, _p_pos_z.kP());
+        _flags.recalc_leash_z = false;
+    }
+}
+
+// run position control for Z axis
+// target altitude should be set with one of these functions: set_alt_target, set_target_to_stopping_point_z, init_takeoff
+// calculates desired rate in earth-frame z axis and passes to rate controller
+// vel_up_max, vel_down_max should have already been set before calling this method
+void AC_PosControl::run_z_controller()
+{
+    //float curr_alt = _inav.get_altitude();//12.19
+	float curr_alt = sub.barometer.get_altitude()*100;//_inav.get_altitude();//12.19
+	
+
+	
+    // clear position limit flags
+    _limit.pos_up = false;
+    _limit.pos_down = false;
+
+    // calculate altitude error
+    _pos_error.z = _pos_target.z - curr_alt;
+
+    // do not let target altitude get too far from current altitude
+    if (_pos_error.z > _leash_up_z) {
+        _pos_target.z = curr_alt + _leash_up_z;
+        _pos_error.z = _leash_up_z;
+        _limit.pos_up = true;
+    }
+    if (_pos_error.z < -_leash_down_z) {
+        _pos_target.z = curr_alt - _leash_down_z;
+        _pos_error.z = -_leash_down_z;
+        _limit.pos_down = true;
+    }
+
+    // calculate _vel_target.z using from _pos_error.z using sqrt controller
+    _vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms, _dt);
+
+    // check speed limits
+    // To-Do: check these speed limits here or in the pos->rate controller
+    _limit.vel_up = false;
+    _limit.vel_down = false;
+    if (_vel_target.z < _speed_down_cms) {
+        _vel_target.z = _speed_down_cms;
+        _limit.vel_down = true;
+    }
+    if (_vel_target.z > _speed_up_cms) {
+        _vel_target.z = _speed_up_cms;
+        _limit.vel_up = true;
+    }
+
+    // add feed forward component
+    if (_flags.use_desvel_ff_z) {
+        _vel_target.z += _vel_desired.z;
+    }
+
+	
+    // the following section calculates acceleration required to achieve the velocity target
+
+    //const Vector3f& curr_vel = _inav.get_velocity();//12.19
+    
+	Vector3f curr_vel;//12.19
+	
+	curr_vel.z=alt_rate.get();
+
+
+    // TODO: remove velocity derivative calculation
+    // reset last velocity target to current target
+    if (_flags.reset_rate_to_accel_z) {//改变高度时候复位
+        _vel_last.z = _vel_target.z;
+    }
+
+    // feed forward desired acceleration calculation
+    if (_dt > 0.0f) {
+        if (!_flags.freeze_ff_z) {
+            _accel_desired.z = (_vel_target.z - _vel_last.z) / _dt;
+        } else {
+            // stop the feed forward being calculated during a known discontinuity
+            _flags.freeze_ff_z = false;
+        }
+    } else {
+        _accel_desired.z = 0.0f;
+    }
+
+
+    // store this iteration's velocities for the next iteration
+    _vel_last.z = _vel_target.z;
+
+    // reset velocity error and filter if this controller has just been engaged
+    if (_flags.reset_rate_to_accel_z) {
+        // Reset Filter
+        _vel_error.z = 0;
+        _vel_error_filter.reset(0);
+		_accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;//02.28
+        _flags.reset_rate_to_accel_z = false;
+    } else {
+        // calculate rate error and filter with cut off frequency of 2 Hz
+        _vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z, _dt);
+		 _accel_target.z = _p_vel_z.get_p(_vel_error.z);//02.28
+    }
+
+   
+
+    _accel_target.z += _accel_desired.z;
+
+
+    // the following section calculates a desired throttle needed to achieve the acceleration target
+    float z_accel_meas;         // actual acceleration
+
+    // Calculate Earth Frame Z acceleration
+    z_accel_meas = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
+
+    // ensure imax is always large enough to overpower hover throttle
+    if (_motors.get_throttle_hover() * 1000.0f > _pid_accel_z.imax()) {
+        _pid_accel_z.imax(_motors.get_throttle_hover() * 1000.0f);
+    }
+
+   // float thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f +_motors.get_throttle_hover();
+	float thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f +0.5;
+
+    // send throttle to attitude controller with angle boost
+    _attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);
+	static uint8_t cnt = 0;
+	cnt++;
+	if (cnt>250)
+	{
+		cnt = 0;
+		
+		//gcs().send_text(MAV_SEVERITY_INFO, "  ff %d %d %f\n",(int)_flags.use_desvel_ff_z,(int)_flags.freeze_ff_z,_motors.get_throttle_hover());
+		//gcs().send_text(MAV_SEVERITY_INFO, "  pos %f %f %f %f\n",_pos_error.z,_vel_target.z,curr_vel.z,_accel_desired.z);
+		gcs().send_text(MAV_SEVERITY_INFO, "  pid %f %f %f %f\n",_accel_target.z,z_accel_meas,_accel_desired.z,thr_out);
+
+	}
+
+
+}
+
+
+
+float AC_PosControl::run_z_controller_wangdan(float _vel)
+{
+//超时复位
+       const uint64_t now_us = AP_HAL::micros64();
+    if (now_us - _last_update_z_us > POSCONTROL_ACTIVE_TIMEOUT_US) {
+        _flags.reset_rate_to_accel_z = true;
+        _pid_accel_z.set_integrator((_attitude_control.get_throttle_in() - _motors.get_throttle_hover()) * 1000.0f);
+        _accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
+        _pid_accel_z.reset_filter();
+    }
+    _last_update_z_us = now_us;
+//速度环
+
+	
+	_vel_target.z = _vel;
+
+    // add feed forward component
+    if (_flags.use_desvel_ff_z) {
+        _vel_target.z += _vel_desired.z;
+    }
+
+	
+    // the following section calculates acceleration required to achieve the velocity target
+
+    //const Vector3f& curr_vel = _inav.get_velocity();//12.19
+    
+	Vector3f curr_vel;//12.19
+	
+	curr_vel.z=alt_rate.get();
+
+
+    // TODO: remove velocity derivative calculation
+    // reset last velocity target to current target
+    if (_flags.reset_rate_to_accel_z) {//改变高度时候复位
+        _vel_last.z = _vel_target.z;
+    }
+
+    // feed forward desired acceleration calculation
+    if (_dt > 0.0f) {
+        if (!_flags.freeze_ff_z) {
+            _accel_desired.z = (_vel_target.z - _vel_last.z) / _dt;
+        } else {
+            // stop the feed forward being calculated during a known discontinuity
+            _flags.freeze_ff_z = false;
+        }
+    } else {
+        _accel_desired.z = 0.0f;
+    }
+
+
+    // store this iteration's velocities for the next iteration
+    _vel_last.z = _vel_target.z;
+
+    // reset velocity error and filter if this controller has just been engaged
+    if (_flags.reset_rate_to_accel_z) {
+        // Reset Filter
+        _vel_error.z = 0;
+        _vel_error_filter.reset(0);
+		_accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;//02.28
+        _flags.reset_rate_to_accel_z = false;
+    } else {
+        // calculate rate error and filter with cut off frequency of 2 Hz
+        _vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z, _dt);
+		 _accel_target.z = _p_vel_z.get_p(_vel_error.z);//02.28
+    }
+
+   
+
+    _accel_target.z += _accel_desired.z;
+
+
+    // the following section calculates a desired throttle needed to achieve the acceleration target
+    float z_accel_meas;         // actual acceleration
+
+    // Calculate Earth Frame Z acceleration
+    z_accel_meas = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
+
+    // ensure imax is always large enough to overpower hover throttle
+    if (_motors.get_throttle_hover() * 1000.0f > _pid_accel_z.imax()) {
+        _pid_accel_z.imax(_motors.get_throttle_hover() * 1000.0f);
+    }
+
+   // float thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f +_motors.get_throttle_hover();
+	float thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f;
+
+    // send throttle to attitude controller with angle boost
+    //_attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);
+	static uint8_t cnt = 0;
+	cnt++;
+	if (cnt>250)
+	{
+		cnt = 0;
+		
+		//gcs().send_text(MAV_SEVERITY_INFO, "  ff %d %d %f\n",(int)_flags.use_desvel_ff_z,(int)_flags.freeze_ff_z,_motors.get_throttle_hover());
+		//gcs().send_text(MAV_SEVERITY_INFO, "  pos %f %f %f %f\n",_pos_error.z,_vel_target.z,curr_vel.z,_accel_desired.z);
+		gcs().send_text(MAV_SEVERITY_INFO, "  pid %f %f %f %f\n",_vel_target.z,curr_vel.z,_accel_target.z,thr_out);
+
+	}
+	return thr_out;
+
+}
+void AC_PosControl::calculate_rate(float dt){
+	LowPassFilterFloat curr_alt(10.0);
+
+	curr_alt.apply(sub.barometer.get_altitude()*100,dt);//_inav.get_altitude();//12.19
+
+	alt_rate.apply((curr_alt.get()-last_alt)/dt,dt);//12.19 cm
+	last_alt = curr_alt.get();//12.19
+	
+
+}
+
+float AC_PosControl::run_z_controller_f()
+{
+    //float curr_alt = _inav.get_altitude();//12.19
+	
+	float curr_alt = sub.barometer.get_altitude()*100;//_inav.get_altitude();//12.19
+    // clear position limit flags
+    _limit.pos_up = false;
+    _limit.pos_down = false;
+
+    // calculate altitude error
+    _pos_error.z = _pos_target.z - curr_alt;
+
+    // do not let target altitude get too far from current altitude
+    if (_pos_error.z > _leash_up_z) {
+        _pos_target.z = curr_alt + _leash_up_z;
+        _pos_error.z = _leash_up_z;
+        _limit.pos_up = true;
+    }
+    if (_pos_error.z < -_leash_down_z) {
+        _pos_target.z = curr_alt - _leash_down_z;
+        _pos_error.z = -_leash_down_z;
+        _limit.pos_down = true;
+    }
+
+    // calculate _vel_target.z using from _pos_error.z using sqrt controller
+    _vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms, _dt);
+
+    // check speed limits
+    // To-Do: check these speed limits here or in the pos->rate controller
+    _limit.vel_up = false;
+    _limit.vel_down = false;
+    if (_vel_target.z < _speed_down_cms) {
+        _vel_target.z = _speed_down_cms;
+        _limit.vel_down = true;
+    }
+    if (_vel_target.z > _speed_up_cms) {
+        _vel_target.z = _speed_up_cms;
+        _limit.vel_up = true;
+    }
+
+    // add feed forward component
+    float ff =(float)SRV_Channels::srv_channel(15)->get_output_max()/10000;//test
+  
+	static float ff_ratio =  ff;
+	
+	
+    if (_flags.use_desvel_ff_z) {	
+		
+       
+	     if (fabsf(_vel_desired.z)>0)
+		{
+			
+			ff_ratio = constrain_float(ff_ratio*0.997,0.0,2.5);
+			 _vel_target.z += _vel_desired.z*(1+ff_ratio);//前馈加系数试试
+			
+			
+	
+		}else{
+			//没有前馈控制
+			
+			ff_ratio = ff;
+			}
+    }else{
+	    
+		ff_ratio = ff;
+    	}
+	
+
+
+	
+    // the following section calculates acceleration required to achieve the velocity target
+
+    //const Vector3f& curr_vel = _inav.get_velocity();//12.19
+    
+	Vector3f curr_vel;//12.19
+	
+	curr_vel.z=alt_rate.get();
+
+
+    // TODO: remove velocity derivative calculation
+    // reset last velocity target to current target
+    if (_flags.reset_rate_to_accel_z) {//改变高度时候复位
+        _vel_last.z = _vel_target.z;
+    }
+
+    // feed forward desired acceleration calculation
+    if (_dt > 0.0f) {
+        if (!_flags.freeze_ff_z) {
+            _accel_desired.z = (_vel_target.z - _vel_last.z) / _dt;
+        } else {
+            // stop the feed forward being calculated during a known discontinuity
+            _flags.freeze_ff_z = false;
+        }
+    } else {
+        _accel_desired.z = 0.0f;
+    }
+
+
+    // store this iteration's velocities for the next iteration
+    _vel_last.z = _vel_target.z;
+	
+    // reset velocity error and filter if this controller has just been engaged
+    if (_flags.reset_rate_to_accel_z) {
+        // Reset Filter
+        _vel_error.z = 0;
+        _vel_error_filter.reset(0);
+		_accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;//02.28
+		
+        _flags.reset_rate_to_accel_z = false;
+    } else {
+        // calculate rate error and filter with cut off frequency of 2 Hz
+        _vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z, _dt);
+		 _accel_target.z = _p_vel_z.get_p(_vel_error.z);//02.28
+		
+    }
+
+   
+
+    _accel_target.z += _accel_desired.z;
+
+
+    // the following section calculates a desired throttle needed to achieve the acceleration target
+    float z_accel_meas;         // actual acceleration
+
+    // Calculate Earth Frame Z acceleration
+    z_accel_meas = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
+
+    // ensure imax is always large enough to overpower hover throttle
+    if (_motors.get_throttle_hover() * 1000.0f > _pid_accel_z.imax()) {
+        _pid_accel_z.imax(_motors.get_throttle_hover() * 1000.0f);
+    }
+
+   // float thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f +_motors.get_throttle_hover();
+	float thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f;
+	
+
+	//如果加一个当前速度前馈的话最多可以用0.002
+    // send throttle to attitude controller with angle boost
+//    _attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);
+	static uint16_t count = 0;
+	count++;
+	if (count>200)
+	{
+		count = 0;
+		gcs().send_text(MAV_SEVERITY_INFO, "ff %f %f %f \n",_vel_target.z,_accel_target.z,z_accel_meas);
+	
+	}
+
+
+	return thr_out;
+}
+
+///
+/// lateral position controller
+///
+
+/// set_max_accel_xy - set the maximum horizontal acceleration in cm/s/s
+void AC_PosControl::set_max_accel_xy(float accel_cmss)
+{
+    if (fabsf(_accel_cms - accel_cmss) > 1.0f) {
+        _accel_cms = accel_cmss;
+        _flags.recalc_leash_xy = true;
+        calc_leash_length_xy();
+    }
+}
+
+/// set_max_speed_xy - set the maximum horizontal speed maximum in cm/s
+void AC_PosControl::set_max_speed_xy(float speed_cms)
+{
+    if (fabsf(_speed_cms - speed_cms) > 1.0f) {
+        _speed_cms = speed_cms;
+        _flags.recalc_leash_xy = true;
+        calc_leash_length_xy();
+    }
+}
+
+/// set_pos_target in cm from home
+void AC_PosControl::set_pos_target(const Vector3f& position)
+{
+    _pos_target = position;
+
+    _flags.use_desvel_ff_z = false;
+    _vel_desired.z = 0.0f;
+    // initialise roll and pitch to current roll and pitch.  This avoids a twitch between when the target is set and the pos controller is first run
+    // To-Do: this initialisation of roll and pitch targets needs to go somewhere between when pos-control is initialised and when it completes it's first cycle
+    //_roll_target = constrain_int32(_ahrs.roll_sensor,-_attitude_control.lean_angle_max(),_attitude_control.lean_angle_max());
+    //_pitch_target = constrain_int32(_ahrs.pitch_sensor,-_attitude_control.lean_angle_max(),_attitude_control.lean_angle_max());
+}
+
+/// set_xy_target in cm from home
+void AC_PosControl::set_xy_target(float x, float y)
+{
+    _pos_target.x = x;
+    _pos_target.y = y;
+}
+
+/// shift position target target in x, y axis
+void AC_PosControl::shift_pos_xy_target(float x_cm, float y_cm)
+{
+    // move pos controller target
+    _pos_target.x += x_cm;
+    _pos_target.y += y_cm;
+}
+
+/// set_target_to_stopping_point_xy - sets horizontal target to reasonable stopping position in cm from home
+void AC_PosControl::set_target_to_stopping_point_xy()
+{
+    // check if xy leash needs to be recalculated
+    calc_leash_length_xy();
+
+    get_stopping_point_xy(_pos_target);
+}
+
+/// get_stopping_point_xy - calculates stopping point based on current position, velocity, vehicle acceleration
+///     distance_max allows limiting distance to stopping point
+///     results placed in stopping_position vector
+///     set_max_accel_xy() should be called before this method to set vehicle acceleration
+///     set_leash_length() should have been called before this method
+void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const
+{
+    const Vector3f curr_pos = _inav.get_position();
+    Vector3f curr_vel = _inav.get_velocity();
+    float linear_distance;      // the distance at which we swap from a linear to sqrt response
+    float linear_velocity;      // the velocity above which we swap from a linear to sqrt response
+    float stopping_dist;	    // the distance within the vehicle can stop
+    float kP = _p_pos_xy.kP();
+
+    // add velocity error to current velocity
+    if (is_active_xy()) {
+        curr_vel.x += _vel_error.x;
+        curr_vel.y += _vel_error.y;
+    }
+
+    // calculate current velocity
+    float vel_total = norm(curr_vel.x, curr_vel.y);
+
+    // avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
+    if (kP <= 0.0f || _accel_cms <= 0.0f || is_zero(vel_total)) {
+        stopping_point.x = curr_pos.x;
+        stopping_point.y = curr_pos.y;
+        return;
+    }
+
+    // calculate point at which velocity switches from linear to sqrt
+    linear_velocity = _accel_cms / kP;
+
+    // calculate distance within which we can stop
+    if (vel_total < linear_velocity) {
+        stopping_dist = vel_total / kP;
+    } else {
+        linear_distance = _accel_cms / (2.0f * kP * kP);
+        stopping_dist = linear_distance + (vel_total * vel_total) / (2.0f * _accel_cms);
+    }
+
+    // constrain stopping distance
+    stopping_dist = constrain_float(stopping_dist, 0, _leash);
+
+    // convert the stopping distance into a stopping point using velocity vector
+    stopping_point.x = curr_pos.x + (stopping_dist * curr_vel.x / vel_total);
+    stopping_point.y = curr_pos.y + (stopping_dist * curr_vel.y / vel_total);
+}
+
+/// get_distance_to_target - get horizontal distance to target position in cm
+float AC_PosControl::get_distance_to_target() const
+{
+    return norm(_pos_error.x, _pos_error.y);
+}
+
+/// get_bearing_to_target - get bearing to target position in centi-degrees
+int32_t AC_PosControl::get_bearing_to_target() const
+{
+    return get_bearing_cd(_inav.get_position(), _pos_target);
+}
+
+// is_active_xy - returns true if the xy position controller has been run very recently
+bool AC_PosControl::is_active_xy() const
+{
+    return ((AP_HAL::micros64() - _last_update_xy_us) <= POSCONTROL_ACTIVE_TIMEOUT_US);
+}
+
+/// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request
+float AC_PosControl::get_lean_angle_max_cd() const
+{
+    if (is_zero(_lean_angle_max)) {
+        return _attitude_control.lean_angle_max();
+    }
+    return _lean_angle_max * 100.0f;
+}
+
+/// init_xy_controller - initialise the xy controller
+///     this should be called after setting the position target and the desired velocity and acceleration
+///     sets target roll angle, pitch angle and I terms based on vehicle current lean angles
+///     should be called once whenever significant changes to the position target are made
+///     this does not update the xy target
+void AC_PosControl::init_xy_controller()
+{
+    // set roll, pitch lean angle targets to current attitude
+    // todo: this should probably be based on the desired attitude not the current attitude
+    _roll_target = _ahrs.roll_sensor;
+    _pitch_target = _ahrs.pitch_sensor;
+
+    // initialise I terms from lean angles
+    _pid_vel_xy.reset_filter();
+    lean_angles_to_accel(_accel_target.x, _accel_target.y);
+    _pid_vel_xy.set_integrator(_accel_target - _accel_desired);
+
+    // flag reset required in rate to accel step
+    _flags.reset_desired_vel_to_pos = true;
+    _flags.reset_accel_to_lean_xy = true;
+
+    // initialise ekf xy reset handler
+    init_ekf_xy_reset();
+}
+
+/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
+void AC_PosControl::update_xy_controller()
+{
+    // compute dt
+    const uint64_t now_us = AP_HAL::micros64();
+    float dt = (now_us - _last_update_xy_us) * 1.0e-6f;
+
+    // sanity check dt
+    if (dt >= POSCONTROL_ACTIVE_TIMEOUT_US * 1.0e-6f) {
+        dt = 0.0f;
+    }
+
+    // check for ekf xy position reset
+    check_for_ekf_xy_reset();
+
+    // check if xy leash needs to be recalculated
+    calc_leash_length_xy();
+
+    // translate any adjustments from pilot to loiter target
+    desired_vel_to_pos(dt);
+
+    // run horizontal position controller
+    run_xy_controller(dt);
+
+    // update xy update time
+    _last_update_xy_us = now_us;
+}
+
+float AC_PosControl::time_since_last_xy_update() const
+{
+    const uint64_t now_us = AP_HAL::micros64();
+    return (now_us - _last_update_xy_us) * 1.0e-6f;
+}
+
+// write log to dataflash
+void AC_PosControl::write_log()
+{
+    const Vector3f &pos_target = get_pos_target();
+    const Vector3f &vel_target = get_vel_target();
+    const Vector3f &accel_target = get_accel_target();
+    const Vector3f &position = _inav.get_position();
+    const Vector3f &velocity = _inav.get_velocity();
+    float accel_x, accel_y;
+    lean_angles_to_accel(accel_x, accel_y);
+
+    AP::logger().Write("PSC",
+                       "TimeUS,TPX,TPY,PX,PY,TVX,TVY,VX,VY,TAX,TAY,AX,AY",
+                       "smmmmnnnnoooo",
+                       "F000000000000",
+                       "Qffffffffffff",
+                       AP_HAL::micros64(),
+                       double(pos_target.x * 0.01f),
+                       double(pos_target.y * 0.01f),
+                       double(position.x * 0.01f),
+                       double(position.y * 0.01f),
+                       double(vel_target.x * 0.01f),
+                       double(vel_target.y * 0.01f),
+                       double(velocity.x * 0.01f),
+                       double(velocity.y * 0.01f),
+                       double(accel_target.x * 0.01f),
+                       double(accel_target.y * 0.01f),
+                       double(accel_x * 0.01f),
+                       double(accel_y * 0.01f));
+}
+
+/// init_vel_controller_xyz - initialise the velocity controller - should be called once before the caller attempts to use the controller
+void AC_PosControl::init_vel_controller_xyz()
+{
+    // set roll, pitch lean angle targets to current attitude
+    _roll_target = _ahrs.roll_sensor;
+    _pitch_target = _ahrs.pitch_sensor;
+
+    _pid_vel_xy.reset_filter();
+    lean_angles_to_accel(_accel_target.x, _accel_target.y);
+    _pid_vel_xy.set_integrator(_accel_target);
+
+    // flag reset required in rate to accel step
+    _flags.reset_desired_vel_to_pos = true;
+    _flags.reset_accel_to_lean_xy = true;
+
+    // set target position
+    const Vector3f& curr_pos = _inav.get_position();
+    set_xy_target(curr_pos.x, curr_pos.y);
+    set_alt_target(curr_pos.z);
+
+    // move current vehicle velocity into feed forward velocity
+    const Vector3f& curr_vel = _inav.get_velocity();
+    set_desired_velocity(curr_vel);
+
+    // set vehicle acceleration to zero
+    set_desired_accel_xy(0.0f, 0.0f);
+
+    // initialise ekf reset handlers
+    init_ekf_xy_reset();
+    init_ekf_z_reset();
+}
+
+/// update_velocity_controller_xy - run the velocity controller - should be called at 100hz or higher
+///     velocity targets should we set using set_desired_velocity_xy() method
+///     callers should use get_roll() and get_pitch() methods and sent to the attitude controller
+///     throttle targets will be sent directly to the motors
+void AC_PosControl::update_vel_controller_xy()
+{
+    // capture time since last iteration
+    const uint64_t now_us = AP_HAL::micros64();
+    float dt = (now_us - _last_update_xy_us) * 1.0e-6f;
+
+    // sanity check dt
+    if (dt >= 0.2f) {
+        dt = 0.0f;
+    }
+
+    // check for ekf xy position reset
+    check_for_ekf_xy_reset();
+
+    // check if xy leash needs to be recalculated
+    calc_leash_length_xy();
+
+    // apply desired velocity request to position target
+    // TODO: this will need to be removed and added to the calling function.
+    desired_vel_to_pos(dt);
+
+    // run position controller
+    run_xy_controller(dt);
+
+    // update xy update time
+    _last_update_xy_us = now_us;
+}
+
+/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher
+///     velocity targets should we set using set_desired_velocity_xyz() method
+///     callers should use get_roll() and get_pitch() methods and sent to the attitude controller
+///     throttle targets will be sent directly to the motors
+void AC_PosControl::update_vel_controller_xyz()
+{
+    update_vel_controller_xy();
+
+    // update altitude target
+    set_alt_target_from_climb_rate_ff(_vel_desired.z, _dt, false);
+
+    // run z-axis position controller
+    update_z_controller();
+}
+
+float AC_PosControl::get_horizontal_error() const
+{
+    return norm(_pos_error.x, _pos_error.y);
+}
+
+///
+/// private methods
+///
+
+/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration
+///     should be called whenever the speed, acceleration or position kP is modified
+void AC_PosControl::calc_leash_length_xy()
+{
+    // todo: remove _flags.recalc_leash_xy or don't call this function after each variable change.
+    if (_flags.recalc_leash_xy) {
+        _leash = calc_leash_length(_speed_cms, _accel_cms, _p_pos_xy.kP());
+        _flags.recalc_leash_xy = false;
+    }
+}
+
+/// move velocity target using desired acceleration
+void AC_PosControl::desired_accel_to_vel(float nav_dt)
+{
+    // range check nav_dt
+    if (nav_dt < 0) {
+        return;
+    }
+
+    // update target velocity
+    if (_flags.reset_desired_vel_to_pos) {
+        _flags.reset_desired_vel_to_pos = false;
+    } else {
+        _vel_desired.x += _accel_desired.x * nav_dt;
+        _vel_desired.y += _accel_desired.y * nav_dt;
+    }
+}
+
+/// desired_vel_to_pos - move position target using desired velocities
+void AC_PosControl::desired_vel_to_pos(float nav_dt)
+{
+    // range check nav_dt
+    if (nav_dt < 0) {
+        return;
+    }
+
+    // update target position
+    if (_flags.reset_desired_vel_to_pos) {
+        _flags.reset_desired_vel_to_pos = false;
+    } else {
+        _pos_target.x += _vel_desired.x * nav_dt;
+        _pos_target.y += _vel_desired.y * nav_dt;
+    }
+}
+
+/// run horizontal position controller correcting position and velocity
+///     converts position (_pos_target) to target velocity (_vel_target)
+///     desired velocity (_vel_desired) is combined into final target velocity
+///     converts desired velocities in lat/lon directions to accelerations in lat/lon frame
+///     converts desired accelerations provided in lat/lon frame to roll/pitch angles
+void AC_PosControl::run_xy_controller(float dt)
+{
+    float ekfGndSpdLimit, ekfNavVelGainScaler;
+    AP::ahrs_navekf().getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
+
+    Vector3f curr_pos = _inav.get_position();
+    float kP = ekfNavVelGainScaler * _p_pos_xy.kP(); // scale gains to compensate for noisy optical flow measurement in the EKF
+
+    // avoid divide by zero
+    if (kP <= 0.0f) {
+        _vel_target.x = 0.0f;
+        _vel_target.y = 0.0f;
+    } else {
+        // calculate distance error
+        _pos_error.x = _pos_target.x - curr_pos.x;
+        _pos_error.y = _pos_target.y - curr_pos.y;
+
+        // Constrain _pos_error and target position
+        // Constrain the maximum length of _vel_target to the maximum position correction velocity
+        // TODO: replace the leash length with a user definable maximum position correction
+        if (limit_vector_length(_pos_error.x, _pos_error.y, _leash)) {
+            _pos_target.x = curr_pos.x + _pos_error.x;
+            _pos_target.y = curr_pos.y + _pos_error.y;
+        }
+
+        _vel_target = sqrt_controller(_pos_error, kP, _accel_cms);
+    }
+
+    // add velocity feed-forward
+    _vel_target.x += _vel_desired.x;
+    _vel_target.y += _vel_desired.y;
+
+    // the following section converts desired velocities in lat/lon directions to accelerations in lat/lon frame
+
+    Vector2f accel_target, vel_xy_p, vel_xy_i, vel_xy_d;
+
+    // check if vehicle velocity is being overridden
+    if (_flags.vehicle_horiz_vel_override) {
+        _flags.vehicle_horiz_vel_override = false;
+    } else {
+        _vehicle_horiz_vel.x = _inav.get_velocity().x;
+        _vehicle_horiz_vel.y = _inav.get_velocity().y;
+    }
+
+    // calculate velocity error
+    _vel_error.x = _vel_target.x - _vehicle_horiz_vel.x;
+    _vel_error.y = _vel_target.y - _vehicle_horiz_vel.y;
+    // TODO: constrain velocity error and velocity target
+
+    // call pi controller
+    _pid_vel_xy.set_input(_vel_error);
+
+    // get p
+    vel_xy_p = _pid_vel_xy.get_p();
+
+    // update i term if we have not hit the accel or throttle limits OR the i term will reduce
+    // TODO: move limit handling into the PI and PID controller
+    if (!_limit.accel_xy && !_motors.limit.throttle_upper) {
+        vel_xy_i = _pid_vel_xy.get_i();
+    } else {
+        vel_xy_i = _pid_vel_xy.get_i_shrink();
+    }
+
+    // get d
+    vel_xy_d = _pid_vel_xy.get_d();
+
+    // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
+    accel_target.x = (vel_xy_p.x + vel_xy_i.x + vel_xy_d.x) * ekfNavVelGainScaler;
+    accel_target.y = (vel_xy_p.y + vel_xy_i.y + vel_xy_d.y) * ekfNavVelGainScaler;
+
+    // reset accel to current desired acceleration
+    if (_flags.reset_accel_to_lean_xy) {
+        _accel_target_filter.reset(Vector2f(accel_target.x, accel_target.y));
+        _flags.reset_accel_to_lean_xy = false;
+    }
+
+    // filter correction acceleration
+    _accel_target_filter.set_cutoff_frequency(MIN(_accel_xy_filt_hz, 5.0f * ekfNavVelGainScaler));
+    _accel_target_filter.apply(accel_target, dt);
+
+    // pass the correction acceleration to the target acceleration output
+    _accel_target.x = _accel_target_filter.get().x;
+    _accel_target.y = _accel_target_filter.get().y;
+
+    // Add feed forward into the target acceleration output
+    _accel_target.x += _accel_desired.x;
+    _accel_target.y += _accel_desired.y;
+
+    // the following section converts desired accelerations provided in lat/lon frame to roll/pitch angles
+
+    // limit acceleration using maximum lean angles
+    float angle_max = MIN(_attitude_control.get_althold_lean_angle_max(), get_lean_angle_max_cd());
+    float accel_max = MIN(GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f)), POSCONTROL_ACCEL_XY_MAX);
+    _limit.accel_xy = limit_vector_length(_accel_target.x, _accel_target.y, accel_max);
+
+    // update angle targets that will be passed to stabilize controller
+    accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target);
+}
+
+// get_lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
+void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const
+{
+    float accel_right, accel_forward;
+
+    // rotate accelerations into body forward-right frame
+    // todo: this should probably be based on the desired heading not the current heading
+    accel_forward = accel_x_cmss * _ahrs.cos_yaw() + accel_y_cmss * _ahrs.sin_yaw();
+    accel_right = -accel_x_cmss * _ahrs.sin_yaw() + accel_y_cmss * _ahrs.cos_yaw();
+
+    // update angle targets that will be passed to stabilize controller
+    pitch_target = atanf(-accel_forward / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
+    float cos_pitch_target = cosf(pitch_target * M_PI / 18000.0f);
+    roll_target = atanf(accel_right * cos_pitch_target / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
+}
+
+// get_lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
+void AC_PosControl::lean_angles_to_accel(float& accel_x_cmss, float& accel_y_cmss) const
+{
+    // rotate our roll, pitch angles into lat/lon frame
+    // todo: this should probably be based on the desired attitude not the current attitude
+    accel_x_cmss = (GRAVITY_MSS * 100) * (-_ahrs.cos_yaw() * _ahrs.sin_pitch() * _ahrs.cos_roll() - _ahrs.sin_yaw() * _ahrs.sin_roll()) / MAX(_ahrs.cos_roll() * _ahrs.cos_pitch(), 0.5f);
+    accel_y_cmss = (GRAVITY_MSS * 100) * (-_ahrs.sin_yaw() * _ahrs.sin_pitch() * _ahrs.cos_roll() + _ahrs.cos_yaw() * _ahrs.sin_roll()) / MAX(_ahrs.cos_roll() * _ahrs.cos_pitch(), 0.5f);
+}
+
+/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
+float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float kP) const
+{
+    float leash_length;
+
+    // sanity check acceleration and avoid divide by zero
+    if (accel_cms <= 0.0f) {
+        accel_cms = POSCONTROL_ACCELERATION_MIN;
+    }
+
+    // avoid divide by zero
+    if (kP <= 0.0f) {
+        return POSCONTROL_LEASH_LENGTH_MIN;
+    }
+
+    // calculate leash length  1/kP 为时间间隔dt
+    if (speed_cms <= accel_cms / kP) {
+        // linear leash length based on speed close in
+        leash_length = speed_cms / kP; //时间间隔很小的时候 V*dt
+    } else {
+        // leash length grows at sqrt of speed further out //时间间隔比较大的时候  1/2*a*dt^2+ v^2/2/a 这是根据开方器来的 看不懂
+        leash_length = (accel_cms / (2.0f * kP * kP)) + (speed_cms * speed_cms / (2.0f * accel_cms));
+    }
+
+    // ensure leash is at least 1m long
+    if (leash_length < POSCONTROL_LEASH_LENGTH_MIN) {
+        leash_length = POSCONTROL_LEASH_LENGTH_MIN;
+    }
+
+    return leash_length;
+}
+
+/// initialise ekf xy position reset check
+void AC_PosControl::init_ekf_xy_reset()
+{
+    Vector2f pos_shift;
+    _ekf_xy_reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
+}
+
+/// check for ekf position reset and adjust loiter or brake target position
+void AC_PosControl::check_for_ekf_xy_reset()
+{
+    // check for position shift
+    Vector2f pos_shift;
+    uint32_t reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
+    if (reset_ms != _ekf_xy_reset_ms) {
+        shift_pos_xy_target(pos_shift.x * 100.0f, pos_shift.y * 100.0f);
+        _ekf_xy_reset_ms = reset_ms;
+    }
+}
+
+/// initialise ekf z axis reset check
+void AC_PosControl::init_ekf_z_reset()
+{
+    float alt_shift;
+    _ekf_z_reset_ms = _ahrs.getLastPosDownReset(alt_shift);
+}
+
+/// check for ekf position reset and adjust loiter or brake target position
+void AC_PosControl::check_for_ekf_z_reset()
+{
+    // check for position shift
+    float alt_shift;
+    uint32_t reset_ms = _ahrs.getLastPosDownReset(alt_shift);
+    if (reset_ms != 0 && reset_ms != _ekf_z_reset_ms) {
+        shift_alt_target(-alt_shift * 100.0f);
+        _ekf_z_reset_ms = reset_ms;
+    }
+}
+
+/// limit vector to a given length, returns true if vector was limited
+bool AC_PosControl::limit_vector_length(float& vector_x, float& vector_y, float max_length)
+{
+    float vector_length = norm(vector_x, vector_y);
+    if ((vector_length > max_length) && is_positive(vector_length)) {
+        vector_x *= (max_length / vector_length);
+        vector_y *= (max_length / vector_length);
+        return true;
+    }
+    return false;
+}
+
+/// Proportional controller with piecewise sqrt sections to constrain second derivative
+Vector3f AC_PosControl::sqrt_controller(const Vector3f& error, float p, float second_ord_lim)
+{
+    if (second_ord_lim < 0.0f || is_zero(second_ord_lim) || is_zero(p)) {
+        return Vector3f(error.x * p, error.y * p, error.z);
+    }
+
+    float linear_dist = second_ord_lim / sq(p);
+    float error_length = norm(error.x, error.y);
+    if (error_length > linear_dist) {
+        float first_order_scale = safe_sqrt(2.0f * second_ord_lim * (error_length - (linear_dist * 0.5f))) / error_length;
+        return Vector3f(error.x * first_order_scale, error.y * first_order_scale, error.z);
+    } else {
+        return Vector3f(error.x * p, error.y * p, error.z);
+    }
+}
+
+bool AC_PosControl::pre_arm_checks(const char *param_prefix,
+                                   char *failure_msg,
+                                   const uint8_t failure_msg_len)
+{
+    // validate AC_P members:
+    const struct {
+        const char *pid_name;
+        AC_P &p;
+    } ps[] = {
+        { "POSXY", get_pos_xy_p() },
+        { "POSZ", get_pos_z_p() },
+        { "VELZ", get_vel_z_p() },
+    };
+    for (uint8_t i=0; i<ARRAY_SIZE(ps); i++) {
+        // all AC_P's must have a positive P value:
+        if (!is_positive(ps[i].p.kP())) {
+            hal.util->snprintf(failure_msg, failure_msg_len, "%s_%s_P must be > 0", param_prefix, ps[i].pid_name);
+            return false;
+        }
+    }
+
+    // z-axis acceleration control PID doesn't use FF, so P and I must be positive
+    if (!is_positive(get_accel_z_pid().kP())) {
+        hal.util->snprintf(failure_msg, failure_msg_len, "%s_ACCZ_P must be > 0", param_prefix);
+        return false;
+    }
+    if (!is_positive(get_accel_z_pid().kI())) {
+        hal.util->snprintf(failure_msg, failure_msg_len, "%s_ACCZ_I must be > 0", param_prefix);
+        return false;
+    }
+
+    return true;
+}

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -206,7 +206,10 @@ AC_PosControl::AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& ina
     _leash(POSCONTROL_LEASH_LENGTH_MIN),
     _leash_down_z(POSCONTROL_LEASH_LENGTH_MIN),
     _leash_up_z(POSCONTROL_LEASH_LENGTH_MIN),
-    _accel_target_filter(POSCONTROL_ACCEL_FILTER_HZ)
+    _accel_target_filter(POSCONTROL_ACCEL_FILTER_HZ),
+    alt_rate(10.0),
+    _vel_target_filter(10.0),
+    current_alt(10.0)
 {
     AP_Param::setup_object_defaults(this, var_info);
 
@@ -223,6 +226,7 @@ AC_PosControl::AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& ina
     _limit.vel_up = true;
     _limit.vel_down = true;
     _limit.accel_xy = true;
+	
 }
 
 ///
@@ -402,8 +406,10 @@ void AC_PosControl::set_target_to_stopping_point_z()
 /// get_stopping_point_z - calculates stopping point based on current position, velocity, vehicle acceleration
 void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const
 {
-    const float curr_pos_z = _inav.get_altitude();
-    float curr_vel_z = _inav.get_velocity_z();
+    //const float curr_pos_z = _inav.get_altitude();
+    const float curr_pos_z = sub.barometer.get_altitude()*100;//_inav.get_altitude();//12.19
+   // float curr_vel_z = _inav.get_velocity_z();
+   float curr_vel_z =alt_rate.get();
 
     float linear_distance; // half the distance we swap between linear and sqrt and the distance we offset sqrt
     float linear_velocity;  // the velocity we swap between linear and sqrt
@@ -482,7 +488,7 @@ void AC_PosControl::update_z_controller()
     calc_leash_length_z();
 
     // call z-axis position controller
-   run_z_controller_f();
+   run_z_controller();
 }
 float AC_PosControl::update_z_controller_f()
 {
@@ -497,7 +503,7 @@ float AC_PosControl::update_z_controller_f()
     _last_update_z_us = now_us;
 
     // check for ekf altitude reset
-    //check_for_ekf_z_reset();
+   // check_for_ekf_z_reset();
 
     // check if leash lengths need to be recalculated
     calc_leash_length_z();
@@ -527,15 +533,7 @@ void AC_PosControl::run_z_controller()
     //float curr_alt = _inav.get_altitude();//12.19
 	float curr_alt = sub.barometer.get_altitude()*100;//_inav.get_altitude();//12.19
 	
-	float vel_from_baro = AC_AttitudeControl::sqrt_controller(curr_alt-last_alt, _p_pos_z.kP(), _accel_z_cms, _dt);//12.19
-	static uint16_t i=0;
-	uint16_t timecnt = (uint16_t)(0.25/_dt);
-	i++;
-	if (i>timecnt)
-	{
-		i = 0;
-		last_alt = curr_alt;//12.19
-	}
+
 	
     // clear position limit flags
     _limit.pos_up = false;
@@ -584,7 +582,7 @@ void AC_PosControl::run_z_controller()
     
 	Vector3f curr_vel;//12.19
 	
-	curr_vel.z=vel_from_baro;
+	curr_vel.z=alt_rate.get();
 
 
     // TODO: remove velocity derivative calculation
@@ -643,35 +641,60 @@ 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 uint8_t cnt = 0;
-	cnt++;
-	if (cnt>250)
-	{
-		cnt = 0;
+
+
+
+}
+
+void AC_PosControl::calculate_rate(float dt){
+	
+
+	current_alt.apply(sub.barometer.get_altitude()*100,dt);//_inav.get_altitude();//12.19
+
+	alt_rate.apply((current_alt.get()-last_alt)/dt,dt);//12.19 cm
+	last_alt = current_alt.get();//12.19
+	sub.velocity_z_filer = (0.95f*sub.velocity_z_filer) + (0.05f*alt_rate.get());//滤波
+
+}
+void AC_PosControl::calculate_target_rate(float dt){
+	float curr_alt = sub.barometer.get_altitude()*100;//_inav.get_altitude();//12.19
+	    _pos_error.z = _pos_target.z - curr_alt;
 		
-		//gcs().send_text(MAV_SEVERITY_INFO, "  ff %d %d %f\n",(int)_flags.use_desvel_ff_z,(int)_flags.freeze_ff_z,_motors.get_throttle_hover());
-		//gcs().send_text(MAV_SEVERITY_INFO, "  pos %f %f %f %f\n",_pos_error.z,_vel_target.z,curr_vel.z,_accel_desired.z);
-		gcs().send_text(MAV_SEVERITY_INFO, "  pid %f %f %f %f\n",_accel_target.z,z_accel_meas,_accel_desired.z,thr_out);
+		_limit.pos_up = false;
+		_limit.pos_down = false;
 
-	}
+    // do not let target altitude get too far from current altitude
+    if (_pos_error.z > _leash_up_z) {
+        _pos_target.z = curr_alt + _leash_up_z;
+        _pos_error.z = _leash_up_z;
+        _limit.pos_up = true;
+    }
+    if (_pos_error.z < -_leash_down_z) {
+        _pos_target.z = curr_alt - _leash_down_z;
+        _pos_error.z = -_leash_down_z;
+        _limit.pos_down = true;
+    }
+	
+	_vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms, dt);
 
+	 _limit.vel_up = false;
+    _limit.vel_down = false;
+    if (_vel_target.z < _speed_down_cms) {
+        _vel_target.z = _speed_down_cms;
+        _limit.vel_down = true;
+    }
+    if (_vel_target.z > _speed_up_cms) {
+        _vel_target.z = _speed_up_cms;
+        _limit.vel_up = true;
+    }
 
 }
+
 float AC_PosControl::run_z_controller_f()
 {
     //float curr_alt = _inav.get_altitude();//12.19
-	float curr_alt = sub.barometer.get_altitude()*100;//_inav.get_altitude();//12.19
-	
-	float vel_from_baro = AC_AttitudeControl::sqrt_controller(curr_alt-last_alt, _p_pos_z.kP(), _accel_z_cms, _dt);//12.19
-	static uint16_t i=0;
-	uint16_t timecnt = (uint16_t)(0.25/_dt);
-	i++;
-	if (i>timecnt)
-	{
-		i = 0;
-		last_alt = curr_alt;//12.19
-	}
 	
+	float curr_alt = sub.barometer.get_altitude()*100;//_inav.get_altitude();//12.19
     // clear position limit flags
     _limit.pos_up = false;
     _limit.pos_down = false;
@@ -690,9 +713,80 @@ float AC_PosControl::run_z_controller_f()
         _pos_error.z = -_leash_down_z;
         _limit.pos_down = true;
     }
+	float scaler = 0.0;
+	
+	scaler = 6.0;//加速度的KI
+	float kp = 0.0;
+
+	if (fabsf(_pos_error.z)>scaler)
+	{
+			kp = 2.0;//位置的KP
+			_p_pos_z.kP(kp);
+	
+			kp = 10.0;//速度的Kp
+			_p_vel_z.kP(kp);
+			
+			kp = 1.3;//加速度的KP
+			_pid_accel_z.kP(kp);
+			
+			kp = 0.8;//加速度的KI
+			_pid_accel_z.kI(kp);
+			
+	}else{
+			kp = 2.0;//位置的KP
+			_p_pos_z.kP(kp);
+			
+			kp = 7.0;//速度的KP
+			_p_vel_z.kP(kp);
+
+			kp = 1.0;//加速度的KP
+			_pid_accel_z.kP(kp);
+	
+			kp = 0.8;//加速度的KI
+	        _pid_accel_z.kI(kp);
+		}
+	/*scaler = (float)SRV_Channels::srv_channel(15)->get_trim()/1000;//加速度的KI
+	float kp = 0.0;
 
+	if (fabsf(_pos_error.z)>scaler)
+	{
+			kp = (float)SRV_Channels::srv_channel(14)->get_output_max()/1000;//位置的KP
+			_p_pos_z.kP(kp);
+	
+			kp = (float)SRV_Channels::srv_channel(14)->get_output_min()/1000;//速度的Kp
+			_p_vel_z.kP(kp);
+			
+			kp = (float)SRV_Channels::srv_channel(13)->get_output_min()/1000;//加速度的KP
+			_pid_accel_z.kP(kp);
+			
+			kp = (float)SRV_Channels::srv_channel(13)->get_trim()/10000;//加速度的KI
+			_pid_accel_z.kI(kp);
+			
+	}else{
+			kp = (float)SRV_Channels::srv_channel(14)->get_trim()/1000;//位置的KP
+			_p_pos_z.kP(kp);
+			
+			kp = (float)SRV_Channels::srv_channel(13)->get_output_max()/1000;//速度的KP
+			_p_vel_z.kP(kp);
+
+			kp = (float)SRV_Channels::srv_channel(15)->get_output_max()/1000;//加速度的KP
+			_pid_accel_z.kP(kp);
+	
+			kp = (float)SRV_Channels::srv_channel(15)->get_output_min()/1000;//加速度的KI
+	        _pid_accel_z.kI(kp);
+		}*/
+	static uint16_t count2 = 0;
+	count2++;
+	if (count2>300)
+	{
+		count2 = 0;
+		//gcs().send_text(MAV_SEVERITY_INFO, "PID %f %f%f %f\n",(float)_p_pos_z.kP(), (float)_p_vel_z.kP(),(float)_pid_accel_z.kP(),(float)_pid_accel_z.kI());
+		
+	}
     // calculate _vel_target.z using from _pos_error.z using sqrt controller
-    _vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms, _dt);
+    
+	_vel_target_filter.apply(AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms, _dt),_dt);
+    _vel_target.z = _vel_target_filter.get();
 
     // check speed limits
     // To-Do: check these speed limits here or in the pos->rate controller
@@ -709,9 +803,10 @@ float AC_PosControl::run_z_controller_f()
 
     // add feed forward component
     if (_flags.use_desvel_ff_z) {
-        _vel_target.z += _vel_desired.z;
+	   
+		 _vel_target.z += _vel_desired.z;
+		
     }
-
 	
     // the following section calculates acceleration required to achieve the velocity target
 
@@ -719,7 +814,7 @@ float AC_PosControl::run_z_controller_f()
     
 	Vector3f curr_vel;//12.19
 	
-	curr_vel.z=vel_from_baro;
+	curr_vel.z=sub.velocity_z_filer;//alt_rate.get();
 
 
     // TODO: remove velocity derivative calculation
@@ -743,27 +838,24 @@ float AC_PosControl::run_z_controller_f()
 
     // store this iteration's velocities for the next iteration
     _vel_last.z = _vel_target.z;
-	static uint8_t freeze_ff_z =1;
+	
     // reset velocity error and filter if this controller has just been engaged
     if (_flags.reset_rate_to_accel_z) {
         // Reset Filter
         _vel_error.z = 0;
         _vel_error_filter.reset(0);
-		_accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;//02.28
-		//freeze_ff_z = 0;//前馈一直有效
+		//_accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;//02.28
+		
         _flags.reset_rate_to_accel_z = false;
     } else {
         // calculate rate error and filter with cut off frequency of 2 Hz
         _vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z, _dt);
-		 _accel_target.z = _p_vel_z.get_p(_vel_error.z);//02.28
-		 freeze_ff_z =1;
+		// _accel_target.z = _p_vel_z.get_p(_vel_error.z);//02.28
+		
     }
-
-   
-
+	_accel_target.z = _p_vel_z.get_p(_vel_error.z);//02.28
     _accel_target.z += _accel_desired.z;
 
-
     // the following section calculates a desired throttle needed to achieve the acceleration target
     float z_accel_meas;         // actual acceleration
 
@@ -777,29 +869,14 @@ float AC_PosControl::run_z_controller_f()
 
    // float thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f +_motors.get_throttle_hover();
 	float thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f;
-	float ff_p =(float)SRV_Channels::srv_channel(15)->get_output_min()/1000000;//参数 
+	
 
-	float	thr_out_ff = thr_out;
-	if (freeze_ff_z!=0)
-	{
-		
-		thr_out_ff = thr_out - curr_vel.z*ff_p;//增加了一个前馈项
-	}
+
+	//如果加一个当前速度前馈的话最多可以用0.002
     // send throttle to attitude controller with angle boost
 //    _attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);
-	static uint8_t cnt = 0;
-	cnt++;
-	if (cnt>250)
-	{
-		cnt = 0;
-		
-		//gcs().send_text(MAV_SEVERITY_INFO, "  ff %d %d %f\n",(int)_flags.use_desvel_ff_z,(int)_flags.freeze_ff_z,_motors.get_throttle_hover());
-		//gcs().send_text(MAV_SEVERITY_INFO, "  pos %f %f %f %f\n",_pos_error.z,_vel_target.z,curr_vel.z,_accel_desired.z);
-		gcs().send_text(MAV_SEVERITY_INFO, "pid %d %f %f %f %f\n",freeze_ff_z,_vel_target.z,thr_out,ff_p,thr_out_ff);
-
-	}
 
-	return thr_out_ff;
+	return thr_out;
 }
 
 ///

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -53,6 +53,7 @@ public:
     ///
     /// initialisation functions
     ///
+	float run_z_controller_wangdan(float _vel);
 
     /// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
     ///     updates z axis accel controller's D term filter
@@ -307,6 +308,12 @@ public:
 	float       _accel_z_cms;           // max vertical acceleration in cm/s/s
 	float  last_alt;
 	AC_PID		_pid_accel_z;
+	 AC_P        _p_vel_z;
+	LowPassFilterFloat alt_rate;
+	LowPassFilterFloat _vel_target_filter;//截止频率10.0hz
+	LowPassFilterFloat current_alt;
+	void calculate_rate(float dt);
+	void calculate_target_rate(float dt);
 
 protected:
 
@@ -342,6 +349,7 @@ protected:
     //          init_takeoff
     void run_z_controller();
 	float run_z_controller_f();
+	
 
     ///
     /// xy controller private methods
@@ -385,7 +393,7 @@ protected:
     AP_Float    _accel_xy_filt_hz;      // XY acceleration filter cutoff frequency
     AP_Float    _lean_angle_max;        // Maximum autopilot commanded angle (in degrees). Set to zero for Angle Max
    
-    AC_P        _p_vel_z;
+   
     
     AC_P        _p_pos_xy;
     AC_PID_2D   _pid_vel_xy;

libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp

@@ -46,6 +46,8 @@ void AC_PosControl_Sub::set_alt_target_from_climb_rate(float climb_rate_cms, flo
 void AC_PosControl_Sub::set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)
 {
     // calculated increased maximum acceleration if over speed
+
+
     float accel_z_cms = _accel_z_cms;
     if (_vel_desired.z < _speed_down_cms && !is_zero(_speed_down_cms)) {
         accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_down_cms;
@@ -56,14 +58,15 @@ void AC_PosControl_Sub::set_alt_target_from_climb_rate_ff(float climb_rate_cms,
     accel_z_cms = constrain_float(accel_z_cms, 0.0f, 750.0f);
 
     // jerk_z is calculated to reach full acceleration in 1000ms.
-    float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO;
+    float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO;//秒钟
 
     float accel_z_max = MIN(accel_z_cms, safe_sqrt(2.0f*fabsf(_vel_desired.z - climb_rate_cms)*jerk_z));
 
-    _accel_last_z_cms += jerk_z * dt;
-    _accel_last_z_cms = MIN(accel_z_max, _accel_last_z_cms);
+    //_accel_last_z_cms += jerk_z * dt;//03.03
+    //_accel_last_z_cms = MIN(accel_z_max, _accel_last_z_cms);//03.03
 
-    float vel_change_limit = _accel_last_z_cms * dt;
+
+    float vel_change_limit =accel_z_max*dt;// _accel_last_z_cms * dt;//03.03
     _vel_desired.z = constrain_float(climb_rate_cms, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);
     _flags.use_desvel_ff_z = true;
 
@@ -82,7 +85,7 @@ void AC_PosControl_Sub::set_alt_target_from_climb_rate_ff(float climb_rate_cms,
     }
 
     // do not let target alt get below limit
-    if (_alt_min < 0 && _alt_min < _alt_max && _pos_target.z < _alt_min) {
+    if (_alt_min < -100 && _alt_min < _alt_max && _pos_target.z < _alt_min) {
         _pos_target.z = _alt_min;
         _limit.pos_down = true;
         // decelerate feed forward to zero
@@ -98,14 +101,30 @@ void AC_PosControl_Sub::relax_alt_hold_controllers()
     //_vel_last.z = _inav.get_velocity_z();// 12.19
 	 _vel_desired.z = 0.0f;
     _flags.use_desvel_ff_z = false;
-    _vel_target.z = AC_AttitudeControl::sqrt_controller(sub.barometer.get_altitude()*100-_pos_target.z, _p_pos_z.kP(), _accel_z_cms, _dt);//12.19
-    _pos_target.z = sub.barometer.get_altitude()*100;// 12.19
+	
+    _vel_target.z =alt_rate.get();
+	_pos_target.z = sub.barometer.get_altitude()*100;// 12.19
     _vel_last.z = _vel_target.z;
     _accel_desired.z = 0.0f;
     _accel_last_z_cms = 0.0f;
     _flags.reset_rate_to_accel_z = true;
     _accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
     _pid_accel_z.reset_filter();
-	last_alt = sub.barometer.get_altitude()*100;// 12.19
-	_pid_accel_z.reset_I();//02.27
+	//last_alt = sub.barometer.get_altitude()*100;// 12.19
+	//_pid_accel_z.reset_I();//02.27
+}
+void AC_PosControl_Sub::reset_I(){
+	_pid_accel_z.reset_I();
+
 }
+/*
+void AC_PosControl_Sub::relax_part_hold_controllers(){
+	 _vel_desired.z = 0.0f;
+    _flags.use_desvel_ff_z = false;
+    _accel_last_z_cms = 0.0f;
+    _flags.reset_rate_to_accel_z = true;
+    _pos_target.z = sub.barometer.get_altitude()*100;// 12.19
+
+
+}*/
+

libraries/AC_AttitudeControl/AC_PosControl_Sub.h

@@ -39,6 +39,8 @@ public:
     /// relax_alt_hold_controllers - set all desired and targets to measured
     ///     integrator is not reset
     void relax_alt_hold_controllers();
+	//void relax_part_hold_controllers();
+	void reset_I();
 
 	float get_alt(){return _inav.get_altitude();};
 

libraries/AP_Motors/AP_Motors6DOF.cpp

@@ -232,7 +232,7 @@ void AP_Motors6DOF::output_min()
 
 extern mavlink_motor_speed_t mav_motor_speed;
 
-#define NETRULPWM 1490
+
 void AP_Motors6DOF::output_to_PWM()
 {
     int8_t i;
@@ -269,8 +269,8 @@ void AP_Motors6DOF::output_to_PWM()
     }
 	if(mav_motor_speed.motorTest == 1)
 	{//测试模式上位机直接控制 仅仅控制6个推进器
-		 motor_out[0] =(int16_t)(mav_motor_speed.motor1*500+NETRULPWM);//230 才启动
-		 motor_out[1] =(int16_t)(mav_motor_speed.motor2*500+NETRULPWM);
+		 motor_out[0] =(int16_t)(mav_motor_speed.motor1*500/2200+NETRULPWM);//230 才启动
+		 motor_out[1] =(int16_t)(mav_motor_speed.motor2*500/2200+NETRULPWM);
 		 motor_out[2] = (int16_t)(mav_motor_speed.motor3*500/2200+NETRULPWM);
 		 motor_out[3] = (int16_t)(mav_motor_speed.motor4*500/2200+NETRULPWM);//大于100能动
 		 motor_out[4] = (int16_t)(mav_motor_speed.motor5*500/2200+NETRULPWM);
@@ -287,9 +287,13 @@ void AP_Motors6DOF::output_to_PWM()
     // send output to each motor
     for (i=0; i<6; i++) {
         if (motor_enabled[i]) {
+			motor_to_detect[i] = motor_out[i];
             rc_write(i, motor_out[i]);
-        }
+        }else{
+			motor_to_detect[i] = NETRULPWM;
+        	}
     }
+
 	output_motor8_and_motor9();
 }
 
@@ -497,6 +501,7 @@ void AP_Motors6DOF::output_armed_stabilizing()
                 rpy_out[i] = roll_thrust * _roll_factor[i] +
                              pitch_thrust * _pitch_factor[i] +
                              yaw_thrust * _yaw_factor[i];
+				// rpy_out[i]  = constrain_float(rpy_out[i] ,-0.8f,0.8f);//wangdan 
 
             }
         }

libraries/AP_Motors/AP_Motors6DOF.h

@@ -7,6 +7,7 @@
 #include <AP_Math/AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
 #include <RC_Channel/RC_Channel.h>     // RC Channel Library
 #include "AP_MotorsMatrix.h"
+#define NETRULPWM 1490
 
 /// @class      AP_MotorsMatrix
 class AP_Motors6DOF : public AP_MotorsMatrix {
@@ -19,6 +20,7 @@ public:
 		for (uint8_t i= 0; i < 6; i++)
 		{
 			motor_to_can[i]=0;
+			motor_to_detect[i] = NETRULPWM;
 		}
 		pwm_track[0] = 1500;
 		pwm_track[1] = 1500;
@@ -42,6 +44,7 @@ public:
 				return _singleton;
 			}
 	int32_t motor_to_can[6];//发送给CAN的数据
+	int32_t motor_to_detect[6];//发送给CAN的数据
 	int16_t pwm_track[2];
 	float last_thrust_in[AP_MOTORS_MAX_NUM_MOTORS];
 	int32_t last_thrust_Dhot[AP_MOTORS_MAX_NUM_MOTORS];

libraries/AP_Motors/AP_Motors_Class.h

@@ -81,11 +81,11 @@ public:
     bool                get_interlock() const { return _flags.interlock; }
 
     // set_roll, set_pitch, set_yaw, set_throttle
-    void                set_roll(float roll_in) { _roll_in = roll_in; };        // range -1 ~ +1
+    void                set_roll(float roll_in) { _roll_in = constrain_float(roll_in,-0.8,0.8); };        // range -1 ~ +1
     void                set_roll_ff(float roll_in) { _roll_in_ff = roll_in; };    // range -1 ~ +1
-    void                set_pitch(float pitch_in) { _pitch_in = pitch_in; };    // range -1 ~ +1
+    void                set_pitch(float pitch_in) { _pitch_in = constrain_float(pitch_in,-0.8,0.8); };    // range -1 ~ +1
     void                set_pitch_ff(float pitch_in) { _pitch_in_ff = pitch_in; };  // range -1 ~ +1
-    void                set_yaw(float yaw_in) { _yaw_in = yaw_in; };            // range -1 ~ +1
+    void                set_yaw(float yaw_in) { _yaw_in = constrain_float(yaw_in,-0.8,0.8); };            // range -1 ~ +1
     void                set_yaw_ff(float yaw_in) { _yaw_in_ff = yaw_in; };      // range -1 ~ +1
     void                set_throttle(float throttle_in) { _throttle_in = throttle_in; };   // range 0 ~ 1
     void                set_throttle_avg_max(float throttle_avg_max) { _throttle_avg_max = constrain_float(throttle_avg_max, 0.0f, 1.0f); };   // range 0 ~ 1