#include "soft_water_device.h" #include "common.h" #include "string.h" #include "soft_p_2_c.h" #include "soft_flow.h" #include "soft_seed_device.h" #include "soft_crc.h" #include "soft_version.h" /** * @file liquid_recieved_hookfuction * @brief 液位计解析 * @param * @details * @author Zhang Sir **/ liquid liquid_inf; short liquid_aver_value[20] = {0}; uint8_t liquid_aver_i = 0; bool liquid_opening = false; //初始化 int liquid_aver_time = 0; void liquid_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t len) { Dev.Checklow_Link.connect_status = COMP_NORMAL; Dev.Checklow_Link.recv_time = HAL_GetTick(); memcpy(&liquid_inf.liquid_percent,&data[0],3); //俯仰小于8度 if((abs(planep.roll_angle / 100.0f) < 8) && (abs(planep.pitch_angle / 100.0f) < 8) && HAL_GetTick() - liquid_aver_time > 1000) { liquid_aver_value[liquid_aver_i] = liquid_inf.liquid_percent; liquid_aver_i++; if(liquid_aver_i > 19 ) { liquid_opening = true; liquid_aver_i = 0; } } else { liquid_aver_time = HAL_GetTick(); } } /** * @file decode_liquid_info * @brief 解析液位计 * @param * @details * @author Zhang Sir **/ int decode_liquid_info(void) { Check_dev_link(&Dev.Checklow_Link,5000,(char *)&liquid_inf.liquid_percent,sizeof(liquid)); if(Dev.Checklow_Link.connect_status == COMP_NORMAL) { int liquid_temp = 0; //上电没滤波直接显示 if(liquid_opening == false) { return liquid_inf.liquid_percent; } for(uint8_t i = 0; i < 20; i++) { liquid_temp += liquid_aver_value[i]; } return liquid_temp / 20; } else { return 0; } } /** * @file Flow_recieved_hookfuction * @brief 流量计解析 * @param * @details * @author Zhang Sir **/ flow flow_inf; void Flow_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t len) { switch (cellCanID) { case 0x2012C0: flow_inf.ch = data[0]; switch (flow_inf.ch) { case 1: memcpy(&flow_inf.ch1.speed,&data[1],5); if(flow_inf.ch1.last_ml > flow_inf.ch1.ml) { flow_inf.ch1.overturn++; } flow_inf.ch1.last_ml = flow_inf.ch1.ml; Dev.Flow_Link1.connect_status = COMP_NORMAL; Dev.Flow_Link1.recv_time = HAL_GetTick(); break; case 2: memcpy(&flow_inf.ch2.speed,&data[1],5); if(flow_inf.ch2.last_ml > flow_inf.ch2.ml) { flow_inf.ch2.overturn++; } flow_inf.ch2.last_ml = flow_inf.ch2.ml; Dev.Flow_Link2.connect_status = COMP_NORMAL; Dev.Flow_Link2.recv_time = HAL_GetTick(); break; default: break; } break; case 0x2812C0: switch (data[0]) { case 0xF1: if(data[1]== 0x01) flow_inf.ch1.k = data[2] + ((data[3] << 8) & 0xff00); if(data[4]== 0x02) flow_inf.ch2.k = data[5] + ((data[6] << 8) & 0xff00); pmu_set_ack(_MSGID_SET,MSGID_SET_MIMO_FLOW,flow_inf.ch1.k,flow_inf.ch2.k); flow_inf.ch1.set_k = false; break; case 0XF2: if(data[1]== 0x01) flow_inf.ch1.k = data[2] + ((data[3] << 8) & 0xff00); if(data[4]== 0x02) flow_inf.ch2.k = data[5] + ((data[6] << 8) & 0xff00); if(flow_inf.ch2.k != 0) //多判断一次，CAN通讯质量不高可能获取不到厂家信息 { Dev.Flow.facid = FAC_QIFEI_DOU; } flow_inf.ch1.get_k = true; break; case 0XF3: flow_inf.ch1.k_multiple = data[1] + ((data[2] << 8) & 0xff00); flow_inf.ch2.k_multiple = data[1] + ((data[2] << 8) & 0xff00); break; case 0XF7: if(data[1] == 'Q' && data[2] == 'F') { Dev.Flow.facid = FAC_QIFEI_SIG; if(Dev.Flow_Link2.connect_status == COMP_NORMAL) { Dev.Flow.facid = FAC_QIFEI_DOU; } } break; default: break; } break; default: break; } } /** * @file distributor_recieved_hookfuction * @brief 解析分电板信息 * @param * @details * @author Zhang Sir **/ void distributor_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t len) { memcpy(&distributor.arm_sensor1,&data[0],2); Distributor_Link.connect_status = COMP_NORMAL; Distributor_Link.recv_time = HAL_GetTick(); } /** * @file Can_sendmsg_to_distributor * @brief 发送信息给分电板 * @param * @details * @author Zhang Sir **/ distributor_info distributor = {.nozzle1 = 1000, .nozzle2 = 1000}; Connect_check Distributor_Link; void Can_sendmsg_to_distributor(void) { if(Distributor_Link.connect_status != COMP_NORMAL) return; uint8_t can_buf[8] = {0}; can_buf[0] = distributor.nozzle1 & 0xff; can_buf[1] = (distributor.nozzle1 >> 8) & 0xff; can_buf[2] = distributor.nozzle2 & 0xff; can_buf[3] = (distributor.nozzle2 >> 8) & 0xff; can_send_msg_normal(can_buf,8,0x81422); } /** * @file can_sendmsg_flow * @brief 发送信息到流量计 * @param * @details * @author Zhang Sir **/ void can_sendmsg_flow(void) { uint8_t can_buf[8] = {0}; //获取厂商ID if(Dev.Flow_Link1.connect_status == COMP_NORMAL && Dev.Flow.facid == 0xff) { can_buf[0] = 0XF7; can_buf[7] = 0XFE; can_send_msg_normal(can_buf,8,0x216020); } //上电读取流量计K值 和 系数单位 else if(Dev.Flow_Link1.connect_status == COMP_NORMAL && (Dev.Flow.facid == FAC_VK || Dev.Flow.facid == FAC_QIFEI_SIG ||Dev.Flow.facid == FAC_QIFEI_DOU) && flow_inf.ch1.get_k == false) { can_buf[0] = 0XF2; can_buf[7] = 0XFE; can_send_msg_normal(can_buf,8,0x216020); HAL_Delay(100); can_buf[0] = 0XF3; can_buf[7] = 0XFE; can_send_msg_normal(can_buf,8,0x216020); } else if (Dev.Flow_Link1.connect_status == COMP_NORMAL && (Dev.Flow.facid == FAC_VK || Dev.Flow.facid == FAC_QIFEI_SIG ||Dev.Flow.facid == FAC_QIFEI_DOU) && flow_inf.ch1.set_k == true) { can_buf[0] = 0XF1; can_buf[1] = 0x1; can_buf[5] = flow_inf.ch1.cal_k; can_buf[6] = (flow_inf.ch1.cal_k >> 8) & 0xff; //12做个调换 can_buf[4] = 0x2; can_buf[2] = flow_inf.ch2.cal_k; can_buf[3] = (flow_inf.ch2.cal_k >> 8) & 0xff; can_buf[7] = 0XFE; can_send_msg_normal(can_buf,8,0x216020); } } /** * @file HW_CanSetESCInfomation * @brief HW设置类信息 * @param * @details * @author Zhang Sir **/ bool set_HWesc_ESCid = false; HWTail SetEscidTail = {0}; uint16_t curNodeID = 1; //当前能接收到的ESC的id _setESCidInfo setESCidInfo = {.reTxMaxCnt = 5}; void HW_CanSetESCInfomation( void ) { uint8_t send_can_buf[8] = {0}; uint32_t canID = 0; if ( set_HWesc_ESCid == true ) { //超时退出.任何阶段配置失败是否要执行退出配置模式？ if ( setESCidInfo.reTxMaxCnt <= 0 && setESCidInfo.setESCidStep != SETESCID_EXIT ) { setESCidInfo.setESCidStep = SETESCID_EXIT; setESCidInfo.reTxMaxCnt = 5; set_HWesc_ESCid = false; //继续配置2号水泵 if(setESCidInfo.setESCidNum == 1) { setESCidInfo.setESCidNum++; setESCidInfo.setESCidStep = SETESCID_INIT; set_HWesc_ESCid = true; } else if(setESCidInfo.setESCidNum == 2) { //设置成功，发送ACK if(Dev.L_pump1_Link.connect_status == COMP_NORMAL && Dev.L_pump2_Link.connect_status == COMP_NORMAL) { pmu_set_ack(_MSGID_SET,MSGID_SET_PUMP_ID,3,1); } else if (Dev.L_pump1_Link.connect_status == COMP_NORMAL) { pmu_set_ack(_MSGID_SET,MSGID_SET_PUMP_ID,1,1); } else if (Dev.L_pump2_Link.connect_status == COMP_NORMAL) { pmu_set_ack(_MSGID_SET,MSGID_SET_PUMP_ID,2,1); } } } // 初始化并清零现场 if ( setESCidInfo.setESCidStep == SETESCID_INIT ) { setESCidInfo.getSetIDModeRes = false; setESCidInfo.reTxMaxCnt = 5; setESCidInfo.getSetIDModeResCnt = 0; if ( setESCidInfo.setESCidNum > 0 && setESCidInfo.setESCidNum <= 4 ) { EscMsg[setESCidInfo.setESCidNum].ResSetID = false; EscMsg[setESCidInfo.setESCidNum].ResSetIDModeExit = false; setESCidInfo.setESCidStep++; } else { set_HWesc_ESCid = false; } } // 进入配置模式 else if ( setESCidInfo.setESCidStep == SETESCID_ENTER ) { if ( setESCidInfo.getSetIDModeRes == false ) { //广播帧 canID = ( ( HW_PRI_MEDIUM << HW_CANID_PRI_POS ) & HW_CANID_PRI_MASK ) | ( ( HW_FRAME_MSG_SET_ID_MODE << HW_CANID_MSGID_POS ) & HW_CANID_MSGID_MASK ) | ( ( HW_TYPE_MSG << HW_CANID_SNM_POS ) & HW_CANID_SNM_MASK ) | ( ( HW_NODE_BDC << HW_CANID_SRCNODE_POS )& HW_CANID_SRCNODE_MASK ); send_can_buf[0] = HW_SETIDMODE_MODE2; send_can_buf[1] = HW_SETIDMODE_CONTROL1; SetEscidTail.HWTailBit.start = 1; SetEscidTail.HWTailBit.end = 1; SetEscidTail.HWTailBit.toggle = 0; send_can_buf[2] = SetEscidTail.HWTailByte; can_send_msg_normal(send_can_buf,3,canID); SetEscidTail.HWTailBit.tranid++; setESCidInfo.reTxMaxCnt--; } else { setESCidInfo.setESCidStep++; setESCidInfo.reTxMaxCnt = 5; setESCidInfo.setEscIDDelayTime = HAL_GetTick(); } } //配合油门，发送设置ID指令（要配置ID的通道PWM给1000，其余通道失能PWM输出） else if ( setESCidInfo.setESCidStep == SETESCID_SET ) { //配置信息 //EscSet.send_type2_canid[0] = 1; //EscSet.send_type2_canid[1] = 1; //setESCidInfo.setESCidNum = 2; //配置ID属于伪服务帧，不关心DESnode canID = ( ( HW_PRI_MEDIUM << HW_CANID_PRI_POS ) & HW_CANID_PRI_MASK ) | ( ( HW_SETID_ID << HW_CANID_SERID_POS ) & HW_CANID_SERID_MASK ) | ( ( HW_TYPE_REQ << HW_CANID_RNR_POS ) & HW_CANID_RNR_MASK ) | ( ( curNodeID << HW_CANID_DESNODE_POS ) & HW_CANID_DESNODE_MASK ) | ( ( HW_TYPE_SER << HW_CANID_SNM_POS ) & HW_CANID_SNM_MASK ) | ( ( HW_NODE_BDC << HW_CANID_SRCNODE_POS )& HW_CANID_SRCNODE_MASK ); //等待电调的油门信号失能判定时间300ms，每次配置一个电调 if ( HAL_GetTick() - setESCidInfo.setEscIDDelayTime > 2000 ) { if ( EscMsg[setESCidInfo.setESCidNum].ResSetID == false ) { send_can_buf[0] = setESCidInfo.setESCidNum; send_can_buf[1] = setESCidInfo.setESCidNum; SetEscidTail.HWTailBit.start = 1; SetEscidTail.HWTailBit.end = 1; SetEscidTail.HWTailBit.toggle = 0; send_can_buf[2] = SetEscidTail.HWTailByte; can_send_msg_normal(send_can_buf,3,canID); SetEscidTail.HWTailBit.tranid++; setESCidInfo.reTxMaxCnt--; } else { setESCidInfo.setESCidStep++; setESCidInfo.reTxMaxCnt = 5; } } } //nodeID配置完成/失败后退出配置模式，广播发送，全部回复 else if ( setESCidInfo.setESCidStep == SETESCID_EXIT ) { if ( EscMsg[setESCidInfo.setESCidNum].ResSetIDModeExit == false ) { //广播帧 canID = ( ( HW_PRI_MEDIUM << HW_CANID_PRI_POS ) & HW_CANID_PRI_MASK ) | ( ( HW_FRAME_MSG_SET_ID_MODE << HW_CANID_MSGID_POS ) & HW_CANID_MSGID_MASK ) | ( ( HW_TYPE_MSG << HW_CANID_SNM_POS ) & HW_CANID_SNM_MASK ) | ( ( HW_NODE_BDC << HW_CANID_SRCNODE_POS )& HW_CANID_SRCNODE_MASK ); send_can_buf[0] = HW_SETIDMODE_MODE1; send_can_buf[1] = HW_SETIDMODE_CONTROL1; SetEscidTail.HWTailBit.start = 1; SetEscidTail.HWTailBit.end = 1; SetEscidTail.HWTailBit.toggle = 0; send_can_buf[2] = SetEscidTail.HWTailByte; can_send_msg_normal(send_can_buf,3,canID); SetEscidTail.HWTailBit.tranid++; setESCidInfo.reTxMaxCnt--; } else { //广播模式，总线上有其他同NODEID的ESC时也会收到反馈 set_HWesc_ESCid = false; setESCidInfo.setESCidStep++; setESCidInfo.reTxMaxCnt = 5; //继续配置2号水泵 if(setESCidInfo.setESCidNum == 1) { setESCidInfo.setESCidNum++; setESCidInfo.setESCidStep = SETESCID_INIT; set_HWesc_ESCid = true; } else if(setESCidInfo.setESCidNum == 2) { //设置成功，发送ACK if(Dev.L_pump1_Link.connect_status == COMP_NORMAL && Dev.L_pump2_Link.connect_status == COMP_NORMAL) { pmu_set_ack(_MSGID_SET,MSGID_SET_PUMP_ID,3,1); } else if (Dev.L_pump1_Link.connect_status == COMP_NORMAL) { pmu_set_ack(_MSGID_SET,MSGID_SET_PUMP_ID,1,1); } else if (Dev.L_pump2_Link.connect_status == COMP_NORMAL) { pmu_set_ack(_MSGID_SET,MSGID_SET_PUMP_ID,2,1); } } } //超时退出 if ( setESCidInfo.reTxMaxCnt <= 0 ) { set_HWesc_ESCid = false; //继续配置2号水泵 if(setESCidInfo.setESCidNum == 1) { setESCidInfo.setESCidNum++; setESCidInfo.setESCidStep = SETESCID_INIT; set_HWesc_ESCid = true; } else if(setESCidInfo.setESCidNum == 2) { //设置成功，发送ACK if(Dev.L_pump1_Link.connect_status == COMP_NORMAL && Dev.L_pump2_Link.connect_status == COMP_NORMAL) { pmu_set_ack(_MSGID_SET,MSGID_SET_PUMP_ID,3,1); } else if (Dev.L_pump1_Link.connect_status == COMP_NORMAL) { pmu_set_ack(_MSGID_SET,MSGID_SET_PUMP_ID,1,1); } else if (Dev.L_pump2_Link.connect_status == COMP_NORMAL) { pmu_set_ack(_MSGID_SET,MSGID_SET_PUMP_ID,2,1); } } setESCidInfo.setESCidStep = SETESCID_INIT; setESCidInfo.reTxMaxCnt = 5; } } } } float canardConvertFloat16ToNativeFloat(uint16_t value) { union FP32 { uint32_t u; float f; }; const union FP32 magic = { (254UL - 15UL) << 23U }; const union FP32 was_inf_nan = { (127UL + 16UL) << 23U }; union FP32 out; out.u = (value & 0x7FFFU) << 13U; out.f *= magic.f; if (out.f >= was_inf_nan.f) { out.u |= 255UL << 23U; } out.u |= (value & 0x8000UL) << 16U; return out.f; } /** * @file uavcan_equipment_power_BatteryInfo_decode * @brief CAN解析电流传感器 * @param * @details * @author Zhang Sir **/ struct uavcan_equipment_power_BatteryInfo power_BatteryInfo; uint8_t batteryInfoBuf[UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MAX_SIZE] = {0}; void uavcan_equipment_power_BatteryInfo_decode(uint8_t *recv_buf, uint32_t len, uint32_t id) { static uint32_t BufIndex = 0; static bool BufGetS = false; Dev.Current_Link.connect_status = COMP_NORMAL; Dev.Current_Link.recv_time = HAL_GetTick(); HWTail revTail = {0}; if ( len > 0 && len <= 8 ) revTail.HWTailByte = recv_buf[len - 1]; if ( revTail.HWTailBit.start == 1 ) { BufIndex = 0; BufGetS = true; } if ( BufGetS == true && BufIndex + len - 1 <= sizeof( batteryInfoBuf ) ) { memcpy( &batteryInfoBuf[BufIndex], recv_buf, len - 1 ); BufIndex += ( len - 1 ); } else { BufIndex = 0; BufGetS = false; } if ( BufGetS == true && revTail.HWTailBit.end == 1 ) { uint16_t sigcrc = crcAddSignature( HW_CRC_INITVALUE, UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE ); uint16_t crc = crcAdd( sigcrc, &batteryInfoBuf[2], BufIndex - 2 ); uint16_t recvCRC = ( batteryInfoBuf[1] << 8 ) + batteryInfoBuf[0]; if ( crc == recvCRC ) { uint16_t float16_val = 0; float16_val = batteryInfoBuf[2] + batteryInfoBuf[3]*256; power_BatteryInfo.temperature = canardConvertFloat16ToNativeFloat(float16_val); float16_val = batteryInfoBuf[4] + batteryInfoBuf[5]*256; power_BatteryInfo.voltage = canardConvertFloat16ToNativeFloat(float16_val); float16_val = batteryInfoBuf[6] + batteryInfoBuf[7]*256; power_BatteryInfo.current = canardConvertFloat16ToNativeFloat(float16_val); regist_dev_info(&dev_current,DEVICE_CURRENT,false,NULL,0,NULL,0,NULL,10,"current",8); } } } /** * @file HobbywingCanRecvHookFunction * @brief CAN解析好盈协议 * @param * @details * @author Zhang Sir **/ HWTail rawcmdTail = {0}; HWTail setIDTail = {0}; _EscLinkDevice EscMsg[4 + 1]; void HobbywingCanRecvHookFunction(uint32_t id, uint8_t *recv_buf, uint8_t len) { //去掉NODEID uint32_t Can_filte_id = id & 0x1FFFFF00; if(Can_filte_id == 0x1F4E5200 || Can_filte_id == 0x1F4E5300 || Can_filte_id == 0x1F4E5400|| Can_filte_id == 0x004E2E00 || Can_filte_id == 0x1000D200 || Can_filte_id == 0x004E2D00|| Can_filte_id == 0x18044400 || Can_filte_id ==0x00D21000 ){} else{return;} uint16_t TypeID = 0; uint8_t SerOrMsg = ( id & HW_CANID_SNM_MASK ) >> HW_CANID_SNM_POS; uint8_t SrcNodeID = ( id & HW_CANID_SRCNODE_MASK ) >> HW_CANID_SRCNODE_POS; if ( SerOrMsg == HW_TYPE_SER ) TypeID = ( id & HW_CANID_SERID_MASK ) >> HW_CANID_SERID_POS; else TypeID = ( id & HW_CANID_MSGID_MASK ) >> HW_CANID_MSGID_POS; switch ( TypeID ) { //单独解析HW_FRAME_MSG_SET_ID_MODE的反馈，进入设置时电调的自身节点不确定。不一定在1~8之间。 case HW_FRAME_MSG_SET_ID_MODE: { uint8_t _mode = recv_buf[0]; uint8_t _control = recv_buf[1]; //进入配置ID模式 if ( _mode == HW_SETIDMODE_MODE2 && _control == HW_SETIDMODE_CONTROL1 ) { setESCidInfo.getSetIDModeRes = true; setESCidInfo.getSetIDModeResCnt++; } } break; case HW_FRAME_MSG_GET_ESC_ID: //hw_canesc_link_status = COMP_NORMAL; break; case UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID: uavcan_equipment_power_BatteryInfo_decode(recv_buf, len, id); break; } _EscLinkDevice *EscPtr = NULL; if ( SrcNodeID >= 1 && SrcNodeID <= 4 ) { EscPtr = &EscMsg[SrcNodeID]; } else if (SrcNodeID == 16 || SrcNodeID == 1) { SrcNodeID = 1; EscPtr = &EscMsg[SrcNodeID]; } if ( EscPtr != NULL ) { if(SrcNodeID == 1) { Dev.L_pump1_Link.connect_status = COMP_NORMAL; Dev.L_pump1_Link.recv_time = HAL_GetTick(); Dev.L_pump1.facid = FAC_HW_ESC; } else if(SrcNodeID == 2) { Dev.L_pump2_Link.connect_status = COMP_NORMAL; Dev.L_pump2_Link.recv_time = HAL_GetTick(); Dev.L_pump2.facid = FAC_HW_ESC; } HWTail revTail = {0}; if ( len > 0 && len <= 8 ) revTail.HWTailByte = recv_buf[len - 1]; switch ( TypeID ) { //esc自动上报 MSG1 2 3 case HW_MSG1_ID: EscPtr->motorRPM = ( recv_buf[1] << 8 ) + recv_buf[0]; EscPtr->pwm_value = ( recv_buf[3] << 8 ) + recv_buf[2]; EscPtr->warn_flag = ( recv_buf[5] << 8 ) + recv_buf[4]; break; case HW_MSG2_ID: EscPtr->esc_voltage = ( ( recv_buf[1] << 8 ) + recv_buf[0] ) / 10.0f; EscPtr->esc_current = ( ( recv_buf[3] << 8 ) + recv_buf[2] ) / 10.0f; EscPtr->esc_temperature = recv_buf[4]; break; case HW_MSG3_ID: EscPtr->esc_mosTemp = recv_buf[0]; EscPtr->esc_capTemp = recv_buf[1]; EscPtr->esc_motorTemp = recv_buf[2]; break; case HW_MSGCONTROL_ID: EscPtr->MsgCtrOpt = recv_buf[0]; memcpy( &EscPtr->MsgCtrCmd, &recv_buf[1], 4 ); break; case HW_FRAME_MSG_GET_ESC_ID: EscPtr->GetESCid = true; EscPtr->GetEscNodeID = recv_buf[0]; EscPtr->GetEscThrCh = recv_buf[1]; break; case HW_FRAME_MSG_SET_ID_MODE: //退出配置ID模式 if ( recv_buf[0] == 0x55 && recv_buf[1] == 0x55 ) EscPtr->ResSetIDModeExit = true; break; case HW_SETID_ID: EscPtr->ResSetID = true; EscPtr->SetIDNodeID = recv_buf[0]; EscPtr->SetIDThrCh = recv_buf[1]; break; case HW_SETBAUD_ID: EscPtr->SetBaud = recv_buf[0]; break; case HW_SETLED_ID: EscPtr->SetLEDOpt = recv_buf[0]; EscPtr->SetLEDClr = recv_buf[1]; EscPtr->SetLEDBlk = recv_buf[2]; break; case HW_SETROTATION_ID: EscPtr->SetRotDir = recv_buf[0]; break; case HW_SETGET_FREQ_ID: EscPtr->SetFreqOpt = recv_buf[0]; EscPtr->SetFreqID = ( recv_buf[2] << 8 ) + recv_buf[1]; EscPtr->SetFreqFreq = recv_buf[3]; break; case HW_THRSELEXT_ID: EscPtr->ThrSrc = recv_buf[0]; break; case HW_SELFTEST_ID: EscPtr->SelfCheckSta = recv_buf[0]; break; case HW_ESCINFO_ID: if ( revTail.HWTailBit.start == 1 ) { EscPtr->_VerBufIndex = 0; EscPtr->_VerBufGetS = true; } if ( EscPtr->_VerBufGetS == true && EscPtr->_VerBufIndex + len - 1 < sizeof( EscPtr->_VerBuf ) ) { memcpy( &EscPtr->_VerBuf[EscPtr->_VerBufIndex], recv_buf, len - 1 ); EscPtr->_VerBufIndex += ( len - 1 ); } else { EscPtr->_VerBufIndex = 0; EscPtr->_VerBufGetS = false; } if ( EscPtr->_VerBufGetS == true && revTail.HWTailBit.end == 1 ) { uint16_t sigcrc = crcAddSignature( HW_CRC_INITVALUE, HW_ESCINFO_SIG ); uint16_t crc = crcAdd( sigcrc, &EscPtr->_VerBuf[2], EscPtr->_VerBufIndex - 2 ); uint16_t recvCRC = ( EscPtr->_VerBuf[1] << 8 ) + EscPtr->_VerBuf[0]; if ( crc == recvCRC ) { uint8_t ESCinfoOpt = EscPtr->_VerBuf[2]; switch ( ESCinfoOpt ) { case HW_ESCINFO_OPT00: EscPtr->GetESCInfoOpt00 = true; memcpy( EscPtr->LinkSoftVer, &EscPtr->_VerBuf[3], 16 ); memcpy( EscPtr->LinkHardVer, &EscPtr->_VerBuf[19], 16 ); memcpy( EscPtr->LinkDevVer, &EscPtr->_VerBuf[35], 16 ); memcpy( EscPtr->LinkSN, &EscPtr->_VerBuf[51], 8 ); break; case HW_ESCINFO_OPT01: EscPtr->GetESCInfoOpt01 = true; memcpy( EscPtr->DriveSoftVer, &EscPtr->_VerBuf[3], 16 ); memcpy( EscPtr->DriveHardVer, &EscPtr->_VerBuf[19], 16 ); memcpy( EscPtr->DriveDevVer, &EscPtr->_VerBuf[35], 16 ); memcpy( EscPtr->DriveSN, &EscPtr->_VerBuf[51], 8 ); break; case HW_ESCINFO_OPT02: EscPtr->GetESCInfoOpt02 = true; memcpy( EscPtr->HardSN, &EscPtr->_VerBuf[3], 16 ); memcpy( EscPtr->ProtocolVer, &EscPtr->_VerBuf[19], 8 ); break; default: break; } } } break; //设置回应ESC配置信息 case HW_GETMAJORCONF_ID: EscPtr->GetMajorConf = true; EscPtr->MajorConfRot = ( recv_buf[0] >> 7 ) & 0x01; //电机旋转方向(0：顺时针，1：逆时针); EscPtr->MajorConfThrSrc = ( recv_buf[0] >> 6 ) & 0x01; //油门信号源(0：CAN数字油门，1：PWM油门); EscPtr->MajorConfThrCh = recv_buf[0] & 0x3F; //定的数字油门通道; EscPtr->MajorConfLEDBlink = ( recv_buf[1] >> 3 ) & 0x1f; //LED闪烁状态; EscPtr->MajorConfLEDColor = recv_buf[1] & 0x07; //LED静态灯色RGB; EscPtr->MajorConfMSG2UPHZ = ( recv_buf[2] >> 4 ) & 0x0f; //MSG2数据帧上报速率; EscPtr->MajorConfMSG1UPHZ = recv_buf[2] & 0x0f; //MSG1数据帧上报速率; EscPtr->MajorConfInstAngle = recv_buf[3] + ( recv_buf[4] << 8 ); //定位角度数值 break; default: break; } } } /** * @file HW_CanGetESCInfomation * @brief 上电获取好盈版本配置信息 * @param * @details * @author Zhang Sir **/ HWTail GetEscInfoTail = {0}; HWTail GetMInfoTail = {0}; HWTail GetMajorConfTail = {0}; HWTail GetESCidTail = {0}; void HW_CanGetESCInfomation( void ) { // uint8_t send_can_buf[8] = {0}; // uint16_t desnodeID = 0; // uint32_t canID = 0; // if ( Dev.Pump_Link.connect_status == COMP_NORMAL && Dev.Pump.facid == FAC_HW_ESC ) // { // 获取版本信息 // if (EscMsg[desnodeID].GetESCInfoOpt00 != true ) // { // canID = ( ( HW_PRI_MEDIUM << HW_CANID_PRI_POS ) & HW_CANID_PRI_MASK ) | // ( ( HW_ESCINFO_ID << HW_CANID_SERID_POS ) & HW_CANID_SERID_MASK ) | // ( ( HW_TYPE_REQ << HW_CANID_RNR_POS ) & HW_CANID_RNR_MASK ) | // ( ( desnodeID << HW_CANID_DESNODE_POS )& HW_CANID_DESNODE_MASK ) | // ( ( HW_TYPE_SER << HW_CANID_SNM_POS ) & HW_CANID_SNM_MASK ) | // ( ( HW_NODE_BDC << HW_CANID_SRCNODE_POS ) & HW_CANID_SRCNODE_MASK ); // send_can_buf[0] = HW_ESCINFO_OPT00; // GetEscInfoTail.HWTailBit.start = 1; // GetEscInfoTail.HWTailBit.end = 1; // GetEscInfoTail.HWTailBit.toggle = 0; // send_can_buf[1] = GetEscInfoTail.HWTailByte; // can_send_msg_normal(send_can_buf,2,canID); // GetEscInfoTail.HWTailBit.tranid++; // } // else if ( EscMsg[desnodeID].GetESCInfoOpt01 != true ) // { // canID = ( ( HW_PRI_MEDIUM << HW_CANID_PRI_POS ) & HW_CANID_PRI_MASK ) | // ( ( HW_ESCINFO_ID << HW_CANID_SERID_POS ) & HW_CANID_SERID_MASK ) | // ( ( HW_TYPE_REQ << HW_CANID_RNR_POS ) & HW_CANID_RNR_MASK ) | // ( ( desnodeID << HW_CANID_DESNODE_POS )& HW_CANID_DESNODE_MASK ) | // ( ( HW_TYPE_SER << HW_CANID_SNM_POS ) & HW_CANID_SNM_MASK ) | // ( ( HW_NODE_BDC << HW_CANID_SRCNODE_POS ) & HW_CANID_SRCNODE_MASK ); // send_can_buf[0] = HW_ESCINFO_OPT01; // GetEscInfoTail.HWTailBit.start = 1; // GetEscInfoTail.HWTailBit.end = 1; // GetEscInfoTail.HWTailBit.toggle = 0; // send_can_buf[1] = GetEscInfoTail.HWTailByte; // can_send_msg_normal(send_can_buf,2,canID); // GetEscInfoTail.HWTailBit.tranid++; // } // else if (EscMsg[desnodeID].GetESCInfoOpt02 != true ) // { // canID = ( ( HW_PRI_MEDIUM << HW_CANID_PRI_POS ) & HW_CANID_PRI_MASK ) | // ( ( HW_ESCINFO_ID << HW_CANID_SERID_POS ) & HW_CANID_SERID_MASK ) | // ( ( HW_TYPE_REQ << HW_CANID_RNR_POS ) & HW_CANID_RNR_MASK ) | // ( ( desnodeID << HW_CANID_DESNODE_POS )& HW_CANID_DESNODE_MASK ) | // ( ( HW_TYPE_SER << HW_CANID_SNM_POS ) & HW_CANID_SNM_MASK ) | // ( ( HW_NODE_BDC << HW_CANID_SRCNODE_POS ) & HW_CANID_SRCNODE_MASK ); // send_can_buf[0] = HW_ESCINFO_OPT02; // GetEscInfoTail.HWTailBit.start = 1; // GetEscInfoTail.HWTailBit.end = 1; // GetEscInfoTail.HWTailBit.toggle = 0; // send_can_buf[1] = GetEscInfoTail.HWTailByte; // can_send_msg_normal(send_can_buf,2,canID); // GetEscInfoTail.HWTailBit.tranid++; // } // else if ( EscMsg[desnodeID].GetMajorConf != true ) // { // canID = ( ( HW_PRI_MEDIUM << HW_CANID_PRI_POS ) & HW_CANID_PRI_MASK ) | // ( ( HW_GETMAJORCONF_ID << HW_CANID_SERID_POS ) & HW_CANID_SERID_MASK ) | // ( ( HW_TYPE_REQ << HW_CANID_RNR_POS ) & HW_CANID_RNR_MASK ) | // ( ( desnodeID << HW_CANID_DESNODE_POS )& HW_CANID_DESNODE_MASK ) | // ( ( HW_TYPE_SER << HW_CANID_SNM_POS ) & HW_CANID_SNM_MASK ) | // ( ( HW_NODE_BDC << HW_CANID_SRCNODE_POS ) & HW_CANID_SRCNODE_MASK ); // send_can_buf[0] = HW_MAJORCONF_OPT0; // GetMajorConfTail.HWTailBit.start = 1; // GetMajorConfTail.HWTailBit.end = 1; // GetMajorConfTail.HWTailBit.toggle = 0; // send_can_buf[1] = GetMajorConfTail.HWTailByte; // can_send_msg_normal(send_can_buf,2,canID); // GetMajorConfTail.HWTailBit.tranid++; // } // 获取的反馈ID和VK电池有冲突 // if (EscMsg[desnodeID].GetESCid != true ) // { // canID = ( ( HW_PRI_MEDIUM << HW_CANID_PRI_POS ) & HW_CANID_PRI_MASK ) | // ( ( HW_FRAME_MSG_GET_ESC_ID << HW_CANID_MSGID_POS ) & HW_CANID_MSGID_MASK ) | // ( ( HW_TYPE_MSG << HW_CANID_SNM_POS ) & HW_CANID_SNM_MASK ) | // ( ( HW_NODE_BDC << HW_CANID_SRCNODE_POS )& HW_CANID_SRCNODE_MASK ); // send_can_buf[0] = HW_GETESCID_OPT0; // GetESCidTail.HWTailBit.start = 1; // GetESCidTail.HWTailBit.end = 1; // GetESCidTail.HWTailBit.toggle = 0; // send_can_buf[1] = GetESCidTail.HWTailByte; // can_send_msg_normal(send_can_buf,2,canID); // GetESCidTail.HWTailBit.tranid++; // } // } } /** * @file esc_throttle_switch * @brief 将PWM转换成HW油门 * @param * @details * @author Zhang Sir **/ uint8_t send_thr_buf[8] = {0}; void esc_throttle_switch( int16_t *buf) { uint8_t temp_swich_buf[16] = {0}; int16_t escbuf[8] = {0}; for ( short i = 0; i < 4; i++ ) { escbuf[i] = math_cons_i16( ( buf[i] - 1050 ), 0, 900 ) * 8191.0f / 900; } memcpy( temp_swich_buf, escbuf, 8 ); send_thr_buf[0] = temp_swich_buf[0]; send_thr_buf[1] = ( temp_swich_buf[1] << 2 ) | ( ( temp_swich_buf[2] >> 6 ) & 0x03 ); send_thr_buf[2] = ( temp_swich_buf[2] << 2 ) | ( ( temp_swich_buf[3] >> 4 ) & 0x03 ); send_thr_buf[3] = ( temp_swich_buf[3] << 4 ) | ( ( temp_swich_buf[4] >> 4 ) & 0x0f ); send_thr_buf[4] = ( temp_swich_buf[4] << 4 ) | ( ( temp_swich_buf[5] >> 2 ) & 0x0f ); send_thr_buf[5] = ( temp_swich_buf[5] << 6 ) | ( ( temp_swich_buf[6] >> 2 ) & 0x3f ); send_thr_buf[6] = ( temp_swich_buf[6] << 6 ) | ( temp_swich_buf[7] & 0x3f ); } /** * @file can_sendmsg_esc * @brief can发送HW * @param * @details * @author Zhang Sir **/ void can_sendmsg_esc(void) { int16_t pwm_buf[4] = {0}; int canID = ( ( 0x00 << HW_CANID_PRI_POS ) & HW_CANID_PRI_MASK ) | ( ( HW_RAWCOMMAND_ID << HW_CANID_MSGID_POS ) & HW_CANID_MSGID_MASK ) | ( ( HW_TYPE_MSG << HW_CANID_SNM_POS ) & HW_CANID_SNM_MASK ) | ( ( HW_NODE_BDC << HW_CANID_SRCNODE_POS )& HW_CANID_SRCNODE_MASK ); pwm_buf[0] = pmu_pin.pump1; pwm_buf[1] = pmu_pin.pump2; pwm_buf[2] = 0; pwm_buf[3] = 0; esc_throttle_switch(pwm_buf); rawcmdTail.HWTailBit.start = 1; rawcmdTail.HWTailBit.end = 1; rawcmdTail.HWTailBit.toggle = 0; send_thr_buf[7] = rawcmdTail.HWTailByte; can_send_msg_normal(send_thr_buf,8,canID); rawcmdTail.HWTailBit.tranid++; } void Hobbywing_esc_func(void) { static uint32_t time_1hz = 0; if((Dev.L_pump1_Link.connect_status == COMP_NORMAL && Dev.L_pump1.facid== FAC_HW_ESC) || (Dev.L_pump2_Link.connect_status == COMP_NORMAL && Dev.L_pump2.facid== FAC_HW_ESC)) { if(Check_Timer_Ready(&time_1hz,_1_HZ_)) { HW_CanGetESCInfomation(); HW_CanSetESCInfomation(); } if(set_HWesc_ESCid != true) { can_sendmsg_esc(); } } }