ArduSub/ArduSub.cpp

@@ -67,7 +67,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
     SCHED_TASK(userhook_50Hz,         50,     75),
 #endif
 #ifdef USERHOOK_MEDIUMLOOP
-    SCHED_TASK(userhook_MediumLoop,   10,     75),
+    SCHED_TASK(userhook_MediumLoop,   15,     75),
 #endif
 #ifdef USERHOOK_SLOWLOOP
     SCHED_TASK(userhook_SlowLoop,     3.3,    75),
@@ -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();
@@ -193,7 +194,12 @@ void Sub::ten_hz_logging_loop()
     if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
         Log_Write_Attitude();
         logger.Write_Rate(&ahrs_view, motors, attitude_control, pos_control);
-        if (should_log(MASK_LOG_PID)) {
+        if (should_log(MASK_LOG_PID)){ 
+			/*attitude_control.get_rate_roll_pid().get_pid_info().P = channel_forward->norm_input();
+			attitude_control.get_rate_roll_pid().get_pid_info().I = channel_lateral->norm_input();
+		    attitude_control.get_rate_roll_pid().get_pid_info().D = channel_yaw->norm_input();
+			attitude_control.get_rate_roll_pid().get_pid_info().error = channel_throttle->norm_input();
+			attitude_control.get_rate_roll_pid().get_pid_info().target = (float)PressLevel_f*0.1;*/
             logger.Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info());
             logger.Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info());
             logger.Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info());
@@ -340,6 +346,9 @@ 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/GCS_Mavlink.cpp

@@ -424,6 +424,7 @@ static const ap_message STREAM_EXTRA2_msgs[] = {
 	
 	//MSG_VFR_HUD
 	//MSG_ROTATION_MATRIX_ARRAY,
+	MSG_ROV_MOTOR_TEMP,
 	MSG_ROV_STATE_MONITORING,
 	//MSG_SET_SLAVE_PARAMETER,
 	MSG_MOTOR_SPEED,

ArduSub/Sub.cpp

@@ -46,6 +46,16 @@ Sub::Sub()
     // init sensor error logging flags
     sensor_health.baro = true;
     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

@@ -93,6 +93,13 @@
 #ifndef USERHOOK_50HZLOOP
 #define USERHOOK_50HZLOOP 1
 #endif
+
+#ifndef USERHOOK_MEDIUMLOOP 
+#define USERHOOK_MEDIUMLOOP 1
+	
+#endif
+
+
 #define startval 0
 #define Speedmax_hand 62
 #define Speedmax_hand_f 62.0
@@ -114,6 +121,7 @@
 #define backward  2
 #define Speedmax 60
 
+#define STARTCONT 2000
 
 //压力等级 包含下压和上浮总共10级 第五级为1500 即中值
 enum PressNetLevel {
@@ -580,6 +588,8 @@ private:
 
     // Handles attitude control for stabilize and althold modes
     void handle_attitude();
+	void handle_attitude_stablize();
+	void handle_attitude_sport();
     bool auto_init(void);
     void auto_run();
     void auto_wp_start(const Vector3f& destination);
@@ -846,7 +856,23 @@ public:
 	PressNetLevel PressLevel;//压力分级枚举类型
 	float PressLevel_f;//压力分级float类型
 
+	float updowmgain;
+	float delaygain;
+
 	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;
@@ -862,6 +888,7 @@ public:
     } _telemetry[OUSHENCAN_MAX_NUM_ESCS];
 	bool sport_init(void);
 	void sport_run();
+	void sport_run_alt(void);
 
 	bool clean_init(void);
 	void clean_run();
@@ -877,6 +904,66 @@ public:
 	void clean_sidenet_run(void);
 	void track_pidcontrol(float _targethead,int16_t &_motor1,int16_t &_motor2);
 	float get_yaw_error(float yaw_heading);
+	float getgainf(float data);
+	float getgainfstatble(float data);
+	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);
+	unsigned char RxDVL[240];//惯导接收缓冲区
+	unsigned char TxDVL[51];//惯导接收缓冲区
+	unsigned char DVL_RxCnt ;//数据接收个数
+	float arryf[20] ;
+	int64_t timearry[3];
+	struct wrz_t {
+			float Vx;//x方向的速度 m/s
+			float Vy;//y方向的速度 m/s
+			float Vz;//z方向的速度 m/s
+			char valid;//当DVL锁定在反射物表面时，y表示高度和速度有效 y/n
+			float altitude;//高度 m
+			float fom;//测量经度 m/s
+			float covariancearr[9];//协方差矩阵
+			int64_t timeofvalidity;//相当于ping的时间 微秒
+			int64_t timeoftransmission;//微秒 TCP
+			float time; //两次报告中间的时间ms
+			int status;//0表示正常，1表示有问题比如温度过高
+			
+		} DVLdata_wrz;
+	struct wru_t {
+			int id;//换能器的号
+			float velocity;//换能器方向上的速度，如果没有接收到反馈信号 0
+			float distance;	//换能器和反射物体的平行距离 如果没有接收到反馈信号 -1
+			int rssi;//信号强度 DBM
+			int nsd;//噪音强度dbm
+		} DVLdata_wru;
+	struct wrp_t {
+			int64_t time_stamp;//报告的时间戳 unix时间 s
+			int16_t time_Mulfactor;
+			float Dx;//x方向距离 m
+			float Dy;//y方向距离 m
+			float Dz;//z方向距离 m
+			float pos_std;//标准差 m
+			float roll;//度
+			float pitch;
+			float yaw;
+			int status;//0没有error 1表示其他
+		} DVLdata_wrp;
+
+	void read_external_DVL(AP_HAL::UARTDriver *uart);
+	void write_external_DVL(AP_HAL::UARTDriver *uart);
+	int chartodec(unsigned char _ch);
+	void transfer_position_delta_msg(void);
+
+	void charArrtobyteArr(unsigned char *charArr ,uint8_t * dec,int16_t len);
+	void charArrtodecArr(unsigned char *charArr ,uint8_t * dec,int16_t len);
 };
 
 extern const AP_HAL::HAL& hal;

ArduSub/UserCan.cpp

@@ -157,14 +157,27 @@ void UserCAN::loop()
 			   if (send_ID<7)
 				{
 
+
+				static int k = 0;
+				k++;
+				if(k>400)
+				{
+					gcs().send_text(MAV_SEVERITY_INFO, "motor_out1 %d %d %d \n", (int)motors6dof.motor_to_can[0],(int)motors6dof.motor_to_can[1],(int)motors6dof.motor_to_can[2]);
+					gcs().send_text(MAV_SEVERITY_INFO, "motor_out2 %d %d %d \n", (int)motors6dof.motor_to_can[3],(int)motors6dof.motor_to_can[4],(int)motors6dof.motor_to_can[5]);
+					//gcs().send_text(MAV_SEVERITY_INFO, "motor_out %f %f %d \f", (float)motor_to_can[3],(float)motor_to_can[4], (float)motor_to_can[5]);
+					k=0;
+				}
+
 					mot_rot_cmd1.data[0] = (uint8_t)(motors6dof.motor_to_can[send_ID-1]>>24);
 					mot_rot_cmd1.data[1] = (uint8_t)(motors6dof.motor_to_can[send_ID-1]>>16);
 					mot_rot_cmd1.data[2] = (uint8_t)(motors6dof.motor_to_can[send_ID-1]>>8);
 					mot_rot_cmd1.data[3] = (uint8_t)motors6dof.motor_to_can[send_ID-1];
 					
 			
-			    	mot_rot_frame1 = {((CAN_PACKET_SET_DUTY<<8 |send_ID) | 0x80000000U), mot_rot_cmd1.data, sizeof(mot_rot_cmd1.data)};
-			   		timeout = uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + timeout_us);
+			    	//mot_rot_frame1 = {((CAN_PACKET_SET_DUTY<<8 |send_ID) | 0x80000000U), mot_rot_cmd1.data, sizeof(mot_rot_cmd1.data)};
+
+					mot_rot_frame1 = {((CAN_PACKET_SET_RPM<<8 |send_ID) | 0x80000000U), mot_rot_cmd1.data, sizeof(mot_rot_cmd1.data)};
+					timeout = uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + timeout_us);
 					if (!write_frame(mot_rot_frame1, timeout)) {
 	                	continue;
 	            	}

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;
@@ -62,7 +63,8 @@ void Sub::userhook_50Hz()
 #ifdef USERHOOK_MEDIUMLOOP
 void Sub::userhook_MediumLoop()
 {
-    // put your 10Hz code here
+   //read_external_DVL(hal.uartD);//uart4
+   //write_external_DVL(hal.uartD);		
 }
 #endif
 
