修改了履带的数据类型，高压推进器的最高速度1850，修改了最大加速度110000

ArduSub/control_clean.cpp

@@ -2,8 +2,8 @@
 //目标姿态
 
 float maxspeed = 51.0;
-int16_t minspeed = 31;
-int16_t maxerrorspeed = 31;
+float minspeed = 31.0;
+float maxerrorspeed = 31.0;
 
 //float turnspeed  = maxspeed - maxerror_f;
 
@@ -93,9 +93,9 @@ void Sub::clean_run()
 		motors.set_lateral(0.0);//右侧摇杆 左推-，右推+
 		motors.set_yaw(0.0);//左侧摇杆左推- 右推+
 		motors.set_roll(0.0);
-   		motors.set_pitch(0.0);
+   		//motors.set_pitch(0.0);
    	   //motors.set_roll(channel_lateral->norm_input());//(channel_roll->norm_input());//左右移动改为roll
-       //motors.set_pitch((0.5-channel_throttle->norm_input())*2);//(channel_pitch->norm_input());//上升下潜改为pitch
+       motors.set_pitch((0.5-channel_throttle->norm_input())*2);//(channel_pitch->norm_input());//上升下潜改为pitch
     	motors.set_yaw(0.0);
 	   static int j = 0;
 		 j++;
@@ -143,7 +143,7 @@ void Sub::autoclean_flag_chose(void){
 	}
 
 }
-int16_t maxspeed_up=50;
+float maxspeed_set=50.0;
 
 void Sub::clean_net_joystick(void)
 {
@@ -154,27 +154,18 @@ void Sub::clean_net_joystick(void)
 	int16_t motors1 =startval;
 	int16_t motors2 =startval;
 	
-	minspeed =SRV_Channels::srv_channel(15)->get_output_max()/100;// 暂时用来做最小速度   -----最小水深
-	maxerrorspeed =SRV_Channels::srv_channel(15)->get_output_min()/100;//两个履带最大差速 ----------最深距离 厘米 
-	maxspeed_up = SRV_Channels::srv_channel(15)->get_trim()/100;
-	if(maxspeed_up<maxspeed)
+	minspeed =(float)SRV_Channels::srv_channel(15)->get_output_max()/100;// 暂时用来做最小速度   -----最小水深
+	maxerrorspeed =(float)SRV_Channels::srv_channel(15)->get_output_min()/100;//两个履带最大差速 ----------最深距离 厘米 
+	maxspeed_set = (float)SRV_Channels::srv_channel(15)->get_trim()/100;
+	if(maxspeed_set<maxspeed)
 	{
-		maxspeed = maxspeed_up;
+		maxspeed = maxspeed_set;
 	}
 	
 	if(handclean == TRUE ){
 		
 		//计算电机的 手动转速
-		
-		/*left  = Constrate1(channel_forward->norm_input());//   新仓 - //履带右摇杆 -1.0~1.0
-		_turn = Constrate1(channel_yaw->norm_input());//新仓 - //转向 右+左-，+右转 左履带增，-左转 右lvdai+
-		//motors1 = (int16_t)((left*Speedmax_hand_f -_turn*maxerror_f));//右履带
-		//motors2 = (int16_t)((left*Speedmax_hand_f +_turn*maxerror_f));//左履带
 
-		
-		//motors1 = (int16_t)((left*maxspeed -_turn*maxerror_f));//右履带        注意上位机传过来的是左1 右2 这里变成了右1左2 这需要今后改
-		//motors2 = (int16_t)((left*maxspeed +_turn*maxerror_f));//左履带*/
-		
 		left  = Constrate1(channel_forward->norm_input());//   
 		_turn = Constrate1(channel_yaw->norm_input());//转向 右+左-，+右转 左履带增，-左转 右lvdai+
 
@@ -193,30 +184,31 @@ void Sub::clean_net_joystick(void)
 			rov_message.turn    = 1;
 			
 			}
+		int16_t minspeed_int = (int16_t)minspeed;
 		if (left>0.1)
 		{
 			//前进
 
-			if (motors1<minspeed)
+			if (motors1<minspeed_int)
 			{
-				motors1 =minspeed;
+				motors1 =minspeed_int;
 			}
-			if (motors2<minspeed)
+			if (motors2<minspeed_int)
 			{
-				motors2 =minspeed;
+				motors2 =minspeed_int;
 			}
 		}
 		else if (left<-0.1)
 		{
 			//后退
 			
-			if (motors1>-minspeed)
+			if (motors1>-minspeed_int)
 			{
-				motors1 =-minspeed;
+				motors1 =-minspeed_int;
 			}
-			if (motors2>-minspeed)
+			if (motors2>-minspeed_int)
 			{
-				motors2 =-minspeed;
+				motors2 =-minspeed_int;
 			}
 		}
 		else{
@@ -226,18 +218,15 @@ void Sub::clean_net_joystick(void)
 				motors2 =0;
 			}
 			else{
-				//motors1 = -_turn*maxerror_f;//
-				//motors2 = _turn*maxerror_f;//
-				motors1 = _turn*maxerrorspeed;////第一台高压电机 
-				motors2 = -_turn*maxerrorspeed;//
+
+				motors1 = (int16_t)(_turn*maxerrorspeed);////第一台高压电机 
+				motors2 = (int16_t)(-_turn*maxerrorspeed);//
 			}
 		}
 
