更改了通信协议 stablize 的初始深度

ArduSub/GCS_Mavlink.cpp

@@ -424,6 +424,7 @@ static const ap_message STREAM_EXTRA2_msgs[] = {
 	
 	//MSG_VFR_HUD
 	//MSG_ROTATION_MATRIX_ARRAY,
+	MSG_ROV_MOTOR_TEMP,
 	MSG_ROV_STATE_MONITORING,
 	//MSG_SET_SLAVE_PARAMETER,
 	MSG_MOTOR_SPEED,

ArduSub/control_sport .cpp

@@ -162,7 +162,7 @@ void Sub::handle_attitude_sport(){
 				 if (count>400)
 				{
 					 count = 0;
-					 gcs().send_text(MAV_SEVERITY_INFO, "ahr %f %f %f \n",nowsinpitch,maxsinpitch,get_pitch_to_ned(rotpitch_limit));
+					// gcs().send_text(MAV_SEVERITY_INFO, "ahr %f %f %f \n",nowsinpitch,maxsinpitch,get_pitch_to_ned(rotpitch_limit));
 				 }
 			}else{
 				attitude_desired_quat_.from_rotation_matrix(rotyaw*rotpitch);
@@ -307,7 +307,7 @@ void Sub::sport_run_alt(void){
 		lastdepth = holding_depth;
 		
 		
-		gcs().send_text(MAV_SEVERITY_INFO, "get_alt %d %f %f\n",(int)holding_depth,pos_control.get_alt_target(),barometer.get_altitude()*100);
+		//gcs().send_text(MAV_SEVERITY_INFO, "get_alt %d %f %f\n",(int)holding_depth,pos_control.get_alt_target(),barometer.get_altitude()*100);
 	
 	}
 	 static uint16_t count = 0;
@@ -315,9 +315,9 @@ void Sub::sport_run_alt(void){
 	 if (count>400)
 		 {
 		 count = 0;
-		 gcs().send_text(MAV_SEVERITY_INFO, "surf %d %d %d %f\n",(int)ap.at_surface,(int)ap.at_bottom,(int)motors.limit.throttle_upper,(float)motors.get_throttle());
+		// gcs().send_text(MAV_SEVERITY_INFO, "surf %d %d %d %f\n",(int)ap.at_surface,(int)ap.at_bottom,(int)motors.limit.throttle_upper,(float)motors.get_throttle());
 		  
-		 gcs().send_text(MAV_SEVERITY_INFO, "alt %f %f %f %f\n",pos_control.get_alt_target(),barometer.get_altitude()*100,pos_control.alt_rate.get(),joystick_z);
+		// gcs().send_text(MAV_SEVERITY_INFO, "alt %f %f %f %f\n",pos_control.get_alt_target(),barometer.get_altitude()*100,pos_control.alt_rate.get(),joystick_z);
 
 	 }
 

ArduSub/control_stabilize.cpp

@@ -5,7 +5,7 @@
 bool Sub::stabilize_init()
 {
     // set target altitude to zero for reporting
-    pos_control.set_alt_target(-10);//03.15
+    pos_control.relax_alt_hold_controllers();//目标高度为当前高度，目标速度和上次速度 当前速度
     
     last_roll_s = 0;
     last_pitch_s = 0;
@@ -124,6 +124,7 @@ void Sub::stabilize_run()
         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();
+		pos_control.relax_alt_hold_controllers();//目标高度为当前高度，目标速度和上次速度 当前速度
         last_roll_s = 0;
         last_pitch_s = 0;
 		updowmgain =0.5;

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1017,6 +1017,7 @@ mavlink_data64_t rov_message3;
 extern mavlink_set_slave_parameter_t get_stm32_param;
 extern mavlink_hv_reg_get_t hv_reg_get;
 extern uint8_t get_stm32_param_buf[7];
+mavlink_rov_motor_temp_t rov_motor_temp = {0,0,0,0,0,0,0,0,0,0};
 
 int countper = 0;
 
@@ -1046,6 +1047,8 @@ void GCS_MAVLINK::update_send()
 			rov_message.deep =fabsf(sub.barometer.get_altitude());//深度
 			//rov_message.temp = 0;
 			rov_message.motor_block_flag =	0;
+			rov_message.track_fault_flag[0] = 0;
+			rov_message.track_fault_flag[1] = 0;
 			rov_message.motor_twine_flag[0] = 0;
 			rov_message.motor_twine_flag[1] = 0;
 			rov_message.motor_twine_flag[2] = 0;
@@ -1054,6 +1057,8 @@ void GCS_MAVLINK::update_send()
 			rov_message.motor_twine_flag[5] = 0;
 			rov_message.motor_twine_flag[6] = 0;
 			rov_message.motor_twine_flag[7] = 0;
+			rov_message.track_power[0] = 0;
+			rov_message.track_power[1] = 0;
 			rov_message.motor_power[0] = 0;
 			rov_message.motor_power[1] = 0;
 			rov_message.motor_power[2] = 0;
@@ -4158,6 +4163,9 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
     bool ret = true;
 
     switch(id) {
+		case MSG_ROV_MOTOR_TEMP:
+		mavlink_msg_rov_motor_temp_send_struct(chan,&rov_motor_temp);
+		break;
 	case MSG_ROV_STATE_MONITORING:
 		mavlink_msg_rov_state_monitoring_send_struct(chan,&rov_message);
 		break;

libraries/GCS_MAVLink/ap_message.h

@@ -58,6 +58,7 @@ enum ap_message : uint8_t {
     MSG_PID_TUNING,
     MSG_VIBRATION,
     MSG_RPM,
+    MSG_ROV_MOTOR_TEMP,
     MSG_HV_REG_GET,
     MSG_ROV_STATE_MONITORING,
     MSG_MOTOR_SPEED,

modules/mavlink/message_definitions/v1.0/ardupilotmega.xml

@@ -1500,6 +1500,11 @@
       <description>RPM sensor output.</description>
       <field type="float" name="rpm1">RPM Sensor1.</field>
       <field type="float" name="rpm2">RPM Sensor2.</field>
+    </message>
+      <message id="227" name="ROV_MOTOR_TEMP">
+      <description>RPM sensor output.</description>
+      <field type="int16_t[2]" name="track_temperature">motors twine flag.</field>
+      <field type="int16_t[8]" name="motor_temperature">motors twine flag.</field>
     </message>
 	<message id="228" name="HV_REG_GET">
 	  <description>high voltage get reg data.</description>
@@ -1518,8 +1523,10 @@
       <field type="uint16_t" name="forward">motor forward direction.</field>
       <field type="uint16_t" name="turn">Motor turning direction.</field>
       <field type="uint16_t" name="hvMotorMod">HV Motor Mod.</field>
+      <field type="uint16_t[2]" name="track_fault_flag">motors twine flag.</field>
       <field type="uint16_t[8]" name="motor_twine_flag">motors twine flag.</field>
-      <field type="uint16_t[8]" name="motor_power">motors twine flag.</field>
+      <field type="int16_t[2]" name="track_power">motors twine flag.</field>
+      <field type="int16_t[8]" name="motor_power">motors twine flag.</field>
     </message>
 	<message id="235" name="MOTOR_SPEED">
 	  <description>motor speed communication.</description>