@@ -83,21 +85,21 @@ void Sub::userhook_SuperSlowLoop()
 void Sub::USBL_PowerSwitch(void)
 {
 	uint32_t tnow = AP_HAL::millis();
+	RC_Channel* chan = RC_Channels::rc_channel(8);
+	uint16_t min = chan->get_radio_min();
+	uint16_t max = chan->get_radio_max();
 
-		if (rov_control.floodlight == 1  && motors.armed())
-	   {
-
-			
-			RC_Channels::set_override(8, 20000, tnow);       // lights 1
-  
-
-	   }
-		else {
-			
-			RC_Channels::set_override(8, 0, tnow);       // lights 1
-	   		
-
-	   }
+	   	if(!motors.armed()){	
+	   		lights1 = constrain_float(min, min, max);
+	   	}
+	   if (lights1>1000)
+		  {
+		   lights = 1;
+		  	}else{
+	   		lights =0;
+		}
+	   
+		RC_Channels::set_override(8, lights1, tnow);
 		if(motors.armed() && (rov_control.USBL_flag==1 || usblpoweroff == 1))
 		{
 			
@@ -132,10 +134,608 @@ void Sub::getgain(void){
 
 void Sub::getyaw(void){
 	if (!motors.armed()){
-		yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
+		//yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
 		 return;
 	}else{
 	
 	}
 }
 
+float Sub::getgainf(float data){
+	float temp =0;
+
+	if (data>=-0.6 && data <-0.08)
+	{
+		temp = -0.2;
+	}else if(data>=-0.8 && data<-0.6 )
+	{
+		temp = -0.5;
+	}else if(data>=-1.0 && data<-0.8 )
+	{
+		temp = -0.8;
+	}
+	else if (data<=0.6 && data >0.08)
+	{
+		temp = 0.2;
+	}else if(data<=0.8 && data>0.6 )
+	{
+		temp = 0.5;
+	}else if(data<=1.0 && data>0.8 )
+	{
+		temp = 0.8;
+	}else{
+		temp = 0.0;
+	}
+	return temp;
+}
+float Sub::getgainfstatble(float data){
+	float temp =0;
+
+	if (data>=-0.5 && data <-0.08)
+	{
+		temp = -0.2;
+	}else if(data>=-0.8 && data<-0.5 )
+	{
+		temp = -0.5;
+	}else if(data>=-1.0 && data<-0.8 )
+	{
+		temp = -1.0;
+	}
+	else if (data<=0.5 && data >0.08)
+	{
+		temp = 0.2;
+	}else if(data<=0.8 && data>0.5 )
+	{
+		temp = 0.5;
+	}else if(data<=1.0 && data>0.8 )
+	{
+		temp = 1.0;
+	}else{
+		temp = 0.0;
+	}
+	return temp;
+}
+
+float Sub::sign_in(float f){
+	return (f<0)? -1 :1;
+}
+float Sub::gaindelay(float gain1){
+
+	if(throttle_continuous(gain1)){
+		
+		bool vel_stationary = velocity_z_filer > -5.0 && velocity_z_filer < 5.0;//cm
+		float	result = (updowmgain-0.5)*2;
+
+		if (vel_stationary){
+			last_continuous = 1;
+			 continuous_count++;
+			 if (continuous_count>=MAIN_LOOP_RATE && continuous_count<=4*MAIN_LOOP_RATE){
+				result = result + sign_in(result)*0.2*(continuous_count/MAIN_LOOP_RATE);//步长是0.2
+				result = constrain_float(result,-1.0,1.0);
+			}else if(continuous_count>4*MAIN_LOOP_RATE){
+				result = result +sign_in(result)*0.2*(continuous_count/MAIN_LOOP_RATE);//步长是0.2
+				result = constrain_float(result,-1.0,1.0);
+				continuous_count = 4*MAIN_LOOP_RATE+1;
+			}
+		}else{
+			if (last_continuous == 1)
+			{
+				if (continuous_countdec<MAIN_LOOP_RATE){
+					continuous_countdec++;
+					result = result + sign_in(result)*0.2*(continuous_count/MAIN_LOOP_RATE);//步长是0.2
+					result = constrain_float(result,-1.0,1.0);
+				}else{
+					result = result + sign_in(result)*0.2*(continuous_count/MAIN_LOOP_RATE);//步长是0.2
+					result = constrain_float(result,-1.0,1.0);
+				}
+			}else{
+					result = (updowmgain-0.5)*2;
+					continuous_count = 0;
+					last_continuous = 0;
+					continuous_countdec = 0;
+			}
+		
+		}
+		 static uint16_t count = 0;
+	 count++;
+	 if (count>400)
+		 {
+		 count = 0;
+		 gcs().send_text(MAV_SEVERITY_INFO, "conti %d %d %d %f\n",(int)continuous_count,(int)last_continuous,(int)continuous_countdec,result);
+
+	 }
+
+		return constrain_float(result/2.0+0.5,0.0,1.0);
+	}else{
+
+		 	
+			continuous_count = 0;
+			last_continuous = 0;
+			continuous_countdec = 0;
+		return gain1;//有压力分级的时候返回压力分级 没有压力分级的时候返回按键
+	}
+    
+}
+bool Sub::throttle_continuous(float gain1){
+	static float last_gain = updowmgain;
+	if (fabsf(gain1 -0.5)<0.05)//没有压力分级
+	{
+		delaygain++;
+		if (delaygain>400)//1秒
+		{
+			delaygain=401;
+			updowmgain = 0.5;
+			last_gain = updowmgain;
+			return FALSE;//1秒内没有持续按键
+		}else{
+			
+			if ((last_gain>0.5 && updowmgain<0.5) || (last_gain<0.5 && updowmgain>0.5))//防止突然反向
+			{
+				last_gain = updowmgain;
+				return FALSE;
+			}
+			last_gain = updowmgain;
+			return TRUE;//持续按键
+		}
+		
+    }else{
+    	delaygain=401;
+    //有压力分级
+    	updowmgain = 0.5;
+		last_gain = updowmgain;
+		continuous_count = 0;
+		last_continuous = 0;
+		continuous_countdec = 0;
+    	return FALSE;//1秒内没有持续按键
+	}
+}
+
+/*float Sub::gaindelay(float gain1){
+	 float result=0.5;
+	 static uint16_t count=0;
+	 static uint16_t countdec=0;
+	 static uint8_t last = 0;
+	if (fabsf(gain1 -0.5)<0.05)
+	{
+		delaygain++;
+		if (delaygain>300)//1秒
+		{
+			delaygain=0;
+			
+			result = 0.5;
+			updowmgain = 0.5;
+			count = 0;
+		}else{
+			float velocity_z = pos_control.alt_rate.get()/100;// m/s
+		    bool vel_stationary = velocity_z > -0.05 && velocity_z < 0.05;
+			if (vel_stationary && updowmgain>0.5)//上升
+			{
+				last = 1;
+				countdec = 0;
+				 count++;
+				 if (count<MAIN_LOOP_RATE)
+				{
+					 result = updowmgain;
+				}
+				 else if (count>=MAIN_LOOP_RATE && count<=3*MAIN_LOOP_RATE)
+				{
+					 result = updowmgain+0.1*(count/MAIN_LOOP_RATE);
+					 result = constrain_float(result,-1.0,1.0);
+				}else if(count>3*MAIN_LOOP_RATE){
+					result = 1.0;
+					count = 3*MAIN_LOOP_RATE+1;
+				}
+				
+			}else{
+				if (last == 1)
+				{
+					if (countdec<MAIN_LOOP_RATE){
+						countdec++;
+						result = updowmgain+0.1*(count/MAIN_LOOP_RATE);
+					    result = constrain_float(result,-1.0,1.0);
+					}else{
+						last = 0;
+						result = updowmgain;
+						count = 0;
+						countdec = 0;
+					}
+				}else{
+					result = updowmgain;
+					count = 0;
+					last = 0;
+					countdec = 0;
+				}
+			
+			}
+		}
+		return result;
+    }else{
+		count = 0;
+		updowmgain = 0.5;
+		last = 0;
+		countdec = 0;
+		return gain1;//有压力分级的时候返回压力分级 没有压力分级的时候返回按键
+    } 
+}*/
+//得到手柄的轴
+void Sub::get_heading_to_ned(Matrix3f &mat_half,Matrix3f mat_body){
+
+	// Matrix3f mat_body = ahrs.get_rotation_body_to_ned();
+	float beta = 0.0;  
+	float dot=0.0;
+	
+	if(mat_body.c.x<0.98 && mat_body.c.x>-0.98){//大约78.5度认为Z轴指向方向，小于78.5进入X轴指向方向
+		Vector3f Xbx = mat_body*Vector3f(1,0,0);
+		Xbx.z = 0.0;
+		Vector3f Xex = Vector3f(1,0,0);
+	
+	   dot = Xbx * Xex/Xbx.length();
+		beta = acosf(dot );
+		if (Xbx.y<0)
+	   {
+		   beta = -beta;
+	   }
+		Vector3f Xbz = mat_body*Vector3f(0,0,1);
+		Vector3f Xez = Vector3f(0,0,1);
+	   float dotz = Xbz * Xez/Xbz.length();
+		if (dotz<0)
+	   {
+			beta = beta-PI;
+			if (beta<-PI)
+		   {
+				beta = beta+2*PI;
+		   }
+	   }
+	
+		
+		mat_half.a.x= cosf(beta);
+		mat_half.a.y=-sinf(beta);
+		mat_half.a.z=0.0;  
+		mat_half.b.x= sinf(beta);
+		mat_half.b.y= cosf(beta);
+		mat_half.b.z=0.0;
+		mat_half.c.x= 0.0;
+		mat_half.c.y= 0.0;
+		mat_half.c.z=1.0;
+	
+	
+	   }
+	else{
+	   
+	   Vector3f Xbz = mat_body*Vector3f(0,0,1);
+	   Xbz.z = 0.0;
+	   Vector3f Xex = Vector3f(1,0,0);
+	   dot = Xbz * Xex/Xbz.length();
+	   beta =  acosf(dot );
+	   if (Xbz.y<0)
+	   {
+		   beta = -beta;
+	   }
+		if (mat_body.c.x>0)//头朝下
+	   {
+			beta = beta-PI;
+			if (beta<-PI)
+		   {
+				beta = beta+2*PI;
+		   }
+	   }
+		mat_half.a.x= cosf(beta);
+		mat_half.a.y=-sinf(beta);
+		mat_half.a.z=0.0;  
+		mat_half.b.x= sinf(beta);
+		mat_half.b.y= cosf(beta);
+		mat_half.b.z=0.0;
+		mat_half.c.x= 0.0;
+		mat_half.c.y= 0.0;
+		mat_half.c.z=1.0;
+	
+	
+	}
+
+		static uint16_t countx = 0;
+	   countx++;
+	   if (countx>400)
+		  {
+		   countx = 0;
+		   
+		  	//   gcs().send_text(MAV_SEVERITY_INFO, "ang %d %d  \n",(int)(beta*180/PI),(int)ahrs.yaw_sensor/100);
+
+	   }
+
+}
+float Sub::get_pitch_to_ned(Matrix3f mat_body){
+
+	float pitch_ang = acosf(mat_body.c.x);
+	if (mat_body.c.z<0)
+	{
+		pitch_ang = -pitch_ang;
+	}
+	pitch_ang =pitch_ang-PI/2;
+	if (pitch_ang<-PI)
+	{
+		pitch_ang =pitch_ang+2*PI;
+	}
+	return pitch_ang;
+}
+void Sub::charArrtobyteArr(unsigned char *charArr ,uint8_t * dec,int16_t len){
+	int16_t i = 0;
+	for(i = 0 ;i<len;i++){
+		dec[i] = (uint8_t)charArr[i] ;
+	}	
+}
+
+void Sub::charArrtodecArr(unsigned char *charArr ,uint8_t * dec,int16_t len){
+	
+	int16_t i = 0;
+	uint8_t j=0;
+	uint8_t k=0;
+	char charArrtemp[20]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+	
+
+	static uint8_t cnt = 0;
+	cnt = 0;
+	int typeflag = 0;
+	for(i = 4 ;i<len;i++){
+		
+		if (charArr[i]!=',' && charArr[i]!=';'&& charArr[i]!='*')//&&charArr[i]!='y'&&charArr[i]!='n')//分割数据
+		{
+			charArrtemp[cnt++] = charArr[i];
+			if (charArr[i] == '.')//不是整形
+			{
+				typeflag = 1;
+			}
+			
+		}else{
+			charArrtemp[cnt] = '\0';
+			if (charArr[i-1]=='y' ||charArr[i-1]=='n')
+			{
+				arryf[j++] = charArr[i-1];
+			}
+			else if(typeflag == 0 && cnt>11)//长长整型 只有时间戳才会有这样的长整形
+			{
+				int64_t l = atoll (charArrtemp);
+				timearry[k++] = l;
+			}else if(cnt>11){//双精度 转换的话会丢掉精度  因此变成整形不会丢失精度 
+				int il, jl,kl=0;
+				for (il = 0, jl = 0; charArrtemp[il] != '\0'; il++) {
+					if (charArrtemp[il] != '.') {
+						charArrtemp[jl++] = charArrtemp[il];
+					}else{
+					kl = il;//找出.的位置
+					}
+				}
+				charArrtemp[jl] = '\0';
+				DVLdata_wrp.time_Mulfactor = jl-kl;
+				int64_t l = atoll (charArrtemp);
+				timearry[2] = l;
+				//gcs().send_text(MAV_SEVERITY_INFO, "kl %02x %lld",DVLdata_wrp.time_Mulfactor,timearry[2]);
+			}
+			else{
+				float f = atof(charArrtemp);
+				arryf[j++] = f;
+			}
+			
+			memset(charArrtemp, 0, sizeof(unsigned char)*20);
+			cnt = 0;
+			typeflag = 0;
+
+		}
+	}
+
+	
+}
+void Sub:: write_external_DVL(AP_HAL::UARTDriver *uart){
+	if (uart == nullptr) {
+			return;
+		}
+	//unsigned char send_sucess = 0;
+	/*if (DVLSet.type == 'D')
+	{
+		//send_sucess = uart->write(DVLSet.data,DVLSet.len);
+		uart->write(DVLSet.data,DVLSet.len);
+		DVLSet.type = '0';
+
+	}*/
+	static int jsn = 0;
+	jsn++;
+	if(jsn>100)
+	{
+	//	gcs().send_text(MAV_SEVERITY_INFO, " send %c%c%c%c%c%c%c%c%c%c%c\n",DVLSet.data[0],DVLSet.data[1],DVLSet.data[2],DVLSet.data[3],DVLSet.data[4],DVLSet.data[5],DVLSet.data[6],DVLSet.data[7],DVLSet.data[8],DVLSet.data[9],DVLSet.data[10]);
+	//	gcs().send_text(MAV_SEVERITY_INFO, " send %d\n",(int)send_sucess);
+		jsn=0;
+	}	
+
+}
+void Sub::read_external_DVL(AP_HAL::UARTDriver *uart){
+	if (uart == nullptr) {
+			return;
+		}
+	
+	uint32_t readtime=AP_HAL::micros();//微秒
+	static uint32_t DVLvalidtime=AP_HAL::millis();//毫秒
+	unsigned char received_char;
+	static bool exitLoop = FALSE;
+	exitLoop = FALSE;
+	while(uart->available()&&!exitLoop)//recive num
+	{
+		received_char = uart->read();
+
+		if (received_char == 'w')// 开始
+		{
+			RxDVL[0] = received_char;
+		}
+		if (RxDVL[0] == 'w' && received_char != '\r' && received_char != '\n')//得到开头 并且不是 尾  后开始存数据  '\r' 回车 '\n' 换行
+		{
+			RxDVL[DVL_RxCnt++]=received_char;//保存数据
+		}
+		if ((received_char == '\r') || (received_char == '\n'))//到达帧尾
+		{
+			if(RxDVL[0] == 'w' && RxDVL[1] == 'r'&&(RxDVL[2] == 'z'||RxDVL[2] == 'p'||RxDVL[2] == 'a'||RxDVL[2] == 'n'||RxDVL[2] == '?')){
+				//开始校验
+				uint8_t stringobj[DVL_RxCnt];
+				charArrtobyteArr(RxDVL,stringobj,DVL_RxCnt);//转换成byte数组
+				uint8_t crcsum = crc_crc8(stringobj, DVL_RxCnt-3);//使用模拟测试字符数组
+				uint8_t dh = chartodec(RxDVL[DVL_RxCnt-2]);
+				uint8_t dl = chartodec(RxDVL[DVL_RxCnt-1]);
+				 for (int gcsi2 = 0; gcsi2 < DVL_RxCnt/32 + 1; gcsi2++)
+				{
+					 gcs().send_text(MAV_SEVERITY_INFO, "org %c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",RxDVL[gcsi2*32+0],RxDVL[gcsi2*32+1],RxDVL[gcsi2*32+2],RxDVL[gcsi2*32+3],RxDVL[gcsi2*32+4],RxDVL[gcsi2*32+5],RxDVL[gcsi2*32+6],RxDVL[gcsi2*32+7],
+																							   RxDVL[gcsi2*32+8],RxDVL[gcsi2*32+9],RxDVL[gcsi2*32+10],RxDVL[gcsi2*32+11],RxDVL[gcsi2*32+12],RxDVL[gcsi2*32+13],RxDVL[gcsi2*32+14],RxDVL[gcsi2*32+15],
+																							   RxDVL[gcsi2*32+16],RxDVL[gcsi2*32+17],RxDVL[gcsi2*32+18],RxDVL[gcsi2*32+19],RxDVL[gcsi2*32+20],RxDVL[gcsi2*32+21],RxDVL[gcsi2*32+22],RxDVL[gcsi2*32+23],
+																							   RxDVL[gcsi2*32+24],RxDVL[gcsi2*32+25],RxDVL[gcsi2*32+26],RxDVL[gcsi2*32+27],RxDVL[gcsi2*32+28],RxDVL[gcsi2*32+29],RxDVL[gcsi2*32+30],RxDVL[gcsi2*32+31]);
+				 }
+
+				
+				if (crcsum == dh*16+dl)
+				{//校验通过
+					charArrtodecArr(RxDVL,stringobj,DVL_RxCnt-2);//string 类型转为数据类型 计算数据
+					switch (RxDVL[2]) {
+					case 'z'://Velocity report
+						//Velocity report wrz, [vx],[vy],[vz],[valid],[altitude],[fom],[covariance],
+						//[time_of_validity],[time_of_transmission],[time],[status]
+						DVLdata_wrz.Vx = arryf[0];
+						DVLdata_wrz.Vy = arryf[1];
+						DVLdata_wrz.Vz = arryf[2];
+						DVLdata_wrz.valid = arryf[3];//  'y'/'n'
+						DVLdata_wrz.altitude = arryf[4];
+						DVLdata_wrz.fom = arryf[5];
+						DVLdata_wrz.covariancearr[0] = arryf[6];
+						DVLdata_wrz.covariancearr[1] = arryf[7];
+						DVLdata_wrz.covariancearr[2] = arryf[8];
+						DVLdata_wrz.covariancearr[3] = arryf[9];
+						DVLdata_wrz.covariancearr[4] = arryf[10];
+						DVLdata_wrz.covariancearr[5] = arryf[11];
+						DVLdata_wrz.covariancearr[6] = arryf[12];
+						DVLdata_wrz.covariancearr[7] = arryf[13];
+						DVLdata_wrz.covariancearr[8] = arryf[14];
+						DVLdata_wrz.timeofvalidity = timearry[0];
+						DVLdata_wrz.timeoftransmission = timearry[1];
+						DVLdata_wrz.time = arryf[15];
+						DVLdata_wrz.status = (int)arryf[16];
+						//树莓派源码中使用的是这一帧数据而不是wrp
+						gcs().send_text(MAV_SEVERITY_INFO, "z %d %c %.3f %.3f %.3f %.3f",DVLdata_wrz.status,DVLdata_wrz.valid,DVLdata_wrz.time,DVLdata_wrz.Vx,DVLdata_wrz.Vy,DVLdata_wrz.Vz) ;
+						gcs().send_text(MAV_SEVERITY_INFO, "z %lld %lld %.3f",DVLdata_wrz.timeofvalidity,DVLdata_wrz.timeoftransmission,DVLdata_wrz.time) ;
+
+						if(DVLdata_wrz.status == 0){//0正常  1 故障
+							//说明数据有效
+							if (DVLdata_wrz.valid == 'y')//说明高度和速度有效
+							{
+								//transfer_position_delta_msg();
+							
+								DVLvalidtime=AP_HAL::millis();
+								gcs().send_text(MAV_SEVERITY_INFO, "wrz %ld",DVLvalidtime) ;
+							}
+                			  
+						}	
+						exitLoop = TRUE;//退出循环
+						break;
+					case 'u'://Transducer report
+							// wru, [id],[velocity],[distance],[rssi],[nsd]
+							DVLdata_wru.id = (int)arryf[0];
+							DVLdata_wru.velocity = arryf[1];
+							DVLdata_wru.distance= arryf[2];
+							DVLdata_wru.rssi = (int)arryf[3];
+							DVLdata_wru.nsd = (int)arryf[4];
+							gcs().send_text(MAV_SEVERITY_INFO, "wru %d %f %f %d",DVLdata_wru.id,DVLdata_wru.velocity,DVLdata_wru.distance,DVLdata_wru.rssi) ;
+							exitLoop = TRUE;//退出循环
+						break;
+					case 'p'://Dead reckoning report
+						// wrp, [time_stamp],[x],[y],[z],[pos_std],[roll],[pitch],[yaw],[status]
+							DVLdata_wrp.time_stamp=timearry[2];//s  不能准确得到或者说不能准确打印出来
+							DVLdata_wrp.Dx = arryf[0];
+							DVLdata_wrp.Dy = arryf[1];
+							DVLdata_wrp.Dz = arryf[2];
+							DVLdata_wrp.pos_std = arryf[3];
+							DVLdata_wrp.roll = arryf[4];
+							DVLdata_wrp.pitch = arryf[5];
+							DVLdata_wrp.yaw = arryf[6];
+							DVLdata_wrp.status = (int)arryf[7];
+							gcs().send_text(MAV_SEVERITY_INFO, "wrp %f %f %f %f",DVLdata_wrp.Dx,DVLdata_wrp.Dy,DVLdata_wrp.Dz,DVLdata_wrp.pos_std) ;
+
+							exitLoop = TRUE;//退出循环
+							break;
+						//Dead reckoning can be reset by issuing the wcr command. The reply will be an ack ( wra ) if
+						//the reset is successful, and a nak ( wrn ) if not.
+					case 'a'://下发命令反馈 Successfully
+							/*memset(DVLreturn.data, 0, 32);
+							DVLreturn.type = 'V';
+							DVLreturn.len = 3;
+							DVLreturn.data[0] = 'w';
+							DVLreturn.data[1] = 'r';
+							DVLreturn.data[2] = 'a';*/
+							gcs().send_text(MAV_SEVERITY_INFO, "wra");
+							exitLoop = TRUE;//退出循环
+						break;	
+					case ('n'||'?')://下发反馈 失败
+							/*memset(DVLreturn.data, 0, 32);
+							DVLreturn.type = 'V';
+							DVLreturn.len = 3;
+							DVLreturn.data[0] = 'w';
+							DVLreturn.data[1] = 'r';
+							DVLreturn.data[2] = 'n';*/
+							gcs().send_text(MAV_SEVERITY_INFO, "wrn");
+							exitLoop = TRUE;//退出循环
+						break;
+					case 'c'://读配置成功
+							/*memset(DVLreturn.data, 0, 32);
+							DVLreturn.type = 'V';
+							DVLreturn.len = DVL_RxCnt;
+							for (int copyi = 0;  copyi < DVL_RxCnt; copyi++)
+							{
+								DVLreturn.data[copyi] = RxDVL[copyi];
+							}*/
+							gcs().send_text(MAV_SEVERITY_INFO, "wrc");
+							exitLoop = TRUE;//退出循环
+						break;	
+							
+					}
+					DVL_RxCnt =0;//数据处理完了
+					exitLoop = TRUE;//退出while
+					memset(RxDVL, 0, sizeof(unsigned char)*240);
+				}
+				else{
+					DVL_RxCnt =0;//校验错误
+					//exitLoop = TRUE;//退出while
+					memset(RxDVL, 0, sizeof(unsigned char)*240);
+				}
+			}
+			else{
+				DVL_RxCnt =0;
+				//exitLoop = TRUE;//退出while
+				//不是要接收数据的数据帧
+				memset(RxDVL, 0, sizeof(unsigned char)*240);
+			}
+				
+		}
+		if(AP_HAL::micros() - readtime >800)//时间溢出0.8ms micros是微秒
+		{//接收数据超时
+				DVL_RxCnt=0;
+				memset(RxDVL, 0, sizeof(unsigned char)*240);
+				gcs().send_text(MAV_SEVERITY_INFO, " E read timeout");
+				break;//退出while
+
+		}
+		
+	}
+	if (AP_HAL::millis() - DVLvalidtime>3000)//超过3秒没有可靠数据 millis是毫秒
+		{
+		DVLvalidtime = AP_HAL::micros();
+		//gcs().send_text(MAV_SEVERITY_WARNING, " DVL lost valid data");
+		}
+
+	
+}
+int Sub::chartodec(unsigned char _ch){
+	int dl = 0;
+	if(_ch>='a' && _ch<='f'){
+		dl = _ch-87;
+	}else if(_ch>='0' && _ch<='9'){
+		dl = _ch-48;
+	}else{
+		//不是数字
+	}
+	return dl;
+
+}
+
+

ArduSub/control_althold.cpp

@@ -40,90 +40,53 @@ bool Sub::althold_init()
 
 void Sub::handle_attitude()
 {
-    uint32_t tnow = AP_HAL::millis();
-
-    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
-
-    // 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_s = target_roll;
-        last_pitch_s = target_pitch;
-        last_yaw_s = target_yaw;
-        
-        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;
-		
-
-		if (prepare_state == Horizontal)
-	   {
-		   pitch_input = 0;
-		   target_roll = 0;
-		   target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
-	   }else if(prepare_state == Vertical)
-	   	{
-		   pitch_input = -8000;
-		   target_roll = ((float)channel_yaw->get_control_in()*0.6);
-		   
-		   target_yaw = 0;
-	   }
-	   else{
-	   		pitch_input = 0;
-	   	}
-	   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;
-		last_roll_s = ahrs.roll_sensor;
-	  	last_input_ms_stable = tnow;
-	  	
-	  	}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_s = ahrs.yaw_sensor;
-			
-            last_input_ms_stable = tnow;
-        } else if (tnow < last_input_ms_stable + 250) {
-            // just brake for a few mooments so we don't bounce
-            last_yaw_s = 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_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;
-		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;
-		}
-        
-    }
+		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());
+
+			target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+	        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);
+	        } 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;
+	            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);
+	        }
+	    }
 
 }
 
