修改了sport mannual althold模式的姿态控制

修改了深度控制
修改了深度计算

ArduSub/ArduSub.cpp

@@ -119,6 +119,17 @@ 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/Sub.h

@@ -580,6 +580,8 @@ private:
 
     // Handles attitude control for stabilize and althold modes
     void handle_attitude();
+	void handle_attitude_stablize();
+	void handle_attitude_sport();
     bool auto_init(void);
     void auto_run();
     void auto_wp_start(const Vector3f& destination);

ArduSub/control_althold.cpp

@@ -40,90 +40,53 @@ bool Sub::althold_init()
 
 void Sub::handle_attitude()
 {
-    uint32_t tnow = AP_HAL::millis();
-
-    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
-
-    // 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_s = target_roll;
-        last_pitch_s = target_pitch;
-        last_yaw_s = target_yaw;
-        
-        attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, target_yaw, true);
-    } else {
-		
-        //int32_t yaw_input= (int32_t)((float)yaw_press*100);
-		//int32_t roll_input    = 0;
-		int32_t pitch_input   = 0;
-		
-
-		if (prepare_state == Horizontal)
-	   {
-		   pitch_input = 0;
-		   target_roll = 0;
-		   target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
-	   }else if(prepare_state == Vertical)
-	   	{
-		   pitch_input = -8000;
-		   target_roll = ((float)channel_yaw->get_control_in()*0.6);
-		   
-		   target_yaw = 0;
-	   }
-	   else{
-	   		pitch_input = 0;
-	   	}
-	   pitch_input_inc = constrain_int32(pitch_input,-9000,9000);
-	   //-------------------------------------
-	  // target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
-	  if (abs(target_roll) > 300 ){
-	  	attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, 0, 0);
-		last_yaw_s = ahrs.yaw_sensor;
-		last_roll_s = ahrs.roll_sensor;
-	  	last_input_ms_stable = tnow;
-	  	
-	  	}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_s = ahrs.yaw_sensor;
-			
-            last_input_ms_stable = tnow;
-        } else if (tnow < last_input_ms_stable + 250) {
-            // just brake for a few mooments so we don't bounce
-            last_yaw_s = 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_s, (float)pitch_input_inc, last_yaw_s, true);
-            //attitude_control.input_euler_angle_roll_pitch_yaw(roll_input, (float)pitch_input_inc, last_yaw, true);
-        }//--------------------------------------------------------------------------------------------------------------
-       //attitude_control.input_euler_angle_roll_pitch_yaw(roll_input, (float)pitch_input_inc, yaw_input, true);//pitch九十度 竖直前进有可能翻车
-		//attitude_control.input_euler_angle_roll_pitch_yaw_quat_control((float)roll_input, (float)pitch_input_inc, (float)yaw_input, true);//四元数控制在pitch90度时的效果与欧拉角控制在80度时相差不多, 但是在360往0的YAW切换的时候会猛烈转动
-		static int f = 0;
-		f++;
-		if(f>800)
-		{
-		//float cosruslt = sinf(radians(286)*0.5f);
-
-			//gcs().send_text(MAV_SEVERITY_INFO, " stableinput %f %f,%f ",(float)last_roll_s,(float)pitch_input_inc,(float)last_yaw_s);
-			f=0;
-		}
-        
-    }
+		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());
+
+			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);
+	        }
+	    }
 
 }
 
@@ -147,8 +110,11 @@ void Sub::althold_run()
 
     // 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();
 
     pos_control.update_z_controller();
     // Read the output of the z controller and rotate it so it always points up
@@ -156,12 +122,14 @@ void Sub::althold_run()
     // 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(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);
     motors.set_forward(-throttle_vehicle_frame.x + channel_forward->norm_input());
     motors.set_lateral(-throttle_vehicle_frame.y + 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.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

ArduSub/control_sport.cpp

@@ -20,23 +20,7 @@ bool Sub::sport_init()
     last_input_ms = AP_HAL::millis();
     return 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);
+void Sub::handle_attitude_sport(){
 	uint32_t tnow = AP_HAL::millis();
 
     // get pilot desired lean angles
@@ -62,7 +46,8 @@ void Sub::sport_run()
     } 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((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) {
@@ -86,7 +71,27 @@ void Sub::sport_run()
         }
     }
 
-   // handle_attitude();
+
+}
+
+// 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);
@@ -96,9 +101,9 @@ void Sub::sport_run()
     //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(channel_lateral->norm_input());
     
-	motors.set_lateral(0);
+	//motors.set_lateral(0);
 
 	static int j = 0;
 	  j++;

ArduSub/control_stabilize.cpp

@@ -26,6 +26,94 @@ bool Sub::stabilize_init()
 // should be called at 100hz or more
 extern mavlink_rov_state_monitoring_t rov_message;
 
