去掉EKF的深度控制

ArduSub/control_sport.cpp

@@ -12,6 +12,9 @@ Quaternion attitude_desired_quat_;
 // althold_init - initialise althold controller
 bool Sub::sport_init()
 {
+
+	attitude_control.set_throttle_out(0.5 ,true, g.throttle_filt);
+	//holding_depth = false;
 	//检查是否有深度计存在
     if(!control_check_barometer()) {
         return false;
@@ -39,8 +42,8 @@ bool Sub::sport_init()
     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());
+	
+	gcs().send_text(MAV_SEVERITY_INFO, " init Z target %f %f",(float)pos_control.get_pos_target().z,barometer.get_altitude()*100);
 
     return true;
 }
@@ -97,16 +100,7 @@ void Sub::handle_attitude_sport(){
             //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;
-		}
     }
 
 
@@ -117,14 +111,14 @@ void Sub::handle_attitude_sport(){
 // should be called at 100hz or more
 void Sub::sport_run()
 {
-	static int f2 = 0;
-	static int f3 = 0;
+	static int f2 = 201;
+	static int f3 = 201;
 	
 
     // When unarmed, disable motors and stabilization
     if (!motors.armed()) {
-			f2 = 0;
-	        f3 = 0;
+			f2 = 201;
+	        f3 = 201;
 	       
         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)
@@ -145,7 +139,7 @@ void Sub::sport_run()
 		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());
+		   gcs().send_text(MAV_SEVERITY_INFO, " disarm Z target %f %f",(float)pos_control.get_pos_target().z,barometer.get_altitude()*100);
 			f=0;
 		}
         return;
@@ -183,28 +177,28 @@ void Sub::sport_run()
         //pos_control.set_alt_target(barometer.get_altitude()*100);//当前深度
 		
 		static int f = 0;
-		f2 = 0;
-	    f3 = 0;
+		f2 = 201;
+	    f3 = 201;
 		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, " Z1 target %f %f ",(float)pos_control.get_pos_target().z,barometer.get_altitude()*100);
 			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 - 20.0f); // set target to 5cm below surface level
+            pos_control.set_alt_target(g.surface_depth - 10.0f); // set target to 5cm below surface level
             holding_depth = true;
 			
 			f2++;
-			if(f2==1)
+			if(f2>200)
 			{
 		
-		  		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, " Zsurface target %f %f ",(float)pos_control.get_pos_target().z,barometer.get_altitude()*100);
 				gcs().send_text(MAV_SEVERITY_INFO, " surface %d at_bottom %d ",ap.at_surface,ap.at_bottom);
-				f2=2;
+				f2=0;
 			}
         } else if (ap.at_bottom) {//最大油门不能向下动
             //pos_control.set_alt_target(inertial_nav.get_altitude() + 20.0f); // set target to 10 cm above bottom
@@ -212,21 +206,21 @@ void Sub::sport_run()
             holding_depth = true;
 			
 			f3++;
-			if(f3==1)
+			if(f3>200)
 			{
 		
-		  		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, " Zbottom target %f %f ",(float)pos_control.get_pos_target().z,barometer.get_altitude()*100);
 				gcs().send_text(MAV_SEVERITY_INFO, " surface %d at_bottom %d ",ap.at_surface,ap.at_bottom);
-				f3=2;
+				f3=0;
 			}
         } 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 当前深度
-            f2 = 0;
-	    	f3 = 0;
+            f2 = 201;
+	    	f3 = 201;
             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, " Z4 target %f %f ",(float)pos_control.get_pos_target().z,barometer.get_altitude()*100);
 			gcs().send_text(MAV_SEVERITY_INFO, " surface %d at_bottom %d ",ap.at_surface,ap.at_bottom);
 			
         }
@@ -236,7 +230,7 @@ void Sub::sport_run()
 			if(f5>500)
 			{
 		
-		  		 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, " Z00 target %f %f ",(float)pos_control.get_pos_target().z,barometer.get_altitude()*100);
 
 				gcs().send_text(MAV_SEVERITY_INFO, " surface %d at_bottom %d ",ap.at_surface,ap.at_bottom);
 				f5=0;

ArduSub/surface_bottom_detector.cpp