@@ -147,8 +110,11 @@ void Sub::althold_run()
 
     // Vehicle is armed, motors are free to run
     motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+	
+	handle_attitude_sport();
+
+    //handle_attitude();
 
-    handle_attitude();
 
     pos_control.update_z_controller();
     // Read the output of the z controller and rotate it so it always points up
@@ -156,12 +122,14 @@ void Sub::althold_run()
     // Output the Z controller + pilot input to all motors.
 
     //TODO: scale throttle with the ammount of thrusters in the given direction
-    motors.set_throttle(0.5+throttle_vehicle_frame.z + channel_throttle->norm_input()-0.5);
+   // motors.set_throttle(0.5+throttle_vehicle_frame.z + channel_throttle->norm_input()-0.5);
+    motors.set_throttle(throttle_vehicle_frame.z + 1.0-(float)PressLevel_f*0.1);
     motors.set_forward(-throttle_vehicle_frame.x + channel_forward->norm_input());
     motors.set_lateral(-throttle_vehicle_frame.y + channel_lateral->norm_input());
 
     // 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.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)));
 
     if (fabsf(earth_frame_rc_inputs.z) > 0.05f) { // Throttle input  above 5%
        // reset z targets to current values

ArduSub/control_clean.cpp

@@ -26,7 +26,7 @@ bool Sub::clean_init()
 	last_yaw = ahrs.yaw_sensor;
 
 	//记录进入stable的YAW的当前值
-	yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
+	//yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
 	last_input_ms = AP_HAL::millis();
 	track_reset();
 	
@@ -35,6 +35,8 @@ bool Sub::clean_init()
 
 }
 void Sub::track_reset(void){
+	updowmgain =0.5;
+
 	autoclean_command = FALSE;//自动刷网
 	clean_bottom_command =FALSE;
 	clean_bottom_flag = FALSE;	
@@ -58,7 +60,7 @@ void Sub::track_reset(void){
     last_pitch = 0;
     last_yaw = ahrs.yaw_sensor;
 
-	yaw_press = (int16_t)ahrs.yaw_sensor/100;//记住方位
+	//yaw_press = (int16_t)ahrs.yaw_sensor/100;//记住方位
 
 	ahrs.get_quat_body_to_ned(_attitude_target_quat);
 	
@@ -77,6 +79,7 @@ void Sub::clean_run()
 			
 			PressLevel_f =5.0;//压力为0
 			PressLevel = no;
+			updowmgain =0.5;
 
 			//-------------
 			track_reset();
@@ -106,25 +109,27 @@ void Sub::clean_run()
 			  j=0;
 		 }
 		 //最大速度-------------
-		 int16_t maxspeed_up=90;//这里是百分比0.9
-		 maxspeed = (float)rov_control.track_limit;
+		 int16_t maxspeed_up=100;//这里是百分比0.9
 		 maxspeed_up = SRV_Channels::srv_channel(15)->get_trim()/100;
-		if(maxspeed_up<maxspeed)
+		 
+		 maxspeed = (float)rov_control.track_limit;
+		 
+		/*if(maxspeed_up<maxspeed)
 		{
 			maxspeed = maxspeed_up;
+		}*/
+		if(maxspeed_up >100)
+		{
+			maxspeed_up = 100;
 		}
-		
-		 maxspeed = constrain_float(maxspeed,30.0,90.0);
+		maxspeed = maxspeed_up;
+		 maxspeed = constrain_float(maxspeed,30.0,100.0);
 		//----------------------
 		 maxerrorspeed =SRV_Channels::srv_channel(15)->get_output_min()/100;//两个履带最大差速 ----------最深距离 厘米 
-		 maxerrorspeed = constrain_int16(maxerrorspeed,20,50);
+		 maxerrorspeed = constrain_int16(maxerrorspeed,20,maxspeed);
 		 
 		 minspeed =SRV_Channels::srv_channel(15)->get_output_max()/100;// 暂时用来做最小速度   -----最小水深
-		 if (minspeed<20)
-			 {
-			 minspeed = 20;
-			 }
-	
+		minspeed = constrain_int16(minspeed,20,maxspeed);
 		autoclean_flag_chose();//自动洗网的状态切换
 		clean_net_joystick();//默认手动洗网
 		
@@ -221,8 +226,8 @@ void Sub::clean_net_joystick(void)
 			}
 		}
 
-		motors1=constrain_int16(motors1,-((int16_t)(maxspeed)/2),100);
-		motors2=constrain_int16(motors2,-((int16_t)(maxspeed)/2),100);
+		motors1=constrain_int16(motors1,-(int16_t)(maxspeed*0.65),maxspeed);
+		motors2=constrain_int16(motors2,-(int16_t)(maxspeed*0.65),maxspeed);
 	
 		
 		static int f = 0;

ArduSub/control_manual.cpp

@@ -9,10 +9,12 @@ bool Sub::manual_init()
     // attitude hold inputs become thrust inputs in manual mode
     // set to neutral to prevent chaotic behavior (esp. roll/pitch)
     set_neutral_controls();
+	updowmgain =0.5;
 
     return true;
 }
 
+
 extern mavlink_rov_state_monitoring_t rov_message;
 
 void Sub::manual_run()
@@ -24,28 +26,40 @@ void Sub::manual_run()
         attitude_control.relax_attitude_controllers();
 		PressLevel_f =5.0;
 		PressLevel = no;
+		updowmgain =0.5;
         return;
     }
 
     motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-    motors.set_roll(channel_lateral->norm_input());//(channel_roll->norm_input());//左右移动改为roll
