自研发电调，修改了电调的转速，修改了压力分级 修改了获取的电调返回的信息

ArduSub/control_clean.cpp

@@ -416,8 +416,8 @@ void Sub::motor_toCan(void)
 
    if(mav_motor_speed.motorTest == 1)//电机测试模式
    {
-	   motors.motor_to_can[8]  = mav_motor_speed.Ltrack*2;//切换成上位机控制 左履带
-	   motors.motor_to_can[9] = mav_motor_speed.Rtrack*2;//右履带
+	   motors.motor_to_can[8]  = mav_motor_speed.Ltrack*4;//切换成上位机控制 左履带
+	   motors.motor_to_can[9] = mav_motor_speed.Rtrack*4;//右履带
    }else{
 	   if(control_mode == CLEAN){
 		motors.motor_to_can[8]	 = motor1_speed_target;//飞控自己的履带

ArduSub/joystick.cpp

@@ -667,19 +667,19 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
 			   
 			}else if(PressLevel == third){
 				PressLevel = forth;
-				PressLevel_f = 1.0;//
+				PressLevel_f = 1.5;//
 			}else if (PressLevel == forth){
 				PressLevel = fifth;
-				PressLevel_f = 4.75;//
+				PressLevel_f = 4.55;//
 			}else if(PressLevel == fifth){
 				PressLevel = no;
 				PressLevel_f = 5.0;	//
 			}else if(PressLevel == no){
 				PressLevel = sixth;
-				PressLevel_f = 5.25;//
+				PressLevel_f = 5.45;//
 			}else if (PressLevel == sixth){
 				PressLevel = seventh;
-				PressLevel_f = 9.0;//
+				PressLevel_f = 8.5;//
 			}else if(PressLevel == seventh){
 				PressLevel = eighth;
 				PressLevel_f = 9.25;//
@@ -709,19 +709,19 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
 				 PressLevel_f = 9.25;//
 			 }else if(PressLevel == eighth){
 				 PressLevel = seventh;
-				 PressLevel_f = 9.0;//
+				 PressLevel_f = 8.5;//
 			 }else if (PressLevel == seventh){
 				 PressLevel = sixth;
-				  PressLevel_f = 5.25;//
+				  PressLevel_f = 5.45;//
 			 }else if(PressLevel == sixth){
 				 PressLevel = no;
 				  PressLevel_f = 5.0;//
 			 }else if(PressLevel == no){
 				 PressLevel = fifth;
-				 PressLevel_f = 4.75; //
+				 PressLevel_f = 4.55; //
 			 }else if (PressLevel == fifth){
 				 PressLevel = forth;
-				  PressLevel_f = 1.0;//	 
+				  PressLevel_f = 1.5;//	 
 			 }else if(PressLevel == forth){
 				 PressLevel = third;
 				  PressLevel_f = 0.75;//

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 2500
+#define hv_max 2200
 int16_t AP_Motors6DOF::calc_thrust_to_HV(float thrust_in,int8_t i)
 { 
 	  int16_t thrust = 0;
@@ -290,13 +290,7 @@ int16_t AP_Motors6DOF::calc_thrust_to_HV(float thrust_in,int8_t i)
 		}
 		
 		
-						static int k = 0;
-		k++;
-		if(k>400)
-		{
 
-		//gcs().send_text(MAV_SEVERITY_INFO, "_thrust_rpyt_out %d %d ,%d \n",(int)last_thrust_Dhot[2],(int)last_thrust_Dhot[3],(int)last_thrust_Dhot[4]);
-		}
 		int16_t speedref = (int16_t)((int32_t)last_thrust_Dhot[i]*hv_max/1000);
 	  	return constrain_int16(speedref,-hv_max, hv_max);
 	  }
@@ -418,6 +412,7 @@ void AP_Motors6DOF::output_to_Dshot()
 			motor_to_can[7] =  motor_out[7];//赋值给can
 
 
+
 		}
 
 

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -49,7 +49,7 @@
 
 
 
