ソースを参照

更改了最大推进器的速度2500 去掉了低压电调的故障 改为比目鱼的推进器 345电调修改了读取参数ID因为比府邸的存储有问题

danny wang 1 年間 前
コミット
f79dadc496

+ 20 - 11
ArduSub/control_clean.cpp

@@ -1,7 +1,7 @@
 #include "Sub.h"
 //目标姿态
 
-float maxspeed = 800.0;
+float maxspeed = 910.0;
 float minspeed = 280.0;
 float maxerrorspeed = 310.0;
 
@@ -121,17 +121,10 @@ void Sub::clean_run()
 			 // gcs().send_text(MAV_SEVERITY_INFO, " track_limit %f\n",(float)rov_control.track_limit);
 			  j=0;
 		 }
-		 maxspeed = (float)rov_control.track_limit*10;
+		 
 		 
 
-		 if (maxspeed<280.0)
-		{
-			 maxspeed = 280.0;
-		}
-		 if(maxspeed>910.0)
-		{
-			 maxspeed = 910.0;
-		}
+
 		//turnspeed  = maxspeed - maxerror_f;
 		 
 		autoclean_flag_chose();//自动洗网的状态切换
@@ -175,10 +168,20 @@ void Sub::clean_net_joystick(void)
 	maxerrorspeed =(float)SRV_Channels::srv_channel(15)->get_output_min()/10;//两个履带最大差速 ----------最深距离 厘米 
 	maxspeed_set = (float)SRV_Channels::srv_channel(15)->get_trim()/10;
 
+	maxspeed = 910.0;
+
 	if(maxspeed_set<maxspeed)
 	{
 		maxspeed = maxspeed_set;
 	}
+	if (maxspeed<280.0)
+	{
+		 maxspeed = 280.0;
+	}
+	 if(maxspeed>910.0)
+	{
+		 maxspeed = 910.0;
+	}
 
 	if(handclean == TRUE ){
 		
@@ -424,7 +427,13 @@ void Sub::motor_toCan(void)
 		   motors.motor_to_can[9] = 0;
 	   }
    }
-	  
+		static int jsn = 0;
+			jsn++;
+			 if(jsn>400)
+			{
+					gcs().send_text(MAV_SEVERITY_INFO, " track %d %d \n",(int)motors.motor_to_can[8],motors.motor_to_can[9]);
+					jsn=0;
+			}  
 
  }
 void Sub::clean_sidenet_auto(void)

+ 1 - 1
libraries/AP_Motors/AP_Motors6DOF.cpp

@@ -229,7 +229,7 @@ void AP_Motors6DOF::output_min()
 
 #define ThrustScale 1000
 #define hv_min 251
-#define hv_max 2000
+#define hv_max 2500
 int16_t AP_Motors6DOF::calc_thrust_to_HV(float thrust_in,int8_t i)
 { 
 	  int16_t thrust = 0;

+ 65 - 57
libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -55,6 +55,9 @@
 #include <uavcan/equipment/esc/HighVoltThruster1GetRegs.hpp>
 #include <uavcan/equipment/esc/HighVoltThruster2GetRegs.hpp>
 #include <uavcan/equipment/esc/HighVoltThruster3GetRegs.hpp>
+#include <uavcan/equipment/esc/HighVoltThruster1GetRegsFujian.hpp>
+#include <uavcan/equipment/esc/HighVoltThruster2GetRegsFujian.hpp>
+#include <uavcan/equipment/esc/HighVoltThruster3GetRegsFujian.hpp>
 
 #include <uavcan/equipment/esc/SpeedCommands.hpp>
 
@@ -178,11 +181,12 @@ union DRIVEBOX_ABNORMAL_UNI motor_stall_state;//获取所有的堵转
 
 
 union PROPELLER_ABNORMAL_UNI propeller_block;//推进器缠绕等级
+union PROPELLER_ABNORMAL_UNI track_block;//推进器缠绕等级
 
 
 int16_t temperature_power = 0;//温度
 int16_t voltage48V= 0;//48V电源
-int16_t driveleak = 0;//泄露
+//int16_t driveleak = 0;//泄露
 
 extern mavlink_set_slave_parameter_t set_stm32_param;
 
@@ -205,7 +209,7 @@ int16_t hv_motor_speed_32 = 0;
 
 int16_t lightstate=0;
 int16_t usbl_power=0;
-int16_t current_trust=0;
+int16_t light_leak=0;
 
 #define reboot_time 3000
 //----------------------------------------------
@@ -225,9 +229,12 @@ AP_UAVCAN::AP_UAVCAN() :
         _SRV_conf[i].esc_pending = false;
         _SRV_conf[i].servo_pending = false;
     }
