stable和sport的前进后退和横移的油门不同；

增加了DVL的读取；
深度PID可以设置；

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),
@@ -194,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 +345,8 @@ void Sub::update_altitude()
 {
     // read in baro altitude
     read_barometer();
+
+	
 	pos_control.calculate_rate(0.1);
 
     if (should_log(MASK_LOG_CTUN)) {

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
@@ -898,6 +905,7 @@ public:
 	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);
 
@@ -910,6 +918,52 @@ public:
 	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/UserCode.cpp

@@ -63,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
 
@@ -167,6 +168,34 @@ float Sub::getgainf(float data){
 	}
 	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;
 }
@@ -212,7 +241,7 @@ float Sub::gaindelay(float gain1){
 	 if (count>400)
 		 {
 		 count = 0;
-		 gcs().send_text(MAV_SEVERITY_INFO, "conti %d %d %d \n",(int)continuous_count,(int)last_continuous,(int)continuous_countdec);
+		 gcs().send_text(MAV_SEVERITY_INFO, "conti %d %d %d %f\n",(int)continuous_count,(int)last_continuous,(int)continuous_countdec,result);
 
 	 }
 
@@ -428,6 +457,285 @@ float Sub::get_pitch_to_ned(Matrix3f mat_body){
 	}
 	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_sport .cpp

@@ -343,8 +343,15 @@ void Sub::altcontrol(){//相对机体系的手柄控制
 		   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 变慢
+		 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();

ArduSub/control_stabilize.cpp

@@ -66,7 +66,7 @@ void Sub::handle_attitude_stablize(){
 		   pitch_input = 0;
 		   target_roll = 0;
 		   last_roll_s = 0;
-		   target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()*0.4);
+		   target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()*1.0);
 
 		   
 	   }else if(prepare_state == Vertical)
@@ -160,8 +160,8 @@ void Sub::stabilize_run()
 		  }else{
 			_forward = 0.0;
 		  }
-    	motors.set_forward(constrain_float(getgainf(_forward),-0.8,0.8));
-    	motors.set_lateral(constrain_float(getgainf(_lateral),-0.8,0.8));
+    	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;

ArduSub/version.h

@@ -9,9 +9,9 @@
 #define THISFIRMWARE "ArduSub V4.0.3"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,0,3,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 3
+#define FW_PATCH 4
 #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -715,7 +715,7 @@ float AC_PosControl::run_z_controller_f()
     }
 	float scaler = 0.0;
 	
-	scaler = 6.0;//加速度的KI
+	/*scaler = 10.0;//加速度的KI
 	float kp = 0.0;
 
 	if (fabsf(_pos_error.z)>scaler)
@@ -723,7 +723,7 @@ float AC_PosControl::run_z_controller_f()
 			kp = 2.0;//位置的KP
 			_p_pos_z.kP(kp);
 	
-			kp = 10.0;//速度的Kp
+			kp = 8.0;//速度的Kp
 			_p_vel_z.kP(kp);
 			
 			kp = 1.3;//加速度的KP
@@ -744,43 +744,43 @@ float AC_PosControl::run_z_controller_f()
 	
 			kp = 0.8;//加速度的KI
 	        _pid_accel_z.kI(kp);
-		}
-	/*scaler = (float)SRV_Channels::srv_channel(15)->get_trim()/1000;//加速度的KI
+		}*/
+	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;//位置的KP
-			_p_pos_z.kP(kp);
+			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;//速度的Kp
-			_p_vel_z.kP(kp);
+			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;//加速度的KP
-			_pid_accel_z.kP(kp);
+			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;//加速度的KI
-			_pid_accel_z.kI(kp);
+			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;//位置的KP
-			_p_pos_z.kP(kp);
+			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;//速度的KP
-			_p_vel_z.kP(kp);
+			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;//加速度的KP
-			_pid_accel_z.kP(kp);
+			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;//加速度的KI
-	        _pid_accel_z.kI(kp);
-		}*/
+			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\n",(float)_p_pos_z.kP(), (float)_p_vel_z.kP(),(float)_pid_accel_z.kP(),(float)_pid_accel_z.kI());
+		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

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[];