深度控制去掉了EKF中读取的数

ArduSub/control_sport.cpp

@@ -12,19 +12,23 @@ Quaternion attitude_desired_quat_;
 // althold_init - initialise althold controller
 bool Sub::sport_init()
 {
+	//检查是否有深度计存在
     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();
+    //get_pilot_speed_dn QGC PILOT 设置的值
+    pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);//(-500,500)设置最大最小速度 和刹车长度
+    pos_control.set_max_accel_z(g.pilot_accel_z);//设置最大加速度值 和刹车长度
+    pos_control.relax_alt_hold_controllers();//目标高度为当前高度，目标速度和上次速度 当前速度
+	//
+    //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);//当前深度12.19
     holding_depth = true;
-	ahrs.get_quat_body_to_ned(attitude_desired_quat_);
+	ahrs.get_quat_body_to_ned(attitude_desired_quat_);//12.19
 
     
     last_roll = 0;
@@ -113,19 +117,28 @@ void Sub::handle_attitude_sport(){
 // should be called at 100hz or more
 void Sub::sport_run()
 {
+	static int f2 = 0;
+	static int f3 = 0;
+	
+
     // When unarmed, disable motors and stabilization
     if (!motors.armed()) {
+			f2 = 0;
+	        f3 = 0;
+	       
         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(-20);//cm
+		
+		//pos_control.set_alt_target(barometer.get_altitude()*100);//当前深度
+		
         last_roll = 0;
         last_pitch = 0;
         last_yaw = ahrs.yaw_sensor;
         holding_depth = false;
-		ahrs.get_quat_body_to_ned(attitude_desired_quat_);
+		ahrs.get_quat_body_to_ned(attitude_desired_quat_);//12.19
 		
 		static int f = 0;
 		f++;
@@ -167,29 +180,67 @@ void Sub::sport_run()
        // 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
+        //pos_control.set_alt_target(barometer.get_altitude()*100);//当前深度
+		
+		static int f = 0;
+		f2 = 0;
+	    f3 = 0;
+		f++;
+		if(f>400)
+		{
+		
+		    gcs().send_text(MAV_SEVERITY_INFO, " Z1 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;
+		}
     } else { // hold z
         if (ap.at_surface) {	//最大油门不能向上动
-            pos_control.set_alt_target(g.surface_depth - 10.0f); // set target to 5cm below surface level
+            pos_control.set_alt_target(g.surface_depth - 20.0f); // set target to 5cm below surface level
             holding_depth = true;
+			
+			f2++;
+			if(f2==1)
+			{
+		
+		  		gcs().send_text(MAV_SEVERITY_INFO, " Zsurface 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);
+				f2=2;
+			}
         } 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
+            //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;
+			
+			f3++;
+			if(f3==1)
+			{
+		
+		  		gcs().send_text(MAV_SEVERITY_INFO, " Zbottom 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);
+				f3=2;
+			}
         } else if (!holding_depth) {
-            pos_control.set_target_to_stopping_point_z();
+            //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 当前深度
+            f2 = 0;
+	    	f3 = 0;
             holding_depth = true;
+		  	gcs().send_text(MAV_SEVERITY_INFO, " Z4 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);
+			
         }
     }
-			static int f = 0;
-		f++;
-		if(f>800)
-		{
+			static int f5 = 0;
+			f5++;
+			if(f5>500)
+			{
 		
-		   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, " Z00 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);
+				f5=0;
+			}
 
-			gcs().send_text(MAV_SEVERITY_INFO, " surface %d at_bottom %d ",ap.at_surface,ap.at_bottom);
-			f=0;
-		}
 	
 }

ArduSub/control_surface.cpp

@@ -14,6 +14,7 @@ bool Sub::surface_init()
     // initialise position and desired velocity
     pos_control.set_alt_target(inertial_nav.get_altitude());
     pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+	gcs().send_text(MAV_SEVERITY_INFO, " surfacemode target %f %f %f",(float)pos_control.get_pos_target().z,pos_control.get_alt(),barometer.get_altitude());
 
     return true;
 

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -2,6 +2,7 @@
 #include "AC_PosControl.h"
 #include <AP_Math/AP_Math.h>
 #include <AP_Logger/AP_Logger.h>
+#include "../../ArduSub/Sub.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -247,12 +248,12 @@ void AC_PosControl::set_max_speed_z(float speed_down, float speed_up)
 {
     // ensure speed_down is always negative
     speed_down = -fabsf(speed_down);
-
+//默认_speed_down_cms -150  ,,_speed_up_cms 250
     if ((fabsf(_speed_down_cms - speed_down) > 1.0f) || (fabsf(_speed_up_cms - speed_up) > 1.0f)) {
         _speed_down_cms = speed_down;
         _speed_up_cms = speed_up;
         _flags.recalc_leash_z = true;
-        calc_leash_length_z();
+        calc_leash_length_z();// 刹车长度
     }
 }
 
@@ -475,7 +476,7 @@ void AC_PosControl::update_z_controller()
     _last_update_z_us = now_us;
 
     // check for ekf altitude reset
-    check_for_ekf_z_reset();
+    //check_for_ekf_z_reset();
 
     // check if leash lengths need to be recalculated
     calc_leash_length_z();
@@ -501,8 +502,11 @@ void AC_PosControl::calc_leash_length_z()
 // vel_up_max, vel_down_max should have already been set before calling this method
 void AC_PosControl::run_z_controller()
 {
-    float curr_alt = _inav.get_altitude();
-
+    //float curr_alt = _inav.get_altitude();//12.19
+	float curr_alt = sub.barometer.get_altitude()*100;//_inav.get_altitude();//12.19
+	static float last_alt = 0.0;//12.19
+	float vel_from_baro = AC_AttitudeControl::sqrt_controller(curr_alt-last_alt, _p_pos_z.kP(), _accel_z_cms, _dt);//12.19
+	last_alt = curr_alt;//12.19
     // clear position limit flags
     _limit.pos_up = false;
     _limit.pos_down = false;
@@ -545,7 +549,11 @@ void AC_PosControl::run_z_controller()
 
     // the following section calculates acceleration required to achieve the velocity target
 
-    const Vector3f& curr_vel = _inav.get_velocity();
+    //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
@@ -1105,12 +1113,12 @@ float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float k
         return POSCONTROL_LEASH_LENGTH_MIN;
     }
 
-    // calculate leash length
+    // calculate leash length  1/kP 为时间间隔dt
     if (speed_cms <= accel_cms / kP) {
         // linear leash length based on speed close in
-        leash_length = speed_cms / kP;
+        leash_length = speed_cms / kP; //时间间隔很小的时候 V*dt
     } else {
-        // leash length grows at sqrt of speed further out
+        // leash length grows at sqrt of speed further out //时间间隔比较大的时候  1/2*a*dt^2+ v^2/2/a 这是根据开方器来的 看不懂
         leash_length = (accel_cms / (2.0f * kP * kP)) + (speed_cms * speed_cms / (2.0f * accel_cms));
     }
 

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -13,6 +13,8 @@
 #include <AP_Vehicle/AP_Vehicle.h>         // common vehicle parameters
 
 
+
+
 // position controller default definitions
 #define POSCONTROL_ACCELERATION_MIN             50.0f   // minimum horizontal acceleration in cm/s/s - used for sanity checking acceleration in leash length calculation
 #define POSCONTROL_ACCEL_XY                     100.0f  // default horizontal acceleration in cm/s/s.  This is overwritten by waypoint and loiter controllers

libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp

@@ -1,4 +1,5 @@
 #include "AC_PosControl_Sub.h"
+#include "../../ArduSub/Sub.h"
 
 AC_PosControl_Sub::AC_PosControl_Sub(const AP_AHRS_View& ahrs, const AP_InertialNav& inav,
                                      const AP_Motors& motors, AC_AttitudeControl& attitude_control) :
@@ -92,11 +93,14 @@ void AC_PosControl_Sub::set_alt_target_from_climb_rate_ff(float climb_rate_cms,
 /// relax_alt_hold_controllers - set all desired and targets to measured
 void AC_PosControl_Sub::relax_alt_hold_controllers()
 {
-    _pos_target.z = _inav.get_altitude();
+    //_pos_target.z = _inav.get_altitude(); // 12.19
+	_pos_target.z = sub.barometer.get_altitude()*100;// 12.19
     _vel_desired.z = 0.0f;
     _flags.use_desvel_ff_z = false;
-    _vel_target.z = _inav.get_velocity_z();
-    _vel_last.z = _inav.get_velocity_z();
+	_vel_target.z = 0.0f;// 12.19
+	_vel_last.z = 0.0f;// 12.19
+    //_vel_target.z = _inav.get_velocity_z();// 12.19
+    //_vel_last.z = _inav.get_velocity_z();// 12.19
     _accel_desired.z = 0.0f;
     _accel_last_z_cms = 0.0f;
     _flags.reset_rate_to_accel_z = true;