-	
+	sublightstate = 0;
+	trackblock_flag =0;
+	propellerblock_flag =0;
 	motor_stall_state.all = 0;
 	propeller_block.all = 0;
+	track_block.all = 0;
 	for (uint8_t cnt = 0; cnt < 12; cnt++)
 	{
 		motors_receive[cnt]=0;
@@ -259,64 +266,55 @@ static void motor_res_st_cb(const uavcan::ReceivedDataStructure<uavcan::equipmen
 
 	if (msg.motor_index == 0){
 		//推进器0,1,2
-		motors_receive[0] = msg.rpm0 & 0x0FFF;
+		motors_receive[0] = msg.rpm0 & 0x0FFF;//推进器0
 		propeller_block.bit.motor0 = (msg.rpm0 & 0xF000)>>12;
-		motors_receive[1] = msg.rpm1 & 0x0FFF;
+		motors_receive[1] = msg.rpm1 & 0x0FFF;//推进器1
 		propeller_block.bit.motor1 = (msg.rpm1 & 0xF000)>>12;
-		motors_receive[2] = msg.rpm2 & 0x0FFF;
-		propeller_block.bit.motor2 = (msg.rpm2 & 0xF000)>>12;
-
-		
-	//	gcs().send_text(MAV_SEVERITY_WARNING, "motor_index = 0 %d %d %d.",(int)motors_receive[0],(int)motors_receive[1],motors_receive[2]);
 		
 	}
-	/*else if(msg.motor_index == 3){
-		//推进器3,4,5
-		motors_receive[3] = msg.rpm0 & 0x0FFF;
-		propeller_block.bit.motor3 = (msg.rpm0 & 0xF000)>>12;
-		motors_receive[4] = msg.rpm1 & 0x0FFF;
-		propeller_block.bit.motor4 = (msg.rpm1 & 0xF000)>>12;
-		motors_receive[5] = msg.rpm2 & 0x0FFF;
-		propeller_block.bit.motor5 = (msg.rpm2 & 0xF000)>>12;
+	else if(msg.motor_index == 3){
+		//推进器0.1.履带左的给定
+		int16_t rmp1  = msg.rpm0;
+		int16_t rmp2 =  msg.rpm1;
+		int16_t rmp3 =  msg.rpm2;
+
 		static int16_t c2  = 0;
 		c2++;
-		if(c2>100){
+		if(c2>50){
 			c2 =0 ;
-			//gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 3 %d %d %d.",(int)motors_receive[3],(int)motors_receive[4],motors_receive[5]);
+			gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 3 %d %d %d.",(int)rmp1,(int)rmp2,(int)rmp3);
 				
 		}
 
-	}*/
+	}
 	else if(msg.motor_index == 6){
-		////推进器6,7,履带1
-		motors_receive[6] = msg.rpm0 & 0x0FFF;
-		propeller_block.bit.motor6 = (msg.rpm0 & 0xF000)>>12;
-		motors_receive[7] = msg.rpm1 & 0x0FFF;
-		propeller_block.bit.motor7 = (msg.rpm1 & 0xF000)>>12;
-		motors_receive[8] = msg.rpm2 & 0x0FFF;
 		
+		
+		int16_t rpm = msg.rpm0;//履带右的给定
+		
+
+		motors_receive[8] = msg.rpm2 & 0x0FFF;//履带1
+		track_block.bit.motor0 = (msg.rpm2 & 0xF000)>>12;
 
 		static int16_t c3  = 0;
 		c3++;
-		if(c3>100){
+		if(c3>50){
 			c3 =0 ;
-			//gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 6 %d %d %d",(int)motors_receive[6],(int)motors_receive[7],motors_receive[8]);
+			gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 6 %d %d ",(int)rpm,motors_receive[8]);
 			
 		}
 	}
 	else if(msg.motor_index == 9){
-	////履带2,毛刷1,毛刷2
-		motors_receive[9] = msg.rpm0 & 0x0FFF;
-		
-		motors_receive[10] = msg.rpm1 & 0x0FFF;
+	////履带2
+		motors_receive[9] = msg.rpm0 & 0x0FFF;//履带2速度
 		
-		motors_receive[11] = msg.rpm2 & 0x0FFF;
+		track_block.bit.motor1 = (msg.rpm0 & 0xF000)>>12;
 		
 		static int16_t c4  = 0;
 		c4++;
 		if(c4>100){
 			c4 =0 ;
-			//gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 9 %d %d %d.",(int)motors_receive[9],(int)motors_receive[10],motors_receive[11]);
+			gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 9 %d.",(int)motors_receive[9]);
 			
 		}
 	}
@@ -338,7 +336,7 @@ static void motor_res_st_cb(const uavcan::ReceivedDataStructure<uavcan::equipmen
 	////履带2,毛刷1,毛刷2
 		lightstate = msg.rpm0;
 		usbl_power = msg.rpm1;
-		current_trust = msg.rpm2;
+		light_leak = msg.rpm2;//灯 漏水
 
 
 	}
@@ -390,7 +388,7 @@ static void (*motor_par1_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::
 		={motor_par1_cb0,motor_par1_cb1};
 
 //高压电调1参数返回
-static void thruster1GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegs>& msg,uint8_t mgr ){
+static void thruster1GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegsFujian>& msg,uint8_t mgr ){
 	uint8_t buffer[7]={0,0,0,0,0,0,0};
 	
 	for (uint8_t i = 0; i < 7; i++)
@@ -407,18 +405,18 @@ static void thruster1GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equi
 
 	//gcs().send_text(MAV_SEVERITY_WARNING, "thruster3 %x .",(uint32_t)hv_reg_get.data[0]);
 }
-static void Thruster1GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegs>& msg ){
+static void Thruster1GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegsFujian>& msg ){
 			thruster1GetRegs_cb(msg,0);
 }
-static void Thruster1GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegs>& msg ){
+static void Thruster1GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegsFujian>& msg ){
 			thruster1GetRegs_cb(msg,1);
 }
 