-		//motors1=constrain_int16(motors1,-((int16_t)(maxspeed)/2),(int16_t)((int16_t)maxspeed+maxerror));
-		//motors2=constrain_int16(motors2,-((int16_t)(maxspeed)/2),(int16_t)((int16_t)maxspeed+maxerror));//做履带
-		int16_t minspeedlimit =(int16_t)(maxspeed*0.6);
-		motors1=constrain_int16(motors1,-(int16_t)minspeedlimit,maxspeed);//100);//maxspeed+maxerror
-		motors2=constrain_int16(motors2,-(int16_t)minspeedlimit,maxspeed);//100);//maxspeed+maxerror
+		int16_t minspeedlimit_back =(int16_t)(maxspeed*0.6);
+		motors1=constrain_int16(motors1,-(int16_t)minspeedlimit_back,(int16_t)maxspeed);
+		motors2=constrain_int16(motors2,-(int16_t)minspeedlimit_back,(int16_t)maxspeed);
 		
 		static int f = 0;
 		f++;
@@ -286,14 +275,7 @@ void Sub::clean_net_joystick(void)
 				slowly_speed2(motor2_speed_target,motors2,1,3);
 
 
-				static int kn1 = 0;
-				kn1++;
-			 	 if(kn1>800)
-			 	 {
-			  		//gcs().send_text(MAV_SEVERITY_WARNING, "left %f, turn %f  %f %f\n",left,_turn,left*Speedmax_hand,_turn*maxerror);
-				   //gcs().send_text(MAV_SEVERITY_WARNING, " speed_target %f %d %d ",left,motor2_speed_target,motor1_speed_target);
-				   kn1=0;
-			 	 }	
+	
 			}else {		
 					
 			}

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -18,8 +18,8 @@
 #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_Y_MAX_DEFAULT_CDSS    22000//27000.0f  // default maximum acceleration for yaw 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    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
 #define AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX          1.0f    // body-frame rate controller maximum output (for roll-pitch axis)

libraries/AP_Motors/AP_Motors6DOF.cpp

@@ -229,7 +229,7 @@ void AP_Motors6DOF::output_min()
 
 #define ThrustScale 1000
 #define hv_min 251
-#define hv_max 1950
+#define hv_max 1850
 int16_t AP_Motors6DOF::calc_thrust_to_HV(float thrust_in,int8_t i)
 { 
 	  int16_t thrust = 0;