-
+#include <uavcan/equipment/esc/Clear.hpp>
 #include <uavcan/equipment/esc/HighVoltThrusterSetRegs.hpp>
 
 #include <uavcan/equipment/esc/HighVoltThruster1GetRegs.hpp>
@@ -146,6 +146,7 @@ static uavcan::Publisher<uavcan::equipment::esc::HighVoltThrusterSetRegs>* HVolt
 static uavcan::Publisher<uavcan::equipment::esc::Bfd1>* esc_Bfd1[MAX_NUMBER_OF_CAN_DRIVERS];
 static uavcan::Publisher<uavcan::equipment::esc::Bfd2>* esc_Bfd2[MAX_NUMBER_OF_CAN_DRIVERS];
 static uavcan::Publisher<uavcan::equipment::esc::Bfd3>* esc_Bfd3[MAX_NUMBER_OF_CAN_DRIVERS];
+static uavcan::Publisher<uavcan::equipment::esc::Clear>* clearFault[MAX_NUMBER_OF_CAN_DRIVERS];
 
 
 
@@ -388,6 +389,33 @@ static void (*motor_par1_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::
 		={motor_par1_cb0,motor_par1_cb1};
 
 //高压电调1参数返回
+		static void thruster1_1GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegs>& msg,uint8_t mgr ){
+			uint8_t buffer[7]={0,0,0,0,0,0,0};
+			
+			for (uint8_t i = 0; i < 7; i++)
+			{
+				buffer[i] = msg.BUFFER[i];
+			}
+		
+			hv_reg_get.aim = 3;
+			hv_reg_get.data[0] = (uint32_t)buffer[1] + (uint32_t)(buffer[2]<<8) + (uint32_t)(buffer[3]<<16) + (uint32_t)(buffer[4]<<24);
+			hv_reg_get.data[1] = 0;
+			hv_reg_get.data[2] = 0;
+			
+			
+		
+			gcs().send_text(MAV_SEVERITY_WARNING, "thruster3 %x .",(uint32_t)hv_reg_get.data[0]);
+		}
+		static void Thruster1_1GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegs>& msg ){
+					thruster1_1GetRegs_cb(msg,0);
+		}
+		static void Thruster1_1GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegs>& msg ){
+					thruster1_1GetRegs_cb(msg,1);
+		}
+		
+		static void (*Thruster1_1GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegs>& msg )
+				={Thruster1_1GetRegs_cb0,Thruster1_1GetRegs_cb1};
+//福建
 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};
 	
@@ -416,6 +444,38 @@ static void (*Thruster1GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<ua
 		={Thruster1GetRegs_cb0,Thruster1GetRegs_cb1};
 
 //高压电调2参数返回
+static void thruster2_1GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegs>& msg ,uint8_t mgr){
+
+	uint8_t buffer[7]={0,0,0,0,0,0,0};
+	
+	for (uint8_t i = 0; i < 7; i++)
+	{
+		buffer[i] = msg.BUFFER[i];
+	}
+
+	hv_reg_get.aim = 4;
+	
+	hv_reg_get.data[0] = (uint32_t)buffer[1] + (uint32_t)(buffer[2]<<8) + (uint32_t)(buffer[3]<<16) + (uint32_t)(buffer[4]<<24);
+	hv_reg_get.data[1] = 0;
+	hv_reg_get.data[2] = 0;
+	
+	
+
+	gcs().send_text(MAV_SEVERITY_WARNING, "thruster4 %x .",(uint32_t)hv_reg_get.data[0]);
+
+
+
+}
+static void Thruster2_1GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegs>& msg ){
+			thruster2_1GetRegs_cb(msg,0);
+}
+static void Thruster2_1GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegs>& msg ){
+			thruster2_1GetRegs_cb(msg,1);
+}
+
+static void (*Thruster2_1GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegs>& msg )
+		={Thruster2_1GetRegs_cb0,Thruster2_1GetRegs_cb1};
+//福建
 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};