-static void (*Thruster1GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegs>& msg )
+static void (*Thruster1GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegsFujian>& msg )
 		={Thruster1GetRegs_cb0,Thruster1GetRegs_cb1};
 
 //高压电调2参数返回
-static void thruster2GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegs>& msg ,uint8_t mgr){
+static void thruster2GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegsFujian>& msg ,uint8_t mgr){
 
 	uint8_t buffer[7]={0,0,0,0,0,0,0};
 	
@@ -440,17 +438,17 @@ static void thruster2GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equi
 
 
 }
-static void Thruster2GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegs>& msg ){
+static void Thruster2GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegsFujian>& msg ){
 			thruster2GetRegs_cb(msg,0);
 }
-static void Thruster2GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegs>& msg ){
+static void Thruster2GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegsFujian>& msg ){
 			thruster2GetRegs_cb(msg,1);
 }
 
-static void (*Thruster2GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegs>& msg )
+static void (*Thruster2GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegsFujian>& msg )
 		={Thruster2GetRegs_cb0,Thruster2GetRegs_cb1};
 //高压电调3参数返回
-static void thruster3GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegs>& msg ,uint8_t mgr){
+static void thruster3GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegsFujian>& msg ,uint8_t mgr){
 	uint8_t buffer[7]={0,0,0,0,0,0,0};
 	
 	for (uint8_t i = 0; i < 7; i++)
@@ -474,14 +472,14 @@ static void thruster3GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equi
 
 	//gcs().send_text(MAV_SEVERITY_WARNING, "thruster3 %d %d %d.",hv_reg_get.aim,(int)data[0],(int)data[1]);
 }