+    motors.set_roll(0.0);//(channel_roll->norm_input());//左右移动改为roll
     motors.set_pitch((0.5-channel_throttle->norm_input())*2);//(channel_pitch->norm_input());//上升下潜改为pitch
     motors.set_yaw(channel_yaw->norm_input() * g.acro_yaw_p / ACRO_YAW_P);
    
     motors.set_forward(channel_forward->norm_input());
-    motors.set_lateral(0.0);//(channel_lateral->norm_input());
-	motors.set_throttle(1.0-(float)PressLevel_f*0.1);//0-0.5下压
+    motors.set_lateral(channel_lateral->norm_input());
+	//motors.set_throttle(1.0-(float)PressLevel_f*0.1);//0-0.5下压
+	float tt = gaindelay(1.0-(float)PressLevel_f*0.1);
+	motors.set_throttle(tt);//0-0.5下压
 	static int j = 0;
+	
 	  j++;
-	  if(j>800)
+	  if(j>300)
 	  {
-	    	//gcs().send_text(MAV_SEVERITY_INFO, " PressLevel_f %f \n",(float)PressLevel_f);
+	  	gcs().send_text(MAV_SEVERITY_INFO, " upgain %f\n",tt);//0~1.0
+		  //gcs().send_text(MAV_SEVERITY_INFO, " throttle %f\n",getgainf(channel_throttle->norm_input()));//0~1.0
+		 // gcs().send_text(MAV_SEVERITY_INFO, " yaw %f\n",getgainf(channel_yaw->norm_input()));//-1.0~1.0
+		 // gcs().send_text(MAV_SEVERITY_INFO, " forward %f\n",getgainf(channel_forward->norm_input()));//-1.0~1.0
+		 // gcs().send_text(MAV_SEVERITY_INFO, " lateral %f\n",getgainf(channel_lateral->norm_input()));//-1.0~1.0
+	    	
 		
-		  //gcs().send_text(MAV_SEVERITY_INFO, " forward lateral throttle %f %f %f \n",channel_forward->norm_input(),channel_lateral->norm_input(),channel_throttle->norm_input());
-		  
-		  // gcs().send_text(MAV_SEVERITY_INFO, " roll pitch yaw %f %f %f \n",channel_roll->norm_input(),channel_pitch->norm_input(),channel_yaw->norm_input());
-		   j=0;
+		 // gcs().send_text(MAV_SEVERITY_INFO, " forward  %d %d %d %d\n",(int16_t)channel_forward->radio_in,(int16_t)channel_forward->radio_min,(int16_t)channel_forward->radio_max,(int16_t)channel_forward->radio_trim);
+		 // gcs().send_text(MAV_SEVERITY_INFO, " lateral %d %d %d %d\n",(int16_t)channel_lateral->radio_in,(int16_t)channel_lateral->radio_min,(int16_t)channel_lateral->radio_max,(int16_t)channel_lateral->radio_trim);
+		 // gcs().send_text(MAV_SEVERITY_INFO, " throttle %d %d %d %d\n",(int16_t)channel_throttle->radio_in,(int16_t)channel_throttle->radio_min,(int16_t)channel_throttle->radio_max,(int16_t)channel_throttle->radio_trim);
+		//  gcs().send_text(MAV_SEVERITY_INFO, " yaw %d %d %d %d\n",(int16_t)channel_yaw->radio_in,(int16_t)channel_yaw->radio_min,(int16_t)channel_yaw->radio_max,(int16_t)channel_yaw->radio_trim);
+		  j=0;
 	  }
 	rov_message.pressure_level = (int)PressLevel;
 }
+
+

ArduSub/control_sport .cpp

@@ -0,0 +1,399 @@
+#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;
+		 }
+	 //变换到集体轴
+		 if (control_mode == SPORT)
+		{
+			 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 变慢
+		}else{
+			motors.set_forward(constrain_float(-throttle_vehicle_frame.x + getgainfstatble(forward),-1.0,1.0));//
+	   		motors.set_lateral(constrain_float(-throttle_vehicle_frame.y + getgainfstatble(lateral),-1.0,1.0));//
+		}
+	   
+	
+	   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.cpp

@@ -1,42 +1,54 @@
 #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
+ */
 
-// stabilize_init - initialise stabilize controller
+// althold_init - initialise althold controller
 bool Sub::sport_init()
 {
-    // set target altitude to zero for reporting
-    pos_control.set_alt_target(0);
-    if (prev_control_mode == ALT_HOLD) {
-        last_roll = ahrs.roll_sensor;
-        last_pitch = ahrs.pitch_sensor;
-    } else {
-        last_roll = 0;
-        last_pitch = 0;
+
+	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;
-	pitch_input_inc=0;
-	yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
     last_input_ms = AP_HAL::millis();
+	last_input_ms_stable = AP_HAL::millis();
+	
+	
+	
     return true;
 }
 
-// stabilize_run - runs the main stabilize controller
-// should be called at 100hz or more
-void Sub::sport_run()
-{
-    // if not armed set throttle to zero and exit immediately
-    if (!motors.armed()) {
-        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
-        attitude_control.set_throttle_out(0,true,g.throttle_filt);
-        attitude_control.relax_attitude_controllers();
-        last_roll = 0;
-        last_pitch = 0;
-        last_yaw = ahrs.yaw_sensor;
-		yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
-        return;
-    }
-    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+void Sub::handle_attitude_sport(){
 	uint32_t tnow = AP_HAL::millis();
 
     // get pilot desired lean angles
@@ -62,19 +74,33 @@ void Sub::sport_run()
     } 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((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(channel_yaw->get_control_in());
+		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_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;
+            //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
@@ -82,37 +108,346 @@ void Sub::sport_run()
             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_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_yaw, true);
+            attitude_control.input_quaternion(attitude_desired_quat_);
         }
+
     }
 
-   // handle_attitude();
 
-    // output pilot's throttle
-    //attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);
-	attitude_control.set_throttle_out(1.0-(float)PressLevel_f*0.1, false, g.throttle_filt);//压力分级
+}
+
+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);
+		 }
+
+}
+
+// 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();
+
+    //handle_attitude();
+
+
+    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, motors.get_throttle_in_bidirectional());
+	  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.
+
+    //TODO: scale throttle with the ammount of thrusters in the given direction
+   // motors.set_throttle(0.5+throttle_vehicle_frame.z + channel_throttle->norm_input()-0.5);
+   // motors.set_throttle(throttle_vehicle_frame.z + 1.0-(float)PressLevel_f*0.1);
+   
+   motors.set_throttle(throttle_vehicle_frame.z + 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 变慢
 
-    //control_in is range -1000-1000
-    //radio_in is raw pwm value
-    motors.set_forward(channel_forward->norm_input());
-    //motors.set_lateral(channel_lateral->norm_input());
-    
-	motors.set_lateral(0);
-
-	static int j = 0;
-	  j++;
-	  if(j>800)
-	  {
-	    	gcs().send_text(MAV_SEVERITY_INFO, " roll p y %d %d %d\n",(int16_t)(channel_lateral->norm_input()*5700),(int16_t)((0.5-channel_throttle->norm_input())*5700*2),channel_yaw->get_control_in());
-	    	//gcs().send_text(MAV_SEVERITY_INFO, " throttle forward lateral %f %f %f \n",(float)PressLevel_f*0.1,channel_forward->norm_input());
-	    	//gcs().send_text(MAV_SEVERITY_INFO, " channel_roll p y %d %d %d\n",channel_roll->get_control_in(),channel_pitch->get_control_in(),channel_yaw->get_control_in());
-	    	//gcs().send_text(MAV_SEVERITY_INFO, " throttle forward lateral %f %f %f \n",channel_throttle->norm_input(),channel_forward->norm_input(),channel_lateral->norm_input());
+
+
+    // 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 +20);//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;
+				
+				
+
 			
+            //pos_control.set_target_to_stopping_point_z();//有对位置的付值
+           
+		  
+        }
+    }
+	static bool lastdepth = holding_depth;
+	if(lastdepth !=holding_depth){
+		lastdepth = holding_depth;
 		
-		   j=0;
-	  }
+	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);
+
+		}
+
+	
 }
 

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_sport_.bak

@@ -0,0 +1,123 @@
+#include "Sub.h"
+extern mavlink_rov_state_monitoring_t rov_message;
+
+
+// stabilize_init - initialise stabilize controller
+bool Sub::sport_init()
+{
+    // set target altitude to zero for reporting
+    pos_control.set_alt_target(0);
+    if (prev_control_mode == ALT_HOLD) {
+        last_roll = ahrs.roll_sensor;
+        last_pitch = ahrs.pitch_sensor;
+    } else {
+        last_roll = 0;
+        last_pitch = 0;
+    }
+    last_yaw = ahrs.yaw_sensor;
+	pitch_input_inc=0;
+	yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
+    last_input_ms = 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());
+
+		target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+        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);
+        } 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;
+            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);
+        }
+    }
+
+
+}
+
+// stabilize_run - runs the main stabilize controller
+// should be called at 100hz or more
+void Sub::sport_run()
+{
+    // if not armed set throttle to zero and exit immediately
+    if (!motors.armed()) {
+        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
+        attitude_control.set_throttle_out(0,true,g.throttle_filt);
+        attitude_control.relax_attitude_controllers();
+        last_roll = 0;
+        last_pitch = 0;
+        last_yaw = ahrs.yaw_sensor;
+		yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
+        return;
+    }
+    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+
+    handle_attitude_sport();
+
+    // output pilot's throttle
+    //attitude_control.set_throttle_out(channel_throttle->norm_input(), 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);
+
+    //control_in is range -1000-1000
+    //radio_in is raw pwm value
+    motors.set_forward(channel_forward->norm_input());
+    motors.set_lateral(channel_lateral->norm_input());
+    
+	//motors.set_lateral(0);
+
+	static int j = 0;
+	  j++;
+	  if(j>800)
+	  {
+	    	gcs().send_text(MAV_SEVERITY_INFO, " roll p y %d %d %d\n",(int16_t)(channel_lateral->norm_input()*5700),(int16_t)((0.5-channel_throttle->norm_input())*5700*2),channel_yaw->get_control_in());
+	    	//gcs().send_text(MAV_SEVERITY_INFO, " throttle forward lateral %f %f %f \n",(float)PressLevel_f*0.1,channel_forward->norm_input());
+	    	//gcs().send_text(MAV_SEVERITY_INFO, " channel_roll p y %d %d %d\n",channel_roll->get_control_in(),channel_pitch->get_control_in(),channel_yaw->get_control_in());
+	    	//gcs().send_text(MAV_SEVERITY_INFO, " throttle forward lateral %f %f %f \n",channel_throttle->norm_input(),channel_forward->norm_input(),channel_lateral->norm_input());
+			
+			
+			
+		
+		   j=0;
+	  }
+}
+

ArduSub/control_stabilize.cpp

@@ -1,23 +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);
-    if (prev_control_mode == ALT_HOLD) {
-        last_roll_s = ahrs.roll_sensor;
-        last_pitch_s = ahrs.pitch_sensor;
-    } else {
-        last_roll_s = 0;
-        last_pitch_s = 0;
-    }
+    pos_control.relax_alt_hold_controllers();//目标高度为当前高度，目标速度和上次速度 当前速度
+    
+    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;
 }
@@ -26,6 +29,93 @@ bool Sub::stabilize_init()
 // should be called at 100hz or more
 extern mavlink_rov_state_monitoring_t rov_message;
 
+void Sub::handle_attitude_stablize(){
+	
+	uint32_t tnow = AP_HAL::millis();
+
+	motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+
+	// 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_s = target_roll;
+		last_pitch_s = target_pitch;
+		last_yaw_s = target_yaw;
+		
+		attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, target_yaw, true);
+	} else {
+		
+
+		int32_t pitch_input   = 0;
+		
+
+		if (prepare_state == Horizontal)
+	   {
+		   pitch_input = 0;
+		   target_roll = 0;
+		   last_roll_s = 0;
+		   target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()*1.0);
+
+		   
+	   }else if(prepare_state == Vertical)
+		{
+		   pitch_input = 8000;
+		   //target_roll = ((float)channel_yaw->get_control_in()*0.4);
+		   target_roll = 0;
+		   target_yaw = ((float)channel_yaw->get_control_in()*0.4);
+	   }
+	   else{
+			pitch_input = 0;
+		}
+	   pitch_input_inc = constrain_int32(pitch_input,-9000,9000);
+	   //-------------------------------------
+	 
+	  if (abs(target_roll) > 300 ){
+		attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, 0, 0);
+		last_yaw_s = ahrs.yaw_sensor;
+		last_roll_s = ahrs.roll_sensor;
+		last_input_ms_stable = tnow;
+		
+		}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_s = ahrs.yaw_sensor;
+			
+			last_input_ms_stable = tnow;
+		} else if (tnow < last_input_ms_stable + 250) {
+			// just brake for a few mooments so we don't bounce
+			last_yaw_s = 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_s, (float)pitch_input_inc, last_yaw_s, true);
+			
+		}//--------------------------------------------------------------------------------------------------------------
+	   static int f = 0;
+		f++;
+		if(f>800)
+		{
+	
+
+			//gcs().send_text(MAV_SEVERITY_INFO, " stableinput %f %f,%f ",(float)last_roll_s,(float)pitch_input_inc,(float)last_yaw_s);
+			f=0;
+		}
+		
+	}
+}
 
 void Sub::stabilize_run()
 {
@@ -34,24 +124,56 @@ void Sub::stabilize_run()
         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
         attitude_control.set_throttle_out(0,true,g.throttle_filt);
         attitude_control.relax_attitude_controllers();
+		pos_control.relax_alt_hold_controllers();//目标高度为当前高度，目标速度和上次速度 当前速度
         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;//压力分级复位
 		PressLevel = no;
-		yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
+		//yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
 		pitch_input_inc=0;
 		prepare_state = Horizontal;
         return;
     }
     motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-    handle_attitude();