+void Sub::handle_attitude_stablize(){
+	
+	uint32_t tnow = AP_HAL::millis();
+
+	motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+
+	// 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_s = target_roll;
+		last_pitch_s = target_pitch;
+		last_yaw_s = target_yaw;
+		
+		attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, target_yaw, true);
+	} else {
+		
+		//int32_t yaw_input= (int32_t)((float)yaw_press*100);
+		//int32_t roll_input	= 0;
+		int32_t pitch_input   = 0;
+		
+
+		if (prepare_state == Horizontal)
+	   {
+		   pitch_input = 0;
+		   target_roll = 0;
+		   last_roll_s = 0;
+		   target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+	   }else if(prepare_state == Vertical)
+		{
+		   pitch_input = 8000;
+		   target_roll = ((float)channel_yaw->get_control_in()*0.3);
+		   
+		   target_yaw = 0;
+	   }
+	   else{
+			pitch_input = 0;
+		}
+	   pitch_input_inc = constrain_int32(pitch_input,-9000,9000);
+	   //-------------------------------------
+	  // target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+	  if (abs(target_roll) > 300 ){
+		attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, 0, 0);
+		last_yaw_s = ahrs.yaw_sensor;
+		last_roll_s = ahrs.roll_sensor;
+		last_input_ms_stable = tnow;
+		
+		}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_s = ahrs.yaw_sensor;
+			
+			last_input_ms_stable = tnow;
+		} else if (tnow < last_input_ms_stable + 250) {
+			// just brake for a few mooments so we don't bounce
+			last_yaw_s = 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_s, (float)pitch_input_inc, last_yaw_s, true);
+			//attitude_control.input_euler_angle_roll_pitch_yaw(roll_input, (float)pitch_input_inc, last_yaw, true);
+		}//--------------------------------------------------------------------------------------------------------------
+	   //attitude_control.input_euler_angle_roll_pitch_yaw(roll_input, (float)pitch_input_inc, yaw_input, true);//pitch九十度 竖直前进有可能翻车
+		//attitude_control.input_euler_angle_roll_pitch_yaw_quat_control((float)roll_input, (float)pitch_input_inc, (float)yaw_input, true);//四元数控制在pitch90度时的效果与欧拉角控制在80度时相差不多, 但是在360往0的YAW切换的时候会猛烈转动
+		static int f = 0;
+		f++;
+		if(f>800)
+		{
+		//float cosruslt = sinf(radians(286)*0.5f);
+
+			//gcs().send_text(MAV_SEVERITY_INFO, " stableinput %f %f,%f ",(float)last_roll_s,(float)pitch_input_inc,(float)last_yaw_s);
+			f=0;
+		}
+		
+	}
+}
 
 void Sub::stabilize_run()
 {
@@ -47,7 +135,7 @@ void Sub::stabilize_run()
     }
     motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-    handle_attitude();
+    handle_attitude_stablize();
 
     motors.set_forward(channel_forward->norm_input());
     motors.set_lateral(channel_lateral->norm_input());

libraries/AP_Baro/AP_Baro.cpp

@@ -755,8 +755,9 @@ void AP_Baro::update(void)
 			altitude = ( 101325- corrected_pressure) / 9800.0f / _specific_gravity;//sensors[i].ground_pressure
 			sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
             if (sensors[i].alt_ok) {
-                sensors[i].altitude = altitude ;//+ _alt_offset_active;
+                sensors[i].altitude = altitude + _alt_offset;
             }
+
 			/*
             float corrected_pressure = sensors[i].pressure + sensors[i].p_correction;
             if (sensors[i].type == BARO_TYPE_AIR) {

libraries/AP_Motors/AP_Motors6DOF.cpp

@@ -359,8 +359,8 @@ void AP_Motors6DOF::output_motor8_and_motor9(){
 	if(k>400)
 	{
 	
-		gcs().send_text(MAV_SEVERITY_INFO, "motor_speed %d %d \n", pwm_track[0],pwm_track[1]);
-		gcs().send_text(MAV_SEVERITY_INFO, "motor_speed %d %d %d \n", (int)mav_motor_speed.motorTest,calc_thrust_to_pwm((float)mav_motor_speed.Ltrack/260),calc_thrust_to_pwm((float)mav_motor_speed.Rtrack/260));
+	//	gcs().send_text(MAV_SEVERITY_INFO, "motor_speed %d %d \n", pwm_track[0],pwm_track[1]);
+	//	gcs().send_text(MAV_SEVERITY_INFO, "motor_speed %d %d %d \n", (int)mav_motor_speed.motorTest,calc_thrust_to_pwm((float)mav_motor_speed.Ltrack/260),calc_thrust_to_pwm((float)mav_motor_speed.Rtrack/260));
 		k=0;
 	}