@@ -448,6 +508,40 @@ static void Thruster2GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equ
 static void (*Thruster2GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegsFujian>& msg )
 		={Thruster2GetRegs_cb0,Thruster2GetRegs_cb1};
 //高压电调3参数返回
+static void thruster3_1GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegs>& msg ,uint8_t mgr){
+	uint8_t buffer[7]={0,0,0,0,0,0,0};
+	
+	for (uint8_t i = 0; i < 7; i++)
+	{
+		buffer[i] = msg.BUFFER[i];
+	}
+
+	hv_reg_get.aim = 5;
+	
+	hv_reg_get.data[0] = (uint32_t)buffer[1] + (uint32_t)(buffer[2]<<8) + (uint32_t)(buffer[3]<<16) + (uint32_t)(buffer[4]<<24);
+	hv_reg_get.data[1] = 0;
+	hv_reg_get.data[2] = 0;
+	
+	
+
+	gcs().send_text(MAV_SEVERITY_WARNING, "thruster5 %x .",(uint32_t)hv_reg_get.data[0]);
+
+
+	
+	//gcs().send_text(MAV_SEVERITY_WARNING, "thruster3data %d %d %d %d.",(int)buffer[0],(int)buffer[1],(int)buffer[2],(int)buffer[3]);
+
+	//gcs().send_text(MAV_SEVERITY_WARNING, "thruster3 %d %d %d.",hv_reg_get.aim,(int)data[0],(int)data[1]);
+}
+static void Thruster3_1GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegs>& msg ){
+			thruster3_1GetRegs_cb(msg,0);
+}
+static void Thruster3_1GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegs>& msg ){
+			thruster3_1GetRegs_cb(msg,1);
+}
+
+static void (*Thruster3_1GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegs>& msg )
+		={Thruster3_1GetRegs_cb0,Thruster3_1GetRegs_cb1};
+//福建
 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};
 	
@@ -509,7 +603,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 %d",(int)Thruster1.voltage,(int)Thruster1.speedref,(int)Thruster1.temperature,(int)Thruster1.motortemperature);
+			gcs().send_text(MAV_SEVERITY_INFO, "HV3 %d %d %d %d %d",(int)Thruster1.voltage,(int)Thruster1.speedref,(int)Thruster1.temperature,(int)Thruster1.currentlimit,(int)Thruster1.Imax/336);
 			}
 			
 	//}