+    handle_attitude_stablize();
+
 
-    motors.set_forward(channel_forward->norm_input());
-    motors.set_lateral(0.0);
+    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(getgainfstatble(_forward),-1.0,1.0));
+    	motors.set_lateral(constrain_float(getgainfstatble(_lateral),-1.0,1.0));
+		
+		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(1.0-(float)PressLevel_f*0.1, false, g.throttle_filt);//压力分级
+	
 	rov_message.pressure_level = int(PressLevel);
+	}
 }

ArduSub/control_surface.cpp

@@ -14,6 +14,7 @@ bool Sub::surface_init()
     // initialise position and desired velocity
     pos_control.set_alt_target(inertial_nav.get_altitude());
     pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+	gcs().send_text(MAV_SEVERITY_INFO, " surfacemode target %f %f %f",(float)pos_control.get_pos_target().z,pos_control.get_alt(),barometer.get_altitude());
 
     return true;
 

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/flight_mode.cpp

@@ -156,7 +156,8 @@ void Sub::update_flight_mode()
         clean_run();
         break;
 	case SPORT:
-		sport_run();
+		//sport_run();
+		sport_run_alt();
 		break;
 
     default:

ArduSub/joystick.cpp

@@ -7,7 +7,7 @@
 namespace {
 float cam_tilt = 1500.0;
 float cam_pan = 1500.0;
-int16_t lights1 = 0;
+
 int16_t lights2 = 0;
 int16_t rollTrim = 0;
 int16_t pitchTrim = 0;
@@ -50,6 +50,7 @@ void Sub::init_joystick()
 
     gain = constrain_float(gain, 0.1, 1.0);
 }
+extern mavlink_rov_control_t rov_control;
 
 void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons)
 {
@@ -64,6 +65,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
     // Neutralize camera tilt and pan speed setpoint
     cam_tilt = 1500;
     cam_pan = 1500;
+	
 
     // Detect if any shift button is pressed
     for (uint8_t i = 0 ; i < 16 ; i++) {
@@ -72,10 +74,12 @@ 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++) {
         if ((buttons & (1 << i))) {
+			
             handle_jsbutton_press(i,shift,(buttons_prev & (1 << i)));
             // buttonDebounce = tnow_ms;
         } else if (buttons_prev & (1 << i)) {
@@ -133,7 +137,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
   //  RC_Channels::set_override(9, lights2, tnow);       // lights 2
     RC_Channels::set_override(10, video_switch, tnow); // video switch
     
-	lights1 =constrain_int16(lights1, 0, 20000);
+	//lights1 =constrain_int16(lights1, 0, 20000);
 	lights2 =constrain_int16(lights2, 0, 20000);
 
     // Store old x, y, z values for use in input hold logic
@@ -153,7 +157,16 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
         );
     Vector3f localPitch = Vector3f(0, 1, 0);
     Vector3f localRoll = Vector3f(1, 0, 0);
-	
+	if(button==15){
+		    if(control_mode == SPORT){
+				updowmgain=0.7;
+				
+        	}else{
+			updowmgain=0.8;
+        	}
+			delaygain =0;
+			
+	}
     // Act based on the function assigned to this button
     switch (get_button(button)->function(shift)) {
     case JSButton::button_function_t::k_arm_toggle:
@@ -174,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;
@@ -245,15 +260,16 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
         break;
     case JSButton::button_function_t::k_lights1_cycle:
          if (!held) {
-			if(lights1 ==0){
-				lights1 = 20000;
-				//sub.lights = 1;
-				
-			}
-			else{
-				lights1 = 0;
-				//sub.lights = 0;
-			}
+		 	RC_Channel* chan = RC_Channels::rc_channel(8);
+		 	uint16_t min = chan->get_radio_min();
+            uint16_t max = chan->get_radio_max();
+			 if(lights1 ==min){
+			    lights1 = constrain_float(max, min, max);
+       		 }
+		   else{
+			   lights1 = constrain_float(min, min, max);
+		   }
+			 gcs().send_text(MAV_SEVERITY_INFO,"min %d %d",rov_control.floodlight,lights1);
          }
         break;
     case JSButton::button_function_t::k_lights1_brighter:
@@ -262,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:
@@ -272,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:
@@ -285,6 +303,9 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
 			   lights2 = 0;
 			   sub.usblpoweroff = 0;
 		   }
+
+
+		   
 		}
 
         break;
@@ -320,53 +341,17 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
         break;
     case JSButton::button_function_t::k_gain_inc:
 		{
-        if (!held) {
-			if(control_mode == CLEAN)
-			{
-					
-				turn_angle =-16.0;
-				gcs().send_text(MAV_SEVERITY_INFO,"%f turn_angle",(float)turn_angle);		
-			}else if(control_mode == STABILIZE){
-				if (!motors.armed()){
-						gcs().send_text(MAV_SEVERITY_INFO,"disarm used by current yaw is %f",(float)yaw_press);
-					}
-				else{
-						
-						yaw_press +=3;
-						if(yaw_press>=360){
-							yaw_press-=360;
-						}
-						gcs().send_text(MAV_SEVERITY_INFO,"%d %f  yaw++",(int)yaw_press,(float)yaw_press);
-					}
-	
-			}
+        {
 
+			
         }
         break;
 		}
     case JSButton::button_function_t::k_gain_dec:
 		{
 		
-        if (!held) {
-			if(control_mode == CLEAN)
-			{
-					
-				turn_angle =16.0;
-				gcs().send_text(MAV_SEVERITY_INFO,"%f turn_angle",(float)turn_angle);		
-			}else if(control_mode == STABILIZE){
-				if (!motors.armed()){
-						gcs().send_text(MAV_SEVERITY_INFO,"disarm used by current yaw is %f",(float)yaw_press);
-					}
-				else{
-						
-						yaw_press -=3;
-						if(yaw_press<0){
-							yaw_press+=360;
-						}
-						gcs().send_text(MAV_SEVERITY_INFO,"%d %f  yaw--",(int)yaw_press,(float)yaw_press);
-					}
+        {
 
-			}
 
         }
         break;
@@ -657,42 +642,43 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
     case JSButton::button_function_t::k_custom_2:
        	if (!held) {
 			//十级
+			
 			if(PressLevel == first)
 			{
 				PressLevel = second;
-				 PressLevel_f = 0.8;//1.1
+				 PressLevel_f = 0.5;//
 			}else if(PressLevel == second){
 				PressLevel = third;
-				PressLevel_f = 1.2;//1.4
+				PressLevel_f = 1.0;//
 			   
 			}else if(PressLevel == third){
 				PressLevel = forth;
-				PressLevel_f = 1.75;//1.8
+				PressLevel_f = 1.8;//
 			}else if (PressLevel == forth){
 				PressLevel = fifth;
-				PressLevel_f = 2.5;//2.5
+				PressLevel_f = 4.5;
 			}else if(PressLevel == fifth){
 				PressLevel = no;
-				PressLevel_f = 5.0;	//5.0
+				PressLevel_f = 5.0;	//
 			}else if(PressLevel == no){
 				PressLevel = sixth;
-				PressLevel_f = 7.5;//7.5
+				PressLevel_f = 5.5;
 			}else if (PressLevel == sixth){
 				PressLevel = seventh;
-				PressLevel_f = 8.25;//8.2
+				PressLevel_f = 8.2;
 			}else if(PressLevel == seventh){
 				PressLevel = eighth;
-				PressLevel_f = 8.8;//8.6
+				PressLevel_f = 9.0;
 			}else if (PressLevel == eighth){
 				PressLevel = ninth;
-				PressLevel_f = 9.2;//8.9
+				PressLevel_f = 9.5;
 				
 			}else if (PressLevel == ninth){
 				PressLevel = tenth;
-				PressLevel_f = 9.4;//9.1
+				PressLevel_f = 10.0;
 			}else{
 				PressLevel = tenth;
-				PressLevel_f = 9.4;//9.1
+				PressLevel_f = 10.0;
 			}
 			gcs().send_text(MAV_SEVERITY_INFO,"%f level",(float)PressLevel_f);
 		}
@@ -700,41 +686,44 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
     case JSButton::button_function_t::k_custom_3:
         if (!held)
 		 {//十级
-		 if(PressLevel == tenth)
+		 
+		 	if(PressLevel == tenth)
 			 {
 				 PressLevel = ninth;
-				 PressLevel_f = 9.2;//8.9
+				 PressLevel_f = 9.5;//8.9
 			 }else if(PressLevel == ninth) {
 				 PressLevel = eighth;
-				 PressLevel_f = 8.8;//8.6
+				 PressLevel_f = 9.0;//8.6
 			 }else if(PressLevel == eighth){
 				 PressLevel = seventh;
-				 PressLevel_f = 8.25;//8.2
+				 PressLevel_f = 8.2;//8.2
 			 }else if (PressLevel == seventh){
 				 PressLevel = sixth;
-				  PressLevel_f = 7.5;//7.5
+				  PressLevel_f = 5.5;//7.5
 			 }else if(PressLevel == sixth){
 				 PressLevel = no;
 				  PressLevel_f = 5.0;//5.0
 			 }else if(PressLevel == no){
 				 PressLevel = fifth;
-				 PressLevel_f = 2.5; //2.5
+				 PressLevel_f = 4.5; //2.5
 			 }else if (PressLevel == fifth){
 				 PressLevel = forth;
-				  PressLevel_f = 1.75;//1.8	 
+				  PressLevel_f = 1.8;//1.8	 
 			 }else if(PressLevel == forth){
 				 PressLevel = third;
-				  PressLevel_f = 1.2;//1.4
+				  PressLevel_f = 1.0;//1.4
 			 }else if (PressLevel == third){
 				 PressLevel = second;
-				  PressLevel_f = 0.8;	//1.1
+				  PressLevel_f = 0.5;	//1.1
 			 }else if (PressLevel == second){
 				 PressLevel = first;
-				  PressLevel_f = 0.6;//0.9	 
+				  PressLevel_f = 0.0;//0.9	 
 			 }else{
 				 PressLevel = first;
-				  PressLevel_f = 0.6; //0.9
-			 }	 
+				  PressLevel_f = 0.0; //0.9
+			 }
+		 
+		
 			 gcs().send_text(MAV_SEVERITY_INFO,"%f level",(float)PressLevel_f);
 		}
         break;
@@ -742,17 +731,30 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
         // Not implemented
         break;
     case JSButton::button_function_t::k_custom_5:
-        // Not implemented
+    	{
+    		if(control_mode == SPORT)
+			{
+			updowmgain=0.3;//下
+			}
+			else{
+				updowmgain=0.2;//下
+			}
+			delaygain =0;
+    	}
         break;
     case JSButton::button_function_t::k_custom_6:
-        if (!held){
-		  	if(clean_mode == 0){
-				clean_mode =1;	
-			}else{
-				clean_mode =0;
-			}
-		  	
-        } 
+    	{ 
+	    	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,25 +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()
 {
+	
     if (!motors.armed()) { // only update when armed
         set_surfaced(false);
         set_bottomed(false);
+		
         return;
     }
 
     Vector3f velocity;
-    ahrs.get_velocity_NED(velocity);
+	//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;
@@ -27,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);
+                set_bottomed(true);//03.02
             }
 
         } else {
@@ -59,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);
+                set_bottomed(true);//03.02
             }
 
         } else { // we're not at the limits of throttle, so reset both detectors
@@ -75,6 +215,7 @@ void Sub::update_surface_and_bottom_detector()
         set_surfaced(false);
         set_bottomed(false);
     }
+	
 }
 
 void Sub::set_surfaced(bool at_surface)

ArduSub/version.h

@@ -6,12 +6,12 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ArduSub V4.0.6"
+#define THISFIRMWARE "ArduSub V4.0.3"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,0,6,FIRMWARE_VERSION_TYPE_OFFICIAL
+#define FIRMWARE_VERSION 4,0,4,FIRMWARE_VERSION_TYPE_OFFICIAL
 
 #define FW_MAJOR 4
 #define FW_MINOR 0
-#define FW_PATCH 7
+#define FW_PATCH 4
 #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -205,9 +205,9 @@ void AC_AttitudeControl::reset_rate_controller_I_terms()
 void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat)
 {
     // calculate the attitude target euler angles
-    _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
+   // _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
 
-    Quaternion attitude_error_quat = _attitude_target_quat.inverse() * attitude_desired_quat;
+    Quaternion attitude_error_quat = _attitude_target_quat.inverse() * attitude_desired_quat;//Q(n)(tb)的转置 *Q(n)(tbnew) = Q(tb)(tbnew),attitude_error_quat在tb坐标下
     Vector3f attitude_error_angle;
     attitude_error_quat.to_axis_angle(attitude_error_angle);
 
@@ -218,11 +218,12 @@ void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat)
         _attitude_target_ang_vel.x = input_shaping_angle(wrap_PI(attitude_error_angle.x), _input_tc, get_accel_roll_max_radss(), _attitude_target_ang_vel.x, _dt);
         _attitude_target_ang_vel.y = input_shaping_angle(wrap_PI(attitude_error_angle.y), _input_tc, get_accel_pitch_max_radss(), _attitude_target_ang_vel.y, _dt);
         _attitude_target_ang_vel.z = input_shaping_angle(wrap_PI(attitude_error_angle.z), _input_tc, get_accel_yaw_max_radss(), _attitude_target_ang_vel.z, _dt);
