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