@@ -12,14 +12,42 @@ static float current_depth = 0;
 // ToDo: doesn't need to be called this fast
 void Sub::update_surface_and_bottom_detector()
 {
+	static float last_alt = 0.0;//12.20
     if (!motors.armed()) { // only update when armed
         set_surfaced(false);
         set_bottomed(false);
+		last_alt =  barometer.get_altitude();
         return;
     }
 
     Vector3f velocity;
-    ahrs.get_velocity_NED(velocity);
+    //ahrs.get_velocity_NED(velocity);//12.20
+    float cur_alt = barometer.get_altitude(); // m
+    
+	float vel = AC_AttitudeControl::sqrt_controller(cur_alt-last_alt, pos_control._p_pos_z.kP(), pos_control._accel_z_cms/100, pos_control._dt);
+	velocity.z = vel;//12.20
+	static int f = 0;
+
+	f++;
+	if(f>600)
+	{
+	
+		
+		gcs().send_text(MAV_SEVERITY_INFO, " velocity.z %f %f %f ",cur_alt,last_alt,velocity.z);
+		f=0;
+	}
+
+	static uint16_t i=0;
+	uint16_t timecnt = (uint16_t)(0.25/MAIN_LOOP_SECONDS);
+	i++;
+	if (i>timecnt)
+	{
+		i = 0;
+		last_alt = cur_alt;//12.19
+	}
+
+	
+
 
     // check that we are not moving up or down
     bool vel_stationary = velocity.z > -0.05 && velocity.z < 0.05;

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -504,9 +504,17 @@ void AC_PosControl::run_z_controller()
 {
     //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
+	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;
@@ -555,6 +563,7 @@ void AC_PosControl::run_z_controller()
 	
 	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) {
@@ -607,6 +616,19 @@ void AC_PosControl::run_z_controller()
 
     // send throttle to attitude controller with angle boost
     _attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);
+
+	static int f = 401;
+
+	f++;
+	if(f>400)
+	{
+
+		gcs().send_text(MAV_SEVERITY_INFO, " dt  %f %f  %f",curr_alt,last_alt,_dt);
+		gcs().send_text(MAV_SEVERITY_INFO, " alt  %f %f  %f",_pos_target.z,curr_alt, _pos_error.z);
+		gcs().send_text(MAV_SEVERITY_INFO, " vel  %f  %f  %f",_vel_target.z,curr_vel.z,_vel_error.z);
+		gcs().send_text(MAV_SEVERITY_INFO, " AC  %f  %f  %f  ",_accel_target.z,z_accel_meas,thr_out);
+		f=0;
+	}
 }
 
 ///

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -301,6 +301,10 @@ public:
                         const uint8_t failure_msg_len);
 
     static const struct AP_Param::GroupInfo var_info[];
+	AC_P		_p_pos_z;
+	float       _dt;    
+	float       _accel_z_cms;           // max vertical acceleration in cm/s/s
+	float  last_alt;
 
 protected:
 
@@ -377,20 +381,20 @@ protected:
     // parameters
     AP_Float    _accel_xy_filt_hz;      // XY acceleration filter cutoff frequency
     AP_Float    _lean_angle_max;        // Maximum autopilot commanded angle (in degrees). Set to zero for Angle Max
-    AC_P        _p_pos_z;
+   
     AC_P        _p_vel_z;
     AC_PID      _pid_accel_z;
     AC_P        _p_pos_xy;
     AC_PID_2D   _pid_vel_xy;
 
     // internal variables
-    float       _dt;                    // time difference (in seconds) between calls from the main program
+                // time difference (in seconds) between calls from the main program
     uint64_t    _last_update_xy_us;     // system time (in microseconds) since last update_xy_controller call
     uint64_t    _last_update_z_us;      // system time (in microseconds) of last update_z_controller call
     float       _speed_down_cms;        // max descent rate in cm/s
     float       _speed_up_cms;          // max climb rate in cm/s
     float       _speed_cms;             // max horizontal speed in cm/s
-    float       _accel_z_cms;           // max vertical acceleration in cm/s/s
+    
     float       _accel_last_z_cms;      // max vertical acceleration in cm/s/s
     float       _accel_cms;             // max horizontal acceleration in cm/s/s
     float       _leash;                 // horizontal leash length in cm.  target will never be further than this distance from the vehicle

libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp

@@ -94,16 +94,17 @@ void AC_PosControl_Sub::set_alt_target_from_climb_rate_ff(float climb_rate_cms,
 void AC_PosControl_Sub::relax_alt_hold_controllers()
 {
     //_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 = 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
+	 _vel_desired.z = 0.0f;
+    _flags.use_desvel_ff_z = false;
+    _vel_target.z = AC_AttitudeControl::sqrt_controller(sub.barometer.get_altitude()*100-_pos_target.z, _p_pos_z.kP(), _accel_z_cms, _dt);//12.19
+    _pos_target.z = sub.barometer.get_altitude()*100;// 12.19
+    _vel_last.z = _vel_target.z;
     _accel_desired.z = 0.0f;
     _accel_last_z_cms = 0.0f;
     _flags.reset_rate_to_accel_z = true;
     _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
 }