-
-        // Limit the angular velocity
+		//_attitude_target_ang_vel在tb系下
+        // Limit the angular velocity  
         ang_vel_limit(_attitude_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
         // Convert body-frame angular velocity into euler angle derivative of desired attitude
-        ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
+        //_attitude_target_ang_vel 表示arget坐标系下的速度 以下这里没用
+        //ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
     } else {
         _attitude_target_quat = attitude_desired_quat;
 
@@ -599,20 +600,34 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
     float roll_rate_rads = radians(roll_rate_bf_cds * 0.01f);
     float pitch_rate_rads = radians(pitch_rate_bf_cds * 0.01f);
     float yaw_rate_rads = radians(yaw_rate_bf_cds * 0.01f);
+//要先将roll_rate_rads，pitch_rate_rads，yaw_rate_rads转化到tb坐标系，原来是在N系-------wangdan
+    Quaternion _ang_vel_quat = Quaternion(0.0f, 0.0f,0.0f, yaw_rate_rads);//手柄输出四元数 这样可以使得转向围绕的是地球坐标系
+
+	Quaternion thrust_vec_quat_body;
+	thrust_vec_quat_body = _attitude_target_quat.inverse() * _ang_vel_quat * _attitude_target_quat;//手柄转换到tb系
+//-----------------------------------------------------------------------------------------------------------------------
 
     // calculate the attitude target euler angles
-    _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
+    //_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
 
     if (_rate_bf_ff_enabled) {
         // Compute acceleration-limited body frame rates
         // When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
         // the output rate towards the input rate.
-        _attitude_target_ang_vel.x = input_shaping_ang_vel(_attitude_target_ang_vel.x, roll_rate_rads, get_accel_roll_max_radss(), _dt);
+        /*_attitude_target_ang_vel.x = input_shaping_ang_vel(_attitude_target_ang_vel.x, roll_rate_rads, get_accel_roll_max_radss(), _dt);
         _attitude_target_ang_vel.y = input_shaping_ang_vel(_attitude_target_ang_vel.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt);
         _attitude_target_ang_vel.z = input_shaping_ang_vel(_attitude_target_ang_vel.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt);
-
-        // Convert body-frame angular velocity into euler angle derivative of desired attitude
-        ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
+*/
+//使用机体四元数------------wangdan--------------
+			_attitude_target_ang_vel.x = input_shaping_ang_vel(_attitude_target_ang_vel.x, roll_rate_rads + thrust_vec_quat_body.q2, get_accel_roll_max_radss(), _dt);
+			_attitude_target_ang_vel.y = input_shaping_ang_vel(_attitude_target_ang_vel.y, pitch_rate_rads + thrust_vec_quat_body.q3, get_accel_pitch_max_radss(), _dt);
+			_attitude_target_ang_vel.z = input_shaping_ang_vel(_attitude_target_ang_vel.z, thrust_vec_quat_body.q4, get_accel_yaw_max_radss(), _dt);
+//-------------------------------------
+        // Convert body-frame angular velocity into euler angle derivative of desired 
+        //attitude _attitude_target_ang_vel默认实在tb坐标系下
+        //注意后面用到的是_attitude_target_ang_vel，也就是说这个函数没有作用
+        
+        //ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
     } else {
         // When feedforward is not enabled, the quaternion is calculated and is input into the target and the feedforward rate is zeroed.
         Quaternion attitude_target_update_quat;
@@ -743,7 +758,7 @@ void AC_AttitudeControl::attitude_controller_run_quat()
     thrust_heading_rotation_angles(_attitude_target_quat, attitude_vehicle_quat, attitude_error_vector, _thrust_error_angle);
 
     // Compute the angular velocity target from the attitude error
-    _rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);
+    _rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);//rate_target_ang_vel则是在当前姿态cb下
 
     // Add feedforward term that attempts to ensure that roll and pitch errors rotate with the body frame rather than the reference frame.
     // todo: this should probably be a matrix that couples yaw as well.
@@ -754,8 +769,14 @@ void AC_AttitudeControl::attitude_controller_run_quat()
 
     // Add the angular velocity feedforward, rotated into vehicle frame
     Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);
-    Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
+//attitude_vehicle_quat在N系下的当前姿态 _attitude_target_quat在N系下的目标姿态 to_to_from_quat表示由目标转向当前的旋转
+	Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
+
+	//将tb下的期望角速率旋转至cb下获取当前姿态的角速率期望前馈叠加量desired_ang_vel_quat      
+	//attitude_target_ang_vel_quat原来的的ardusub程序是在目标坐标系下
+
     Quaternion desired_ang_vel_quat = to_to_from_quat.inverse() * attitude_target_ang_vel_quat * to_to_from_quat;
+   //  Quaternion desired_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);//wangdan
 
     // Correct the thrust vector and smoothly add feedforward and yaw input
     if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE * 2.0f) {
@@ -792,11 +813,11 @@ void AC_AttitudeControl::attitude_controller_run_quat()
 void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat, const Quaternion& att_from_quat, Vector3f& att_diff_angle, float& thrust_vec_dot)
 {
     Matrix3f att_to_rot_matrix; // rotation from the target body frame to the inertial frame.
-    att_to_quat.rotation_matrix(att_to_rot_matrix);
+    att_to_quat.rotation_matrix(att_to_rot_matrix);//ned坐标系
     Vector3f att_to_thrust_vec = att_to_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);
 
     Matrix3f att_from_rot_matrix; // rotation from the current body frame to the inertial frame.
-    att_from_quat.rotation_matrix(att_from_rot_matrix);
+    att_from_quat.rotation_matrix(att_from_rot_matrix);//ned坐标系
     Vector3f att_from_thrust_vec = att_from_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);
 
     // the cross product of the desired and target thrust vector defines the rotation vector
@@ -816,8 +837,8 @@ void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat,
     Quaternion thrust_vec_correction_quat;
     thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot);
 
-    // Rotate thrust_vec_correction_quat to the att_from frame
-    thrust_vec_correction_quat = att_from_quat.inverse() * thrust_vec_correction_quat * att_from_quat;
+    // Rotate thrust_vec_correction_quat to the att_from frame  这个旋转其实是变换了坐标系，但是向量本身没有变
+    thrust_vec_correction_quat = att_from_quat.inverse() * thrust_vec_correction_quat * att_from_quat;//thrust_vec_correction_quat 在ned坐标系 转换到机体坐标系
 
     // calculate the remaining rotation required after thrust vector is rotated transformed to the att_from frame
     Quaternion yaw_vec_correction_quat = thrust_vec_correction_quat.inverse() * att_from_quat.inverse() * att_to_quat;

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -18,7 +18,7 @@
 #define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS        radians(10.0f)   // minimum body-frame acceleration limit for the stability controller (for yaw axis)
 #define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS        radians(120.0f)  // maximum body-frame acceleration limit for the stability controller (for yaw axis)
 #define AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT_CDS        6000      // constraint on yaw angle error in degrees.  This should lead to maximum turn rate of 10deg/sec * Stab Rate P so by default will be 45deg/sec.
-#define AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS   4500.0f//110000.0f // default maximum acceleration for roll/pitch axis in centidegrees/sec/sec
+#define AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS   110000.0f // default maximum acceleration for roll/pitch axis in centidegrees/sec/sec
 #define AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_CDSS    22000//27000.0f  // default maximum acceleration for yaw axis in centidegrees/sec/sec
 
 #define AC_ATTITUDE_RATE_CONTROLLER_TIMEOUT             1.0f    // body-frame rate controller timeout in seconds
@@ -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_AttitudeControl_Sub.cpp

@@ -331,16 +331,7 @@ void AC_AttitudeControl_Sub::rate_controller_run()
     _motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll));
     _motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch));
     _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw));
-	static int f = 0;
-	f++;
-	if(f>200)
-	{
-		
-		//gcs().send_text(MAV_SEVERITY_INFO, " get_P_pid %d, %d, %d,",(int)((get_rate_roll_pid().get_p()+get_rate_roll_pid().get_i())*1000),(int)((get_rate_pitch_pid().get_p()+get_rate_pitch_pid().get_i())*1000),(int)((get_rate_yaw_pid().get_p()+get_rate_yaw_pid().get_i())*1000));
-		//gcs().send_text(MAV_SEVERITY_INFO, " get_I_pid %d, %d, %d,",(int)(get_rate_roll_pid().get_i()*1000),(int)(get_rate_pitch_pid().get_i()*1000),(int)(get_rate_yaw_pid().get_i()*1000));
-	
-		f=0;
-	}
+
 
     control_monitor_update();
 }

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

@@ -2,6 +2,7 @@
 #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;
 
@@ -205,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);
 
@@ -222,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;
+	
 }
 
 ///
@@ -247,12 +252,12 @@ 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();
+        calc_leash_length_z();// 刹车长度
     }
 }
 
@@ -401,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
@@ -475,14 +482,36 @@ void AC_PosControl::update_z_controller()
     _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();
 
     // call z-axis position controller
-    run_z_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
@@ -501,8 +530,11 @@ void AC_PosControl::calc_leash_length_z()
 // 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();
+    //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;
@@ -543,13 +575,19 @@ void AC_PosControl::run_z_controller()
         _vel_target.z += _vel_desired.z;
     }
 
+	
     // the following section calculates acceleration required to achieve the velocity target
 
-    const Vector3f& curr_vel = _inav.get_velocity();
+    //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) {
+    if (_flags.reset_rate_to_accel_z) {//改变高度时候复位
         _vel_last.z = _vel_target.z;
     }
 
@@ -565,6 +603,7 @@ void AC_PosControl::run_z_controller()
         _accel_desired.z = 0.0f;
     }
 
+
     // store this iteration's velocities for the next iteration
     _vel_last.z = _vel_target.z;
 
@@ -573,13 +612,15 @@ void AC_PosControl::run_z_controller()
         // 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 = _p_vel_z.get_p(_vel_error.z);
+   
 
     _accel_target.z += _accel_desired.z;
 
@@ -595,10 +636,247 @@ void AC_PosControl::run_z_controller()
         _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 +_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);
