更改了motor的油门的数据类型int32_t,原来的int16_t溢出，

mannual和stablize增加了横移功能
版本号4.0.3
RC 和serv的范围1100-1900
roll和yaw的加速度110000

ArduSub/control_clean.cpp

@@ -221,8 +221,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)/2),maxspeed);
+		motors2=constrain_int16(motors2,-((int16_t)(maxspeed)/2),maxspeed);
 	
 		
 		static int f = 0;

ArduSub/control_manual.cpp

@@ -29,23 +29,23 @@ void Sub::manual_run()
 
     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_lateral(channel_lateral->norm_input());
 	motors.set_throttle(1.0-(float)PressLevel_f*0.1);//0-0.5下压
 	static int j = 0;
 	  j++;
 	  if(j>800)
 	  {
-	    	//gcs().send_text(MAV_SEVERITY_INFO, " PressLevel_f %f \n",(float)PressLevel_f);
+	    //	gcs().send_text(MAV_SEVERITY_INFO, " PressLevel_f %f \n",(float)1.0-(float)PressLevel_f*0.1);
 		
-		  //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, " 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, " roll pitch yaw %f %f %f \n",channel_roll->norm_input(),channel_pitch->norm_input(),channel_yaw->norm_input());
+		  j=0;
 	  }
 	rov_message.pressure_level = (int)PressLevel;
 }

ArduSub/control_stabilize.cpp

@@ -50,7 +50,7 @@ void Sub::stabilize_run()
     handle_attitude();
 
     motors.set_forward(channel_forward->norm_input());
-    motors.set_lateral(0.0);
+    motors.set_lateral(channel_lateral->norm_input());
 
 	attitude_control.set_throttle_out(1.0-(float)PressLevel_f*0.1, false, g.throttle_filt);//压力分级
 	rov_message.pressure_level = int(PressLevel);

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,3,FIRMWARE_VERSION_TYPE_OFFICIAL
 
 #define FW_MAJOR 4
 #define FW_MINOR 0
-#define FW_PATCH 7
+#define FW_PATCH 3
 #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL

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

libraries/AP_Motors/AP_Motors6DOF.cpp

@@ -234,42 +234,17 @@ void AP_Motors6DOF::output_min()
 #define MAXDUTY 88000
 int32_t AP_Motors6DOF::calc_thrust_to_motor(float thrust_in,int8_t i)
 { 
-	  int16_t thrust = 0;
+	  int32_t thrust32b = 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];
+	  thrust32b = (int32_t)(thrust_in* ThrustScale);
+	 
+	 int32_t speedref = thrust32b;//last_thrust_Dhot[i];
+	  if (speedref>-10000 && speedref<10000)
+		  {
+		  speedref = 0;
+		  }
 		return constrain_int32(speedref,-MAXDUTY, MAXDUTY);
 
 
@@ -322,8 +297,8 @@ void AP_Motors6DOF::output_to_Dshot()
 		//将上位机转发过来的，转换成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[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;
@@ -354,8 +329,9 @@ void AP_Motors6DOF::output_to_Dshot()
 		k++;
 		if(k>400)
 		{
-
-			//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]);
+			//gcs().send_text(MAV_SEVERITY_INFO, "motor_out1 %f %f %f \n", (float)motor_to_can[0],(float)motor_to_can[1],(float)motor_to_can[2]);
+			
+			//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;
 		}
 }
@@ -365,24 +341,26 @@ 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);
 }
 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;
 	}
 

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/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