-static void Thruster3GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegs>& msg ){
+static void Thruster3GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegsFujian>& msg ){
 			thruster3GetRegs_cb(msg,0);
 }
-static void Thruster3GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegs>& msg ){
+static void Thruster3GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegsFujian>& msg ){
 			thruster3GetRegs_cb(msg,1);
 }
 
-static void (*Thruster3GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegs>& msg )
+static void (*Thruster3GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegsFujian>& msg )
 		={Thruster3GetRegs_cb0,Thruster3GetRegs_cb1};
 
 //高压电调1定时返回
@@ -511,7 +509,7 @@ struct HVmotor haoye2={0,0,0,0,0,0};
 		if (per>150)
 		{
 			per = 0;
-			gcs().send_text(MAV_SEVERITY_INFO, "HV3 %d %d %d",(int)Thruster1.voltage,(int)Thruster1.speedref,(int)Thruster1.temperature);
+			gcs().send_text(MAV_SEVERITY_INFO, "HV3 %d %d %d %d",(int)Thruster1.voltage,(int)Thruster1.speedref,(int)Thruster1.temperature,(int)Thruster1.motortemperature);
 			}
 			
 	//}
@@ -729,8 +727,8 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
 		return;
 	}
 	//高压电调1 参数返回
-	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster1GetRegs>* Thruster1GetRegs;
-	Thruster1GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster1GetRegs>(*node);
+	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster1GetRegsFujian>* Thruster1GetRegs;
+	Thruster1GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster1GetRegsFujian>(*node);
 	const int Tht1GetReg = Thruster1GetRegs->start(Thruster1GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
 	if (Tht1GetReg<0)
 	{
@@ -738,8 +736,8 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
 	}	
 
 	//高压电调2 参数返回
-	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster2GetRegs>* Thruster2GetRegs;
-	Thruster2GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster2GetRegs>(*node);
+	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster2GetRegsFujian>* Thruster2GetRegs;
+	Thruster2GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster2GetRegsFujian>(*node);
 	const int Tht2GetReg = Thruster2GetRegs->start(Thruster2GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
 	if (Tht2GetReg<0)
 	{
@@ -747,8 +745,8 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
 	}
 
 	//高压电调3 参数返回
-	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster3GetRegs>* Thruster3GetRegs;
-	Thruster3GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster3GetRegs>(*node);
+	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster3GetRegsFujian>* Thruster3GetRegs;
+	Thruster3GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster3GetRegsFujian>(*node);
 	const int Tht3GetReg = Thruster3GetRegs->start(Thruster3GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
 	if (Tht3GetReg<0)
 	{
@@ -954,10 +952,13 @@ void AP_UAVCAN::loop(void)
 
 			
 			propellerblock_flag = propeller_block.all;//推进器缠绕等级
+			trackblock_flag = track_block.all;//推进器缠绕等级
+
+			
 			temperature_48Vpower = temperature_power;//can获取18B20温度
 			board_voltage = voltage48V;//can获取48V电源
-			driverleakstate = driveleak;
-			
+			driverleakstate = light_leak;//灯 漏水
+			sublightstate = lightstate;
 
 			HVmotor1 = Thruster1;
 			HVmotor2 = Thruster2;
@@ -1138,6 +1139,13 @@ void AP_UAVCAN::loop(void)
 
 
 
+			static int16_t c2  = 0;
+			c2++;
+			if(c2>200){
+				c2 =0 ;
+				gcs().send_text(MAV_SEVERITY_WARNING, "motorget %d %d.",(int)hv_reg_get.aim,(int)hv_reg_get.data[0]);
+					
+			}
 
 
 			

+ 2 - 1
libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -137,10 +137,11 @@ public:
 	uint16_t motor_stall_flag;//堵转标志
 	
 	uint32_t propellerblock_flag;//推进器缠绕
+	uint32_t trackblock_flag;
 	int16_t temperature_48Vpower;//48V温度
 	int16_t board_voltage;//48V电源
 	int16_t driverleakstate;//驱动仓漏水
-	
+	int16_t sublightstate;
 	
 	static AP_UAVCAN *_singleton;
 	static AP_UAVCAN *get_singleton() {

+ 11 - 21
libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1020,17 +1020,7 @@ 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;
-uint16_t lvtruster_fault(uint8_t i,uint16_t motor_stall_flag,uint32_t propellerblock_flag){
-	
-	if((motor_stall_flag&(0x0001<<i))!=0){//堵转
-		uint16_t lvtrusterfault = (motor_stall_flag&(0x0001<<i))<<(4-i);
-		return lvtrusterfault;
 
-	}else{
-	return (propellerblock_flag&(0x0000000F<<(4*i)))>>(4*i);
-	}
-	
-}
 void GCS_MAVLINK::update_send()
 {
 	
@@ -1044,8 +1034,8 @@ void GCS_MAVLINK::update_send()
 			
 			mav_motor_speed_back.Ltrack =uavcan.motor_from_stm32[8];
 			mav_motor_speed_back.Rtrack =uavcan.motor_from_stm32[9];
-			mav_motor_speed_back.motor1 =(float)uavcan.motor_from_stm32[0]*100/7;
-			mav_motor_speed_back.motor2 =(float)uavcan.motor_from_stm32[1]*100/7;
+			mav_motor_speed_back.motor1 =(float)uavcan.motor_from_stm32[0]*100/2;//比目鱼2
+			mav_motor_speed_back.motor2 =(float)uavcan.motor_from_stm32[1]*100/2;
 			mav_motor_speed_back.motor3 =(float)uavcan.HVmotor1.speed;
 			mav_motor_speed_back.motor4 =(float)uavcan.HVmotor2.speed;
 			mav_motor_speed_back.motor5 =(float)uavcan.HVmotor3.speed;
@@ -1053,7 +1043,7 @@ void GCS_MAVLINK::update_send()
 			mav_motor_speed_back.motor7 =0;//(float)uavcan.motor_from_stm32[6];
 			mav_motor_speed_back.motor8 =0;//(float)uavcan.motor_from_stm32[7];	
 
-			rov_message.floodlight = sub.lights;
+			rov_message.floodlight = uavcan.sublightstate;//sub.lights;
 			//pressure_level  在各个模式中已经赋值
 			rov_message.low_voltage = (float)(uavcan.board_voltage)/10;//传过来的是*100的?
 			rov_message.high_voltage = (float)(uavcan.HVmotor2.voltage);//电调1的电压
@@ -1062,17 +1052,17 @@ void GCS_MAVLINK::update_send()
 			rov_message.motor_block_flag =  (uint16_t)(uavcan.motor_stall_flag);
 
 
-			rov_message.track_fault_flag[0] = (uint16_t)0;
-			rov_message.track_fault_flag[1] = (uint16_t)0;
+			rov_message.track_fault_flag[0] = 0;//lvtruster_fault(0,uavcan.motor_stall_flag,uavcan.trackblock_flag);
+			rov_message.track_fault_flag[1] = 0;//lvtruster_fault(1,uavcan.motor_stall_flag,uavcan.trackblock_flag);
 
-			rov_message.motor_twine_flag[0] = lvtruster_fault(0,uavcan.motor_stall_flag,uavcan.propellerblock_flag);//(uint16_t)(uavcan.propellerblock_flag & 0x0000000F)|((uavcan.motor_stall_flag&0x0001)<<4);
-			rov_message.motor_twine_flag[1] = lvtruster_fault(1,uavcan.motor_stall_flag,uavcan.propellerblock_flag);//(uint16_t)((uavcan.propellerblock_flag & 0x000000F0)>>4)|((uavcan.motor_stall_flag&0x0002)<<3);
+			rov_message.motor_twine_flag[0] = 0;//lvtruster_fault(0,uavcan.motor_stall_flag,uavcan.propellerblock_flag);//(uint16_t)(uavcan.propellerblock_flag & 0x0000000F)|((uavcan.motor_stall_flag&0x0001)<<4);
+			rov_message.motor_twine_flag[1] = 0;//lvtruster_fault(1,uavcan.motor_stall_flag,uavcan.propellerblock_flag);//(uint16_t)((uavcan.propellerblock_flag & 0x000000F0)>>4)|((uavcan.motor_stall_flag&0x0002)<<3);
 			rov_message.motor_twine_flag[2] = (uint16_t)(uavcan.HVmotor1.fault);
 			rov_message.motor_twine_flag[3] = (uint16_t)(uavcan.HVmotor2.fault);
 			rov_message.motor_twine_flag[4] = (uint16_t)(uavcan.HVmotor3.fault);
-			rov_message.motor_twine_flag[5] = 0;//(uint16_t)((uavcan.propellerblock_flag & 0x00F00000)>>20)|((uavcan.motor_stall_flag&0x0020)>>1);//高八位堵转 低八位缠绕
-			rov_message.motor_twine_flag[6] = 0;//(uint16_t)((uavcan.propellerblock_flag & 0x0F000000)>>24)|((uavcan.motor_stall_flag&0x0040)>>2);
-			rov_message.motor_twine_flag[7] = 0;//(uint16_t)((uavcan.propellerblock_flag & 0xF0000000)>>28)|((uavcan.motor_stall_flag&0x0080)>>3);
+			rov_message.motor_twine_flag[5] = 0;//
+			rov_message.motor_twine_flag[6] = (uint16_t)(uavcan.driverleakstate & 0x00ff);//灯
+			rov_message.motor_twine_flag[7] = (uint16_t)((uavcan.driverleakstate & 0xff00)>>8);//漏水
 
 
 			rov_message.track_power[0] = 0;
@@ -1096,7 +1086,7 @@ void GCS_MAVLINK::update_send()
 			rov_motor_temp.motor_temperature[1] = 0;
 			rov_motor_temp.motor_temperature[2] = (uint16_t)(uavcan.HVmotor1.motortemperature);
 			rov_motor_temp.motor_temperature[3] = (uint16_t)(uavcan.HVmotor2.motortemperature);
-			rov_motor_temp.motor_temperature[4] = (uint16_t)(uavcan.HVmotor2.motortemperature);
+			rov_motor_temp.motor_temperature[4] = (uint16_t)(uavcan.HVmotor3.motortemperature);
 			rov_motor_temp.motor_temperature[5] = 0;
 			rov_motor_temp.motor_temperature[6] = 0;
 			rov_motor_temp.motor_temperature[7] = 0;

+ 3 - 0
modules/uavcan/dsdl/uavcan/equipment/esc/22621.HighVoltThruster1GetRegsFujian.uavcan

@@ -0,0 +1,3 @@
+# GET REGS
+
+uint8[7] BUFFER

+ 3 - 0
modules/uavcan/dsdl/uavcan/equipment/esc/22622.HighVoltThruster2GetRegsFujian.uavcan

@@ -0,0 +1,3 @@
+# GET REGS
+
+uint8[7] BUFFER

+ 3 - 0
modules/uavcan/dsdl/uavcan/equipment/esc/22623.HighVoltThruster3GetRegsFujian.uavcan

@@ -0,0 +1,3 @@
+# GET REGS
+
+uint8[7] BUFFER