+
+
+
+}
+
+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;
+		
+		_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
+    // 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;
+    }
+	float scaler = 0.0;
+	
+	/*scaler = 10.0;//加速度的KI
+	float kp = 0.0;
+
+	if (fabsf(_pos_error.z)>scaler)
+	{
+			kp = 2.0;//位置的KP
+			_p_pos_z.kP(kp);
+	
+			kp = 8.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;//16trim 加速度的KI    
+	float kp = 0.0;
+
+	if (fabsf(_pos_error.z)>scaler)
+	{
+			kp = (float)SRV_Channels::srv_channel(14)->get_output_max()/1000;//15max 位置的KP
+			_p_pos_z.kP(kp);//2.0
+	
+			kp = (float)SRV_Channels::srv_channel(14)->get_output_min()/1000;//15min 速度的Kp
+			_p_vel_z.kP(kp);//8.0
+			
+			kp = (float)SRV_Channels::srv_channel(13)->get_output_min()/1000;//14min 加速度的KP
+			_pid_accel_z.kP(kp);//1.3
+			
+			kp = (float)SRV_Channels::srv_channel(13)->get_trim()/10000;//14trim 加速度的KI
+			_pid_accel_z.kI(kp);//0.8
+			
+	}else{
+			kp = (float)SRV_Channels::srv_channel(14)->get_trim()/1000;//15trim 位置的KP
+			_p_pos_z.kP(kp);//2.0
+			
+			kp = (float)SRV_Channels::srv_channel(13)->get_output_max()/1000;//14max 速度的KP
+			_p_vel_z.kP(kp);//7.0
+
+			kp = (float)SRV_Channels::srv_channel(15)->get_output_max()/1000;//16max 加速度的KP
+			_pid_accel_z.kP(kp);//1.0
+	
+			kp = (float)SRV_Channels::srv_channel(15)->get_output_min()/1000;//16min 加速度的KI
+	        _pid_accel_z.kI(kp);//0.8
+		}
+	static uint16_t count2 = 0;
+	count2++;
+	if (count2>300)
+	{
+		count2 = 0;
+		gcs().send_text(MAV_SEVERITY_INFO, "PID%f,%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(),scaler);
+		
+	}
+    // calculate _vel_target.z using from _pos_error.z using sqrt controller
+    
+	_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
+    _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=sub.velocity_z_filer;//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 = _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);
+
+	return thr_out;
 }
 
 ///
@@ -1105,12 +1383,12 @@ float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float k
         return POSCONTROL_LEASH_LENGTH_MIN;
     }
 
-    // calculate leash length
+    // 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;
+        leash_length = speed_cms / kP; //时间间隔很小的时候 V*dt
     } else {
-        // leash length grows at sqrt of speed further out
+        // 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));
     }
 

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -13,6 +13,8 @@
 #include <AP_Vehicle/AP_Vehicle.h>         // common vehicle parameters
 
 
+
+
 // position controller default definitions
 #define POSCONTROL_ACCELERATION_MIN             50.0f   // minimum horizontal acceleration in cm/s/s - used for sanity checking acceleration in leash length calculation
 #define POSCONTROL_ACCEL_XY                     100.0f  // default horizontal acceleration in cm/s/s.  This is overwritten by waypoint and loiter controllers
@@ -51,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
@@ -146,6 +149,7 @@ public:
 
     /// update_z_controller - fly to altitude in cm above home
     void update_z_controller();
+	float update_z_controller_f();
 
     // get_leash_down_z, get_leash_up_z - returns vertical leash lengths in cm
     float get_leash_down_z() const { return _leash_down_z; }
@@ -299,6 +303,17 @@ public:
                         const uint8_t failure_msg_len);
 
     static const struct AP_Param::GroupInfo var_info[];
+	AC_P		_p_pos_z;
+	float       _dt;    
+	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:
 
@@ -333,6 +348,8 @@ protected:
     //          set_target_to_stopping_point_z
     //          init_takeoff
     void run_z_controller();
+	float run_z_controller_f();
+	
 
     ///
     /// xy controller private methods
@@ -375,20 +392,20 @@ protected:
     // parameters
     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_pos_z;
-    AC_P        _p_vel_z;
-    AC_PID      _pid_accel_z;
+   
+   
+    
     AC_P        _p_pos_xy;
     AC_PID_2D   _pid_vel_xy;
 
     // internal variables
-    float       _dt;                    // time difference (in seconds) between calls from the main program
+                // time difference (in seconds) between calls from the main program
     uint64_t    _last_update_xy_us;     // system time (in microseconds) since last update_xy_controller call
     uint64_t    _last_update_z_us;      // system time (in microseconds) of last update_z_controller call
     float       _speed_down_cms;        // max descent rate in cm/s
     float       _speed_up_cms;          // max climb rate in cm/s
     float       _speed_cms;             // max horizontal speed in cm/s
-    float       _accel_z_cms;           // max vertical acceleration in cm/s/s
+    
     float       _accel_last_z_cms;      // max vertical acceleration in cm/s/s
     float       _accel_cms;             // max horizontal acceleration in cm/s/s
     float       _leash;                 // horizontal leash length in cm.  target will never be further than this distance from the vehicle

libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp

@@ -1,4 +1,5 @@
 #include "AC_PosControl_Sub.h"
+#include "../../ArduSub/Sub.h"
 
 AC_PosControl_Sub::AC_PosControl_Sub(const AP_AHRS_View& ahrs, const AP_InertialNav& inav,
                                      const AP_Motors& motors, AC_AttitudeControl& attitude_control) :
@@ -45,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;
@@ -55,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;
 
@@ -81,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
@@ -92,14 +96,35 @@ void AC_PosControl_Sub::set_alt_target_from_climb_rate_ff(float climb_rate_cms,
 /// relax_alt_hold_controllers - set all desired and targets to measured
 void AC_PosControl_Sub::relax_alt_hold_controllers()
 {
-    _pos_target.z = _inav.get_altitude();
-    _vel_desired.z = 0.0f;
+    //_pos_target.z = _inav.get_altitude(); // 12.19
+    //_vel_target.z = _inav.get_velocity_z();// 12.19
+    //_vel_last.z = _inav.get_velocity_z();// 12.19
+	 _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();
+	
+    _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
+}
+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,10 @@ 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();};
 
 private:
     float       _alt_max; // max altitude - should be updated from the main code with altitude limit from fence

libraries/AC_PID/AC_PID.h

@@ -104,6 +104,7 @@ public:
     void set_integrator(float i) { _integrator = constrain_float(i, -_kimax, _kimax); }
 
     const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
+	//const AP_Logger::PID_Info& get_pid_info(void)  { return _pid_info; }
 
     // parameter var table
     static const struct AP_Param::GroupInfo var_info[];

libraries/AP_Baro/AP_Baro.cpp

@@ -206,7 +206,7 @@ void AP_Baro::calibrate(bool save)
 
     // reset the altitude offset when we calibrate. The altitude
     // offset is supposed to be for within a flight
-    _alt_offset.set_and_save(0);
+    //_alt_offset.set_and_save(0);
 
     // let the barometer settle for a full second after startup
     // the MS5611 reads quite a long way off for the first second,
@@ -755,8 +755,9 @@ void AP_Baro::update(void)
 			altitude = ( 101325- corrected_pressure) / 9800.0f / _specific_gravity;//sensors[i].ground_pressure
 			sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
             if (sensors[i].alt_ok) {
-                sensors[i].altitude = altitude ;//+ _alt_offset_active;
+                sensors[i].altitude = altitude + _alt_offset;
             }
+
 			/*
             float corrected_pressure = sensors[i].pressure + sensors[i].p_correction;
             if (sensors[i].type == BARO_TYPE_AIR) {

libraries/AP_HAL/AP_HAL_Macros.h

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

libraries/AP_Motors/AP_Motors6DOF.cpp

@@ -137,10 +137,10 @@ void AP_Motors6DOF::setup_motors(motor_frame_class frame_class, motor_frame_type
     case SUB_FRAME_BLUEROV1:
         add_motor_raw_6dof(AP_MOTORS_MOT_1,     0,              0,              -1.0f,          0,                  1.0f,               0,              1);
         add_motor_raw_6dof(AP_MOTORS_MOT_2,     0,              0,              1.0f,           0,                  1.0f,               0,              2);
-        add_motor_raw_6dof(AP_MOTORS_MOT_3,     0.5f,           -0.5f,           0,             -1.0f,               0,                  0,              3);
-        add_motor_raw_6dof(AP_MOTORS_MOT_4,     -0.5f,          -0.5f,           0,             -1.0f,               0,                  0,              4);
+        add_motor_raw_6dof(AP_MOTORS_MOT_3,     1.0f,           -1.0f,           0,             -1.0f,               0,                  0,              3);
+        add_motor_raw_6dof(AP_MOTORS_MOT_4,     -1.0f,          -1.0f,           0,             -1.0f,               0,                  0,              4);
         add_motor_raw_6dof(AP_MOTORS_MOT_5,     0,              1.0f,          0,               -1.0f,               0,                  0,              5);
-        add_motor_raw_6dof(AP_MOTORS_MOT_6,     -0.25f,         0,              0,              0,                  0,                  1.0f,           6);
+        add_motor_raw_6dof(AP_MOTORS_MOT_6,     0.0f,         0,              0,              0,                  0,                  1.0f,           6);
         break;
 
     case SUB_FRAME_VECTORED_6DOF_90DEG:
@@ -227,62 +227,16 @@ void AP_Motors6DOF::output_min()
     }
 }
 
-#define ThrustScale 100000
-#define hv_min 251
-#define hv_max 24100
-#define DutyScale 100000
-#define MAXDUTY 88000
-int32_t AP_Motors6DOF::calc_thrust_to_motor(float thrust_in,int8_t i)
-{ 
-	  int16_t thrust = 0;
-	 
-	 
-	  //int16_t stephv = 500;
-	  thrust = (int32_t)(thrust_in* ThrustScale);
-	  /*
-	  	
-	  	if (thrust>=last_thrust_Dhot[i] - stephv && thrust<=last_thrust_Dhot[i] + stephv){
-			
-			last_thrust_Dhot[i] = thrust;
-		 }else if (thrust >= last_thrust_Dhot[i] + stephv)
-	 	 {//新的Dshot >当前的Dshot + 步长
-			 last_thrust_Dhot[i] += stephv;
-	 	 }
-	 	 else if(thrust <= last_thrust_Dhot[i] -stephv)
-	 	 {//新的Dshot < 当前的Dshot -步长
-	  		 last_thrust_Dhot[i] -= stephv;
-	 	 }
-
-	  	if (last_thrust_Dhot[i]< stephv && last_thrust_Dhot[i]> -stephv)
-		{
-		  last_thrust_Dhot[i] =0;
-		}
-		
-		
-		static int k = 0;
-		k++;
-		if(k>400)
-		{
-
-		//gcs().send_text(MAV_SEVERITY_INFO, "_thrust_rpyt_out %d %d ,%d \n",(int)last_thrust_Dhot[2],(int)last_thrust_Dhot[3],(int)last_thrust_Dhot[4]);
-		}
-		//int16_t speedref = last_thrust_Dhot[i]*hv_max/1000;
-	  	//return constrain_int16(speedref,-hv_max, hv_max);
-	  */
-		int16_t speedref = thrust;//last_thrust_Dhot[i];
-		return constrain_int32(speedref,-MAXDUTY, MAXDUTY);
-
-
-}
 
 
 
 extern mavlink_motor_speed_t mav_motor_speed;
 
-void AP_Motors6DOF::output_to_Dshot()
+
+void AP_Motors6DOF::output_to_PWM()
 {
-	int8_t i;
-    int32_t motor_out[AP_MOTORS_MAX_NUM_MOTORS];    // final pwm values sent to the motor
+    int8_t i;
+    int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS];    // final pwm values sent to the motor
 
     switch (_spool_state) {
     case SpoolState::SHUT_DOWN:
@@ -290,7 +244,7 @@ void AP_Motors6DOF::output_to_Dshot()
         // set motor output based on thrust requests
         for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
             if (motor_enabled[i]) {
-                motor_out[i] = 0;
+                motor_out[i] = NETRULPWM;
             }
         }
         break;
@@ -298,91 +252,81 @@ void AP_Motors6DOF::output_to_Dshot()
         // sends output to motors when armed but not flying
         for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
             if (motor_enabled[i]) {
-                motor_out[i] = 0;
+                motor_out[i] = NETRULPWM;
             }
         }
         break;
     case SpoolState::SPOOLING_UP:
     case SpoolState::THROTTLE_UNLIMITED:
     case SpoolState::SPOOLING_DOWN:
-    	{
-   			 for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
-       			 if (motor_enabled[i]) {
-				  //将油门比例转换成Dshot协议形式，增加了缓启动
-            		
-            		
-            		motor_out[i] =  calc_thrust_to_motor(_thrust_rpyt_out[i],i);
-       			}
-   			 }
-        break;}
+        // set motor output based on thrust requests
+        for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
+            if (motor_enabled[i]) {
+                motor_out[i] = calc_thrust_to_pwm(_thrust_rpyt_out[i]);
+            }
+        }
+        break;
     }
-	
-		//const AP_UAVCAN &uavcan = AP::uavcan();//6自由度电机计算出来的PWM
-
-		//将上位机转发过来的，转换成Dshot协议并使用CAN 转发
-		if(mav_motor_speed.motorTest == 1)
-		{//测试模式上位机直接控制 仅仅控制6个推进器
-			 motor_to_can[0] =(int32_t)mav_motor_speed.motor1*ThrustScale;//230 才启动
-			 motor_to_can[1] =(int32_t)mav_motor_speed.motor2*ThrustScale;
-			 motor_to_can[2] = (int32_t)mav_motor_speed.motor3*ThrustScale/2000;
-			 motor_to_can[3] = (int32_t)mav_motor_speed.motor4*ThrustScale/2000;//大于100能动
-			 motor_to_can[4] = (int32_t)mav_motor_speed.motor5*ThrustScale/2000;
-			 motor_to_can[5] = (int32_t)mav_motor_speed.motor6*ThrustScale/2000;
-			// motor_to_can[6] = mav_motor_speed.motor7;
-			// motor_to_can[7] = mav_motor_speed.motor8;
+	if(mav_motor_speed.motorTest == 1)
+	{//测试模式上位机直接控制 仅仅控制6个推进器
+		 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);
+		 motor_out[5] = (int16_t)(mav_motor_speed.motor6*500/2200+NETRULPWM);
 
 
-			
 
-			 
-		}
-		else
-		{
-			motor_to_can[0] =  motor_out[0];//赋值给can
-			motor_to_can[1] =  motor_out[1];//赋值给can
-			motor_to_can[2] =  motor_out[2];//赋值给can
-			motor_to_can[3] =  motor_out[3];//赋值给can
-			motor_to_can[4] =  motor_out[4];//赋值给can
-			motor_to_can[5] =  motor_out[5];//赋值给can
-			//motor_to_can[6] =  motor_out[6];//赋值给can
-			//motor_to_can[7] =  motor_out[7];//赋值给can
+		
 
+		 
+	}
+	
 
-		}
-		output_motor8_and_motor9();
-		static int k = 0;
-		k++;
-		if(k>400)
-		{
+    // 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;
+        	}
+    }
 
-			//gcs().send_text(MAV_SEVERITY_INFO, "motor_out %d %d %d %d\n", (int)mav_motor_speed.motorTest,(int)motor_to_can[2],(int)motor_to_can[3], (int)motor_to_can[4]);
-			k=0;
-		}
+	output_motor8_and_motor9();
 }
 
 int16_t AP_Motors6DOF::calc_thrust_to_pwm(float thrust_in) const
 {
-    return constrain_int16(1500 + thrust_in * 500, _throttle_radio_min, _throttle_radio_max);
+	int16_t _throttl = thrust_in * (NETRULPWM-1000);
+	if (_throttl<30 && _throttl>-30)
+		{
+		_throttl = 0;//死区
+		}
+    return constrain_int16(NETRULPWM + _throttl, _throttle_radio_min, _throttle_radio_max);
 }
 void AP_Motors6DOF::output_motor8_and_motor9(){
-	int8_t i;
+	
 	if(mav_motor_speed.motorTest == 1)
 	{
 	
 		rc_write(6, calc_thrust_to_pwm((float)mav_motor_speed.Ltrack/260));;//切换成上位机控制 左履带
 		rc_write(7, calc_thrust_to_pwm((float)mav_motor_speed.Rtrack/260));;//切换成上位机控制 右履带
 	}else{
-			for (i=6; i<8; i++) {
+			
 	       	
-	         	 rc_write(i, pwm_track[i-6]);
-			}
+	         	 rc_write(6, pwm_track[0]);
+				 rc_write(7, pwm_track[1]);
+			
 	}
 	static int k = 0;
 	k++;
 	if(k>400)
 	{
 	
-		gcs().send_text(MAV_SEVERITY_INFO, "mav_motor_speed %d %f %f \n", (int)mav_motor_speed.motorTest,(float)mav_motor_speed.Ltrack/260,(float)mav_motor_speed.Rtrack/260);
+	//	gcs().send_text(MAV_SEVERITY_INFO, "motor_speed %d %d \n", pwm_track[0],pwm_track[1]);
+	//	gcs().send_text(MAV_SEVERITY_INFO, "motor_speed %d %d %d \n", (int)mav_motor_speed.motorTest,calc_thrust_to_pwm((float)mav_motor_speed.Ltrack/260),calc_thrust_to_pwm((float)mav_motor_speed.Rtrack/260));
 		k=0;
 	}
 
@@ -557,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];
@@ -61,7 +64,7 @@ public:
 
     // output_to_motors - sends minimum values out to the motors
     void output_to_motors() override;
