使用了飞盈佳乐电调

增加了sport模式中的定身模式
使用四元数的方式保持姿态

ArduSub/ArduSub.cpp

@@ -119,17 +119,6 @@ void Sub::fast_loop()
     // --------------------
     read_AHRS();
 	
-	  static int j = 0;
-	j++;
-	if(j>500)
-	{
-		
-			float depth_now =barometer.get_altitude();
-			 gcs().send_text(MAV_SEVERITY_INFO, "  %f  %f\n",(float)depth_now,inertial_nav.get_altitude());
-	  
-	 
-		j=0;
-	}
 
 
     // Inertial Nav

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

@@ -132,7 +132,7 @@ 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{
 	

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();
 	
@@ -58,7 +58,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);
 	
@@ -106,25 +106,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 +223,8 @@ void Sub::clean_net_joystick(void)
 			}
 		}
 
-		motors1=constrain_int16(motors1,-((int16_t)(maxspeed)/2),maxspeed);
-		motors2=constrain_int16(motors2,-((int16_t)(maxspeed)/2),maxspeed);
+		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

@@ -40,11 +40,16 @@ void Sub::manual_run()
 	  j++;
 	  if(j>800)
 	  {
-	    //	gcs().send_text(MAV_SEVERITY_INFO, " PressLevel_f %f \n",(float)1.0-(float)PressLevel_f*0.1);
+		//  gcs().send_text(MAV_SEVERITY_INFO, " throttle %f\n",channel_throttle->norm_input());
+		 // gcs().send_text(MAV_SEVERITY_INFO, " yaw %f\n",channel_yaw->norm_input());
+		 // gcs().send_text(MAV_SEVERITY_INFO, " forward %f\n",channel_forward->norm_input());
+		 // gcs().send_text(MAV_SEVERITY_INFO, " lateral %f\n",channel_lateral->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());
+		 // 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

@@ -1,25 +1,46 @@
 #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;
+    if(!control_check_barometer()) {
+        return false;
     }
+
+    // initialize vertical speeds and leash lengths
+    // sets the maximum speed up and down returned by position controller
+    pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
+    pos_control.set_max_accel_z(g.pilot_accel_z);
+    pos_control.relax_alt_hold_controllers();
+	//pos_control.set_alt_target(-20);//cm
+    pos_control.set_target_to_stopping_point_z();
+    holding_depth = true;
+	ahrs.get_quat_body_to_ned(attitude_desired_quat_);
+
+    
+    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();
+	last_input_ms_stable = AP_HAL::millis();
+	
+	gcs().send_text(MAV_SEVERITY_INFO, " last_attitudeinit %d %d,%d ",(int)last_roll_s,(int)pitch_input_inc,(int)last_yaw_s);
+	gcs().send_text(MAV_SEVERITY_INFO, " init Z target %f %f",(float)pos_control.get_pos_target().z,barometer.get_altitude());
+
     return true;
 }
+
 void Sub::handle_attitude_sport(){
 	uint32_t tnow = AP_HAL::millis();
 
@@ -51,15 +72,17 @@ void Sub::handle_attitude_sport(){
 
 		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_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
@@ -67,57 +90,106 @@ void Sub::handle_attitude_sport(){
             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_);
         }
+		static int f = 0;
+		f++;
+		if(f>800)
+		{
+		
+		   gcs().send_text(MAV_SEVERITY_INFO, " sportrpy %f %f,%f ",(float)target_roll,(float)target_pitch,(float)target_yaw);
+
+			gcs().send_text(MAV_SEVERITY_INFO, " sportatd %f %f,%f ",(float)last_roll,(float)last_pitch,(float)last_yaw);
+			f=0;
+		}
     }
 
 
 }
 