@@ -551,7 +645,7 @@ static void Thruster2timeback_cb(const uavcan::ReceivedDataStructure<uavcan::equ
 		if (per>150)
 		{
 			per = 0;
-			gcs().send_text(MAV_SEVERITY_INFO, "HV4 %d %d %d",(int)Thruster2.voltage,(int)Thruster2.speedref,(int)Thruster2.temperature);
+			gcs().send_text(MAV_SEVERITY_INFO, "HV4 %d %d %d %d %d",(int)Thruster2.voltage,(int)Thruster2.speedref,(int)Thruster2.temperature,(int)Thruster2.currentlimit,(int)Thruster2.Imax/336);
 		}
 			
 	//}
@@ -592,7 +686,7 @@ static void Thruster3timeback_cb(const uavcan::ReceivedDataStructure<uavcan::equ
 		if (per>150)
 		{
 			per = 0;
-			gcs().send_text(MAV_SEVERITY_INFO, "HV5 %d %d %d ",(int)Thruster3.voltage,(int)Thruster3.speedref,(int)Thruster3.temperature);
+			gcs().send_text(MAV_SEVERITY_INFO, "HV5 %d %d %d %d %d",(int)Thruster3.voltage,(int)Thruster3.speedref,(int)Thruster3.temperature,(int)Thruster3.currentlimit,(int)Thruster3.Imax/336);
 		}
 			
 	//}
@@ -726,6 +820,32 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
 	{
 		return;
 	}
+		//高压电调1 参数返回------------------------------------------------------------
+	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster1GetRegs>* Thruster1_1GetRegs;
+	Thruster1_1GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster1GetRegs>(*node);
+	const int Tht1_1GetReg = Thruster1_1GetRegs->start(Thruster1_1GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
+	if (Tht1_1GetReg<0)
+	{
+		return;
+	}	
+
+	//高压电调2 参数返回
+	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster2GetRegs>* Thruster2_1GetRegs;
+	Thruster2_1GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster2GetRegs>(*node);
+	const int Tht2_1GetReg = Thruster2_1GetRegs->start(Thruster2_1GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
+	if (Tht2_1GetReg<0)
+	{
+		return;
+	}
+
+	//高压电调3 参数返回
+	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster3GetRegs>* Thruster3_1GetRegs;
+	Thruster3_1GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster3GetRegs>(*node);
+	const int Tht3_1GetReg = Thruster3_1GetRegs->start(Thruster3_1GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
+	if (Tht3_1GetReg<0)
+	{
+		return;
+	}//以下是福建的--------------------------
 	//高压电调1 参数返回
 	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster1GetRegsFujian>* Thruster1GetRegs;
 	Thruster1GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster1GetRegsFujian>(*node);
@@ -790,6 +910,10 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
 
 
 	//HVoltThtSetReg
+	//高压电调清故障
+	clearFault[driver_index] = new uavcan::Publisher<uavcan::equipment::esc::Clear>(*_node);
+    clearFault[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
+    clearFault[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);	
 	//高压电调参数设置
 	HVoltThtSetReg[driver_index] = new uavcan::Publisher<uavcan::equipment::esc::HighVoltThrusterSetRegs>(*_node);
     HVoltThtSetReg[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
@@ -996,6 +1120,8 @@ void AP_UAVCAN::loop(void)
 			stm32_rboot = 1;
 			//高压电调消除故障
 			HVreset = 7;	
+			uavcan::equipment::esc::Clear Clearesg;//高压电调参数设置
+			clearFault[_driver_index]->broadcast(Clearesg);//高压电调读取或者设置参数
 		}
 		
 
@@ -1139,13 +1265,7 @@ 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]);
-					
-			}
+
 
 
 			

libraries/AP_UAVCAN/HVcallback.H

@@ -17,7 +17,9 @@
   {
 	  int16_t voltage;
 	  int32_t speed;
+	  int16_t currentlimit;
 	  uint32_t fault;
+	  int16_t Imax;
 	  int16_t speedref;
 	  int16_t torqueIQ;
 	  int16_t temperature;

libraries/AP_UAVCAN/HVcallback.cpp

@@ -22,13 +22,15 @@ void HVcycle(uint8_t _code,uint8_t _buffer[6],HVmes &Thruster){
 			case 0x03:
 				{
 					Thruster.voltage = _buffer[0]+(_buffer[1]<<8);
-					Thruster.speed = (int16_t)(_buffer[2]+(_buffer[3]<<8) + (_buffer[4]<<16) + (_buffer[5]<<24));
+					Thruster.speed = (int16_t)(_buffer[2]+(_buffer[3]<<8)) ;//+ (_buffer[4]<<16) + (_buffer[5]<<24));
+					Thruster.currentlimit = (int16_t)(_buffer[4]+(_buffer[5]<<8));
 					
 				}
 				break;
 			case 0x05:
 				{
-					Thruster.fault = _buffer[0]+(_buffer[1]<<8) + (_buffer[2]<<16)+(_buffer[3]<<24);
+					Thruster.fault =  (int16_t)_buffer[0]+( (int16_t)_buffer[1]<<8);// + (_buffer[2]<<16)+(_buffer[3]<<24);
+					Thruster.Imax = (int16_t)_buffer[2]+( (int16_t)_buffer[3]<<8);
 					Thruster.speedref = (int16_t)( _buffer[4] + (_buffer[5]<<8));
 					
 				}

modules/uavcan/dsdl/uavcan/equipment/esc/20049.Clear.uavcan

@@ -0,0 +1 @@
+# clear

modules/uavcan/libuavcan/dsdl_compiler/pyuavcan/dsdl/uavcan/equipment/esc/20049.Clear.uavcan

@@ -0,0 +1 @@
+# clear