V9_PMU302/user_src/soft_water_device.c

#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"
#include "soft_flash.h"
#include "soft_tattu.h"
#include "soft_terrain.h"
#include "soft_obstacle.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);

        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[3];
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 <= 2 )
    {
        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();
        }
    }

}
Z70_tranfer z70_info;
water_dev churn,turntable;
void Get_auger_sowing_mag(uint32_t CanID, uint8_t data[], uint8_t len)
{
    if(CanID == CAN_JIEXING1_ID)
    {
        memcpy(&z70_info, &data[0], len);
        switch (z70_info.dev_type)
        {
         case Z70_Churn:
            churn.rpm = z70_info.rpm;
            churn.error_status = z70_info.warning;
            churn.reserve = (z70_info.reserve1 & 0x01) | ((z70_info.reserve2 & 0x07) << 1);
            //churn.reserve += (1<<(z70_info.reserve2 & 0x07));

            static uint8_t churn_size = 0;
            if(churn_size != z70_info.reserve3 & 0x07)
            {
               churn_size = z70_info.reserve3 & 0x07;
               DM_4DRADARMAG.Packing_auger_size = churn_size;
               DM4Dmsg_send_fmu = true;
            }
             
            Dev.Seed_Link.connect_status = COMP_NORMAL;
            Dev.Seed_Link.recv_time = HAL_GetTick();
            Dev.Seed.facid = FAC_CHURN_SEED;
            break;
        case Z70_Turntable:
            turntable.rpm = z70_info.rpm;
            turntable.error_status = z70_info.warning;

            Dev.Seed_Link.connect_status = COMP_NORMAL;
            Dev.Seed_Link.recv_time = HAL_GetTick();
            Dev.Seed.facid = FAC_CHURN_SEED;
            break;
        default:
            break;
        }
    }
    else if(CanID == CAN_JIEXING2_ID)
    {
        if(data[0] == 0xe6)
        {
            pmu_set_ack(_MSGID_SET,MSGID_SET_CHURN_RESET,0,0);
        }
        else if(data[0] == 0xe7)
        {
            DM_4DRADARMAG.Packing_auger_size = data[1];
            DM4Dmsg_send_fmu = true;
        }
    }

}