-// stabilize_run - runs the main stabilize controller
+
+// althold_run - runs the althold controller
 // should be called at 100hz or more
 void Sub::sport_run()
 {
-    // if not armed set throttle to zero and exit immediately
+    // When unarmed, disable motors and stabilization
     if (!motors.armed()) {
         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
-        attitude_control.set_throttle_out(0,true,g.throttle_filt);
+        // 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(-20);//cm
         last_roll = 0;
         last_pitch = 0;
         last_yaw = ahrs.yaw_sensor;
-		yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
+        holding_depth = false;
+		ahrs.get_quat_body_to_ned(attitude_desired_quat_);
+		
+		static int f = 0;
+		f++;
+		if(f>800)
+		{
+		
+		   gcs().send_text(MAV_SEVERITY_INFO, " disarm Z target %f %f %f",(float)pos_control.get_pos_target().z,pos_control.get_alt(),barometer.get_altitude());
+			f=0;
+		}
         return;
     }
+
+    // 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_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);//压力分级
+    pos_control.update_z_controller();
+	
+    // 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());
+    // 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);
 	rov_message.pressure_level = int(PressLevel);
+    motors.set_forward(-throttle_vehicle_frame.x + channel_forward->norm_input());
+    motors.set_lateral(-throttle_vehicle_frame.y + channel_lateral->norm_input());
 
-    //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)));
+
+    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();
+		//pos_control.set_alt_target(barometer.get_altitude()*100);//cm
+    } 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();
+            holding_depth = true;
+        }
+    }
+			static int f = 0;
+		f++;
+		if(f>800)
+		{
 		
-		   j=0;
-	  }
-}
+		   gcs().send_text(MAV_SEVERITY_INFO, " Z target %f %f %f",(float)pos_control.get_pos_target().z,pos_control.get_alt(),barometer.get_altitude());
 
+			gcs().send_text(MAV_SEVERITY_INFO, " surface %d at_bottom %d ",ap.at_surface,ap.at_bottom);
+			f=0;
+		}
+	
+}

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

@@ -5,19 +5,16 @@ 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;
-    }
+    
+    last_roll_s = 0;
+    last_pitch_s = 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);//记住方位
+	//yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
 	prepare_state = Horizontal;
     return true;
 }
@@ -68,7 +65,7 @@ void Sub::handle_attitude_stablize(){
 	   }else if(prepare_state == Vertical)
 		{
 		   pitch_input = 8000;
-		   target_roll = ((float)channel_yaw->get_control_in()*0.3);
+		   target_roll = ((float)channel_yaw->get_control_in()*0.5);
 		   
 		   target_yaw = 0;
 	   }
@@ -128,7 +125,7 @@ void Sub::stabilize_run()
 
 		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;

ArduSub/joystick.cpp

@@ -328,15 +328,15 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
 				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);
+						//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);
+						//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);
 					}
 	
 			}
@@ -355,15 +355,15 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
 				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);
+					//	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);
+					//	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);
 					}
 
 			}
@@ -657,42 +657,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 +701,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;

libraries/AC_AttitudeControl/AC_PosControl_Sub.h

@@ -40,6 +40,8 @@ public:
     ///     integrator is not reset
     void relax_alt_hold_controllers();
 
+	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
     float       _alt_min; // min altitude - should be updated from the main code with altitude limit from fence

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,

libraries/AP_Motors/AP_Motors6DOF.cpp

@@ -227,37 +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)
-{ 
-	  int32_t thrust32b = 0;
-	 
-	 
-	  //int16_t stephv = 500;
-	  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);
-
-
-}
 
 
 
 extern mavlink_motor_speed_t mav_motor_speed;
 
-void AP_Motors6DOF::output_to_Dshot()
+#define NETRULPWM 1490
+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:
@@ -265,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;
@@ -273,72 +252,55 @@ 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+NETRULPWM);//230 才启动
+		 motor_out[1] =(int16_t)(mav_motor_speed.motor2*500+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)
-		{
-			//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;
-		}
+    // send output to each motor
+    for (i=0; i<6; i++) {
+        if (motor_enabled[i]) {
+            rc_write(i, motor_out[i]);
+        }
+    }
+	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(){
 	

libraries/AP_Motors/AP_Motors6DOF.h

@@ -61,7 +61,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

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