-	void output_to_Dshot() override;//dshot赋值
+	void output_to_PWM() override;//dshot赋值
 
     // This allows us to read back the output of the altidude controllers
     // The controllers are in charge of the throttle input, so this gives vehicle access/visibility to the output of those controllers

libraries/AP_Motors/AP_MotorsMulticopter.cpp

@@ -242,8 +242,8 @@ void AP_MotorsMulticopter::output()
     thrust_compensation();
 
     // convert rpy_thrust values to pwm
-    //output_to_motors();
-    output_to_Dshot();
+    output_to_PWM();
+
 
     // output any booster throttle
     output_boost_throttle();

libraries/AP_Motors/AP_MotorsMulticopter.h

@@ -10,7 +10,7 @@
 
 #define AP_MOTORS_YAW_HEADROOM_DEFAULT  200
 #define AP_MOTORS_THST_EXPO_DEFAULT     0.65f   // set to 0 for linear and 1 for second order approximation
-#define AP_MOTORS_THST_HOVER_DEFAULT    0.35f   // the estimated hover throttle, 0 ~ 1
+#define AP_MOTORS_THST_HOVER_DEFAULT    0.50f   // the estimated hover throttle, 0 ~ 1
 #define AP_MOTORS_THST_HOVER_TC         10.0f   // time constant used to update estimated hover throttle, 0 ~ 1
 #define AP_MOTORS_THST_HOVER_MIN        0.125f  // minimum possible hover throttle
 #define AP_MOTORS_THST_HOVER_MAX        0.6875f // maximum possible hover throttle
@@ -100,7 +100,7 @@ protected:
 
     // output_to_motors - sends commands to the motors
     virtual void        output_to_motors() = 0;
-	virtual void		output_to_Dshot() = 0;
+	virtual void		output_to_PWM() = 0;
 
     // update the throttle input filter
     virtual void        update_throttle_filter() override;

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
@@ -97,7 +97,10 @@ public:
     float               get_roll() const { return _roll_in; }
     float               get_pitch() const { return _pitch_in; }
     float               get_yaw() const { return _yaw_in; }
+	
+    float               get_throttle_t() const { return constrain_float(_throttle_in, 0.0f, 1.0f); }
     float               get_throttle() const { return constrain_float(_throttle_filter.get(), 0.0f, 1.0f); }
+	LowPassFilterFloat  get_throttle_filter() const { return _throttle_filter; }
     float               get_throttle_bidirectional() const { return constrain_float(2 * (_throttle_filter.get() - 0.5f), -1.0f, 1.0f); }
     float               get_forward() const { return _forward_in; }
     float               get_lateral() const { return _lateral_in; }

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1017,6 +1017,7 @@ mavlink_data64_t rov_message3;
 extern mavlink_set_slave_parameter_t get_stm32_param;
 extern mavlink_hv_reg_get_t hv_reg_get;
 extern uint8_t get_stm32_param_buf[7];
+mavlink_rov_motor_temp_t rov_motor_temp = {0,0,0,0,0,0,0,0,0,0};
 
 int countper = 0;
 
@@ -1030,37 +1031,42 @@ void GCS_MAVLINK::update_send()
 			
 			mav_motor_speed_back.Ltrack = 0;
 			mav_motor_speed_back.Rtrack =0;
-			mav_motor_speed_back.motor1 =(float)sub._telemetry[0].rpm/7;//uint32 rpm
-			mav_motor_speed_back.motor2 =(float)sub._telemetry[1].rpm/7;
-			mav_motor_speed_back.motor3 =(float)sub._telemetry[2].rpm/7;
-			mav_motor_speed_back.motor4 =(float)sub._telemetry[3].rpm/7;
-			mav_motor_speed_back.motor5 =(float)sub._telemetry[4].rpm/7;
-			mav_motor_speed_back.motor6 =(float)sub._telemetry[5].rpm/7;
+			mav_motor_speed_back.motor1 =(float)0.0;//sub._telemetry[0].rpm/7;//uint32 rpm
+			mav_motor_speed_back.motor2 =(float)0.0;//sub._telemetry[1].rpm/7;
+			mav_motor_speed_back.motor3 =(float)0.0;//sub._telemetry[2].rpm/7;
+			mav_motor_speed_back.motor4 =(float)0.0;//sub._telemetry[3].rpm/7;
+			mav_motor_speed_back.motor5 =(float)0.0;//sub._telemetry[4].rpm/7;
+			mav_motor_speed_back.motor6 =(float)0.0;//sub._telemetry[5].rpm/7;
 			mav_motor_speed_back.motor7 =0;
 			mav_motor_speed_back.motor8 =0;
 
 			rov_message.floodlight = sub.lights;
 			//pressure_level  在各个模式中已经赋值
 			rov_message.low_voltage = (float)sub._telemetry[0].voltage/10;
-			rov_message.high_voltage = (float)sub._telemetry[1].voltage/10;//sub._telemetry[0].voltage;//电压
+			rov_message.high_voltage = (float)sub._telemetry[1].voltage/10;
 			rov_message.deep =fabsf(sub.barometer.get_altitude());//深度
 			//rov_message.temp = 0;
 			rov_message.motor_block_flag =	0;
-			rov_message.motor_twine_flag[0] = 0;//(uint16_t)uavcan.thruster[0].error_count;//故障码？
-			rov_message.motor_twine_flag[1] = 0;//(uint16_t)uavcan.thruster[1].error_count;
-			rov_message.motor_twine_flag[2] = 0;//(uint16_t)uavcan.thruster[2].error_count;
-			rov_message.motor_twine_flag[3] = 0;//(uint16_t)uavcan.thruster[3].error_count;
-			rov_message.motor_twine_flag[4] = 0;//(uint16_t)uavcan.thruster[4].error_count;
-			rov_message.motor_twine_flag[5] = 0;//(uint16_t)uavcan.thruster[5].error_count;
+			rov_message.track_fault_flag[0] = 0;
+			rov_message.track_fault_flag[1] = 0;
+			rov_message.motor_twine_flag[0] = 0;
+			rov_message.motor_twine_flag[1] = 0;
+			rov_message.motor_twine_flag[2] = 0;
+			rov_message.motor_twine_flag[3] = 0;
+			rov_message.motor_twine_flag[4] = 0;
+			rov_message.motor_twine_flag[5] = 0;
 			rov_message.motor_twine_flag[6] = 0;
 			rov_message.motor_twine_flag[7] = 0;
-			rov_message.motor_power[0] = (int16_t)sub._telemetry[0].totalcurrent;//float current
-			rov_message.motor_power[1] = (int16_t)sub._telemetry[1].totalcurrent;
-			rov_message.motor_power[2] = (int16_t)sub._telemetry[2].totalcurrent;
-			rov_message.motor_power[3] = (int16_t)sub._telemetry[3].totalcurrent;
-			rov_message.motor_power[4] = (int16_t)sub._telemetry[4].totalcurrent;
-			rov_message.motor_power[5] = (int16_t)sub._telemetry[5].totalcurrent;
-			
+			rov_message.track_power[0] = 0;
+			rov_message.track_power[1] = 0;
+			rov_message.motor_power[0] = 0;
+			rov_message.motor_power[1] = 0;
+			rov_message.motor_power[2] = 0;
+			rov_message.motor_power[3] = 0;
+			rov_message.motor_power[4] = 0;
+			rov_message.motor_power[5] = 0;
+			rov_message.motor_power[6] = 0;
+			rov_message.motor_power[7] = 0;			
 
 
 			get_stm32_param.number = 0;
@@ -1088,32 +1094,7 @@ void GCS_MAVLINK::update_send()
 				
 			}
 			
-
-			/*const NavEKF2 &ekf2 = AP::ahrs_navekf().get_NavEKF2_const();
-								Matrix3f mat;
-								ekf2.getRotationBodyToNED(mat);
-
-			countper++;
-			if (countper>50)
-			{
-				countper = 0;
-				
-				mavlink_msg_rotation_matrix_array_send(chan,
-											   mat.c.x,mat.c.z,mat.c.y,
-											   mat.b.x,mat.b.z,mat.b.y, 
-											   mat.a.x,mat.a.z,mat.a.y);
-				mavlink_msg_rov_state_monitoring_send_struct(chan,&rov_message);
-							
-				mavlink_msg_set_slave_parameter_send_struct(chan,&get_stm32_param); 
-									
-				//mavlink_msg_motor_speed_send_struct(chan,&mav_motor_speed_back);
-				//mavlink_msg_hv_reg_get_send_struct(chan,&hv_reg_get);
-			}*/
-			
-
-			
-
-		  	
+	  	
 		
 
     if (!hal.scheduler->in_delay_callback()) {
@@ -4182,6 +4163,9 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
     bool ret = true;
 
     switch(id) {
+		case MSG_ROV_MOTOR_TEMP:
+		mavlink_msg_rov_motor_temp_send_struct(chan,&rov_motor_temp);
+		break;
 	case MSG_ROV_STATE_MONITORING:
 		mavlink_msg_rov_state_monitoring_send_struct(chan,&rov_message);
 		break;

libraries/GCS_MAVLink/ap_message.h

@@ -58,6 +58,7 @@ enum ap_message : uint8_t {
     MSG_PID_TUNING,
     MSG_VIBRATION,
     MSG_RPM,
+    MSG_ROV_MOTOR_TEMP,
     MSG_HV_REG_GET,
     MSG_ROV_STATE_MONITORING,
     MSG_MOTOR_SPEED,

libraries/RC_Channel/RC_Channel.cpp

@@ -49,7 +49,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = {
     // @Range: 800 2200
     // @Increment: 1
     // @User: Advanced
-    AP_GROUPINFO("MIN",  1, RC_Channel, radio_min, 1000),
+    AP_GROUPINFO("MIN",  1, RC_Channel, radio_min, 1100),
 
     // @Param: TRIM
     // @DisplayName: RC trim PWM
@@ -67,7 +67,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = {
     // @Range: 800 2200
     // @Increment: 1
     // @User: Advanced
-    AP_GROUPINFO("MAX",  3, RC_Channel, radio_max, 2000),
+    AP_GROUPINFO("MAX",  3, RC_Channel, radio_max, 1900),
 
     // @Param: REVERSED
     // @DisplayName: RC reversed

libraries/RC_Channel/RC_Channel.h

@@ -15,6 +15,11 @@ public:
     friend class RC_Channels;
     // Constructor
     RC_Channel(void);
+	
+	int16_t 	radio_in;
+    AP_Int16    radio_min;
+    AP_Int16    radio_trim;
+    AP_Int16    radio_max;
 
     enum ChannelType {
         RC_CHANNEL_TYPE_ANGLE = 0,
@@ -214,14 +219,12 @@ protected:
 private:
 
     // pwm is stored here
-    int16_t     radio_in;
+    
 
     // value generated from PWM normalised to configured scale
     int16_t    control_in;
 
-    AP_Int16    radio_min;
-    AP_Int16    radio_trim;
-    AP_Int16    radio_max;
+
 
     AP_Int8     reversed;
     AP_Int16    dead_zone;

libraries/SRV_Channel/SRV_Channel.cpp

@@ -35,7 +35,7 @@ const AP_Param::GroupInfo SRV_Channel::var_info[] = {
     // @Range: 500 2200
     // @Increment: 1
     // @User: Standard
-    AP_GROUPINFO("MIN",  1, SRV_Channel, servo_min, 1000),
+    AP_GROUPINFO("MIN",  1, SRV_Channel, servo_min, 1100),
 
     // @Param: MAX
     // @DisplayName: Maximum PWM
@@ -44,7 +44,7 @@ const AP_Param::GroupInfo SRV_Channel::var_info[] = {
     // @Range: 800 2200
     // @Increment: 1
     // @User: Standard
-    AP_GROUPINFO("MAX",  2, SRV_Channel, servo_max, 2000),
+    AP_GROUPINFO("MAX",  2, SRV_Channel, servo_max, 1900),
 
     // @Param: TRIM
     // @DisplayName: Trim PWM

modules/mavlink/message_definitions/v1.0/ardupilotmega.xml

@@ -1500,6 +1500,11 @@
       <description>RPM sensor output.</description>
       <field type="float" name="rpm1">RPM Sensor1.</field>
       <field type="float" name="rpm2">RPM Sensor2.</field>
+    </message>
+      <message id="227" name="ROV_MOTOR_TEMP">
+      <description>RPM sensor output.</description>
+      <field type="int16_t[2]" name="track_temperature">motors twine flag.</field>
+      <field type="int16_t[8]" name="motor_temperature">motors twine flag.</field>
     </message>
 	<message id="228" name="HV_REG_GET">
 	  <description>high voltage get reg data.</description>
@@ -1518,8 +1523,10 @@
       <field type="uint16_t" name="forward">motor forward direction.</field>
       <field type="uint16_t" name="turn">Motor turning direction.</field>
       <field type="uint16_t" name="hvMotorMod">HV Motor Mod.</field>
+      <field type="uint16_t[2]" name="track_fault_flag">motors twine flag.</field>
       <field type="uint16_t[8]" name="motor_twine_flag">motors twine flag.</field>
-      <field type="uint16_t[6]" name="motor_power">motors twine flag.</field>
+      <field type="int16_t[2]" name="track_power">motors twine flag.</field>
+      <field type="int16_t[8]" name="motor_power">motors twine flag.</field>
     </message>
 	<message id="235" name="MOTOR_SPEED">
 	  <description>motor speed communication.</description>