|
@@ -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]);
|
|
|
-
|
|
|
- }
|
|
|
+
|
|
|
|
|
|
|
|
|
|