更改了深度和姿态 手柄

ArduSub/Sub.h

@@ -868,6 +868,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();

ArduSub/control_sport.cpp

@@ -76,9 +76,18 @@ void Sub::handle_attitude_sport(){
         //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());
-		get_pilot_desired_lean_angles(0, (int16_t)((0.5-channel_throttle->norm_input())*5700), 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()*0.3);//*0.6 变慢
+		target_yaw = get_pilot_desired_yaw_rate(yaw);//*0.6 变慢
 		
         if (abs(target_roll) > 300 || abs(target_pitch) > 300) {
             //last_roll = ahrs.roll_sensor;
@@ -108,6 +117,195 @@ void Sub::handle_attitude_sport(){
 
 }
 
+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
@@ -145,10 +343,15 @@ void Sub::sport_run()
     //handle_attitude();
 
 
-    pos_control.update_z_controller();
+    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, 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
@@ -156,26 +359,51 @@ void Sub::sport_run()
    // 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);
-    motors.set_forward(constrain_float(-throttle_vehicle_frame.x + getgainf(channel_forward->norm_input()),-0.8,0.8));//*0.6 变慢
-    motors.set_lateral(constrain_float(-throttle_vehicle_frame.y + getgainf(channel_lateral->norm_input()),-0.8,0.8));//*0.6 变慢
+   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_inputs = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), 0.0, (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_inputs.z) > 0.05f) { // Throttle input  above 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) {//最大油门不能向下动
@@ -183,47 +411,43 @@ void Sub::sport_run()
             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 当前深度
+			
+		
+				 pos_control.calc_leash_length_z();//刹车长度 12.19
+	             pos_control.set_alt_target(barometer.get_altitude()*100);//cm 当前深度
 
-            holding_depth = true;
+	            holding_depth = true;
+				
+				
+
+			
+            //pos_control.set_target_to_stopping_point_z();//有对位置的付值
+           
 		  
         }
     }
-
-
-	
-}
-/*
-float rmpforward(float _gain){
-	static uint16_t rmpcounter = 0;
-	float get_forward_gain = motors.get_forward();
-	float temp=0;
-	rmpcounter++;
-	if (rmpcounter>20)
-	{
-		rmpcounter =0;
-		if(get_forward_gain - _gain>0.01){
-			temp = get_forward_gain-0.01;
+	static bool lastdepth = holding_depth;
+	if(lastdepth !=holding_depth){
+		lastdepth = holding_depth;
 		
-	 	}else if(get_forward_gain- _gain<-0.01){
-			temp = get_forward_gain+0.01;
-		}
-		else{
-			temp = _gain;
-		}
+	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());
 	}
-	if (_gain>0.3)
-	{
-		temp =constrain_float(temp,0.4,1.0);
-	}else if(temp<-0.3){
-		temp = constrain_float(temp,-1.0,-0.4);
-	}else{
-		temp=0.0;
+
+	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_stabilize.cpp

@@ -67,9 +67,9 @@ void Sub::handle_attitude_stablize(){
 	   }else if(prepare_state == Vertical)
 		{
 		   pitch_input = 8000;
-		   target_roll = ((float)channel_yaw->get_control_in()*0.4);
-		   
-		   target_yaw = 0;
+		   //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;

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

@@ -65,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++) {
@@ -77,6 +78,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
     // 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)) {
@@ -154,7 +156,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.8;
+				
+        	}else{
+			updowmgain=0.7;
+        	}
+			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:
@@ -326,9 +337,7 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
     case JSButton::button_function_t::k_gain_inc:
 		{
         {
-			
-			updowmgain=0.3;//下
-			delaygain =0;
+
 			
         }
         break;
@@ -337,8 +346,7 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
 		{
 		
         {
-			updowmgain=0.7;
-			delaygain =0;
+
 
         }
         break;
@@ -718,17 +726,20 @@ 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.2;//下
+			}
+			else{
+				updowmgain=0.3;//下
+			}
+			delaygain =0;
+    	}
         break;
     case JSButton::button_function_t::k_custom_6:
-        if (!held){
-		  	if(clean_mode == 0){
-				clean_mode =1;	
-			}else{
-				clean_mode =0;
-			}
-		  	
-        } 
+    	{
+    	}
         break;
     }
 }

ArduSub/surface_bottom_detector.cpp

@@ -59,7 +59,7 @@ void Sub::update_surface_and_bottom_detector()
             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 {
@@ -82,7 +82,7 @@ void Sub::update_surface_and_bottom_detector()
             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

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_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.cpp

@@ -482,9 +482,31 @@ void AC_PosControl::update_z_controller()
     calc_leash_length_z();
 
     // call z-axis position controller
-    run_z_controller();
+   run_z_controller_f();
+}
+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()
@@ -555,6 +577,7 @@ 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();//12.19
@@ -566,7 +589,7 @@ void AC_PosControl::run_z_controller()
 
     // 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;
     }
 
@@ -582,6 +605,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;
 
@@ -590,13 +614,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;
 
@@ -612,12 +638,168 @@ 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);
+	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_f()
+{
+    //float curr_alt = _inav.get_altitude();//12.19
+	float curr_alt = sub.barometer.get_altitude()*100;//_inav.get_altitude();//12.19
+	
+	float vel_from_baro = AC_AttitudeControl::sqrt_controller(curr_alt-last_alt, _p_pos_z.kP(), _accel_z_cms, _dt);//12.19
+	static uint16_t i=0;
+	uint16_t timecnt = (uint16_t)(0.25/_dt);
+	i++;
+	if (i>timecnt)
+	{
+		i = 0;
+		last_alt = curr_alt;//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=vel_from_baro;
+
+
+    // 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;
+	static uint8_t freeze_ff_z =1;
+    // 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
+		//freeze_ff_z = 0;//前馈一直有效
+        _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
+		 freeze_ff_z =1;
+    }
+
+   
+
+    _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;
+	float ff_p =(float)SRV_Channels::srv_channel(15)->get_output_min()/1000000;//参数 
+
+	float	thr_out_ff = thr_out;
+	if (freeze_ff_z!=0)
+	{
+		
+		thr_out_ff = thr_out - curr_vel.z*ff_p;//增加了一个前馈项
+	}
+    // 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 %d %f %f %f %f\n",freeze_ff_z,_vel_target.z,thr_out,ff_p,thr_out_ff);
+
+	}
 
+	return thr_out_ff;
 }
 
 ///

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -148,6 +148,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; }
@@ -305,6 +306,7 @@ public:
 	float       _dt;    
 	float       _accel_z_cms;           // max vertical acceleration in cm/s/s
 	float  last_alt;
+	AC_PID		_pid_accel_z;
 
 protected:
 
@@ -339,6 +341,7 @@ protected:
     //          set_target_to_stopping_point_z
     //          init_takeoff
     void run_z_controller();
+	float run_z_controller_f();
 
     ///
     /// xy controller private methods
@@ -383,7 +386,7 @@ protected:
     AP_Float    _lean_angle_max;        // Maximum autopilot commanded angle (in degrees). Set to zero for Angle Max
    
     AC_P        _p_vel_z;
-    AC_PID      _pid_accel_z;
+    
     AC_P        _p_pos_xy;
     AC_PID_2D   _pid_vel_xy;
 

libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp

@@ -107,4 +107,5 @@ void AC_PosControl_Sub::relax_alt_hold_controllers()
     _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
 }

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:

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

libraries/AP_Motors/AP_Motors_Class.h

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