EFTZ_PMU/user_src/soft_water.c

#include "soft_water.h"
#include "soft_device.h"
#include "usart_data_handle.h"
#include "string.h"
#include "common.h"
#include "stdlib.h"
#include "soft_can.h"
#include "soft_version.h"
#include "soft_crc.h"
#include "soft_update.h"
#include "soft_seed_weight.h"
#include "config.h"
#include "main_task.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();
    }
}


void Eft_Dev_Ack_recieved_hookfuction(uint32_t CanID, uint8_t data[], uint8_t len)
{

    switch (CanID)
    {
    case Pump1 - 1:
        switch (data[0])
        {
        case 0xF4:
            // if( dev_pump1.regist.sn == false)
            //     regist_dev_info(&dev_pump1,PUMP1,false,(char *)data[1], ,NULL,0,NULL,0,"pump1",5);
            break;
        case 0xF5:
            break;
        case 0xF6:
            break;
        default:
            break;
        }
        break;
    case Pump2 - 1:
        switch (data[0])
        {
        case 0xF4:
            // if( dev_pump2.regist.sn == false)
            //     regist_dev_info(&dev_pump1,PUMP2,false,(char *)data[1], ,NULL,0,NULL,0,"pump2",5);
            break;
        case 0xF5:
            break;
        case 0xF6:
            break;
        default:
            break;
        }
        break;
    case Nozzle1 - 1:
        switch (data[0])
        {
        case 0xF4:
            // if( dev_nozzle1.regist.sn == false)
            //     regist_dev_info(&dev_nozzle1,NOZZLE1,false,(char *)data[1], ,NULL,0,NULL,0,"nozzle1",8);
            break;
        case 0xF5:
            break;
        case 0xF6:
            break;
        default:
            break;
        }
        break;
    case Nozzle2 - 1:
        switch (data[0])
        {
        case 0xF4:
            // if( dev_nozzle2.regist.sn == false)
            //     regist_dev_info(&dev_nozzle2,NOZZLE2,false,(char *)data[1], ,NULL,0,NULL,0,"nozzle2",8);
            break;
        case 0xF5:
            break;
        case 0xF6:
            break;
        default:
            break;
        }
        break;
    case Nozzle3 - 1:
        switch (data[0])
        {
        case 0xF4:
            // if( dev_nozzle3.regist.sn == false)
            //     regist_dev_info(&dev_nozzle3,NOZZLE3,false,(char *)data[1], ,NULL,0,NULL,0,"nozzle3",8);
            break;
        case 0xF5:
            break;
        case 0xF6:
            break;
        default:
            break;
        }
        break;
    case Nozzle4 - 1:
        switch (data[0])
        {
        case 0xF4:
            // if( dev_nozzle4.regist.sn == false)
            //     regist_dev_info(&dev_nozzle4,NOZZLE4,false,(char *)data[1], ,NULL,0,NULL,0,"nozzle4",8);
            break;
        case 0xF5:
            break;
        case 0xF6:
            break;
        default:
            break;
        }
        break;
    default:
        break;
    }
}

/**
  * @file    Eft_Pump_Nozzle_recieved_hookfuction
  * @brief   eft水泵 离心喷头
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
water_dev pump1,pump2;
water_dev pump2;
water_dev nozzle1,nozzle2,nozzle3,nozzle4,churn,turntable;

void Eft_Pump_Nozzle_recieved_hookfuction(uint32_t CanID, uint8_t data[], uint8_t len)
{
    switch (CanID)
    {
    case 0x9527:
        pump1.rpm = data[0] + data[1] *256;
        pump2.rpm = data[2] + data[3] *256;
        
        Dev.Pump_Link.connect_status = COMP_NORMAL;
        Dev.Pump.facid = FAC_EFT;
        Dev.Pump_Link.recv_time = HAL_GetTick();
        break;
    case Pump1:
        memcpy(&pump1,&data[0],sizeof(water_dev));

        Dev.Eft_CanDev_Link[0].connect_status = COMP_NORMAL;

        Dev.L_pump1_Link.connect_status = COMP_NORMAL;
        Dev.L_pump1.facid = FAC_EFT;
        Dev.L_pump1_Link.recv_time = HAL_GetTick();

        if(set_eftdev_canid_flag == true)
            set_eftdev_canid_status = set_eftdev_canid_status | 1;
        break;
    case Pump2:
        memcpy(&pump2,&data[0],sizeof(water_dev));

        Dev.Eft_CanDev_Link[1].connect_status = COMP_NORMAL;

        Dev.L_pump2_Link.connect_status = COMP_NORMAL;
        Dev.L_pump2.facid = FAC_EFT;
        Dev.L_pump2_Link.recv_time = HAL_GetTick();
        

        if(set_eftdev_canid_flag == true)
            set_eftdev_canid_status = set_eftdev_canid_status | (1 << 1);
        break;
    case Nozzle1:

        Dev.Nozzle_Link.connect_status = COMP_NORMAL;
        Dev.Nozzle.facid = FAC_EFT;
        Dev.Nozzle_Link.recv_time = HAL_GetTick();

        memcpy(&nozzle1,&data[0],sizeof(water_dev));

        Dev.Eft_CanDev_Link[2].connect_status = COMP_NORMAL;
        Dev.Eft_CanDev_Link[2].recv_time = HAL_GetTick();

        if(set_eftdev_canid_flag == true)
            set_eftdev_canid_status = set_eftdev_canid_status | (1 << 4);
        break;
    case Nozzle2:
        Dev.Nozzle_Link.connect_status = COMP_NORMAL;
        Dev.Nozzle.facid = FAC_EFT;
        Dev.Nozzle_Link.recv_time = HAL_GetTick();

        memcpy(&nozzle2,&data[0],sizeof(water_dev));

        Dev.Eft_CanDev_Link[3].connect_status = COMP_NORMAL;
        Dev.Eft_CanDev_Link[3].recv_time = HAL_GetTick();
        if(set_eftdev_canid_flag == true)
            set_eftdev_canid_status = set_eftdev_canid_status | (1 << 5);
        break;
    case Nozzle3:
        Dev.Nozzle_Link.connect_status = COMP_NORMAL;
        Dev.Nozzle.facid = FAC_EFT;
        Dev.Nozzle_Link.recv_time = HAL_GetTick();

        memcpy(&nozzle3,&data[0],sizeof(water_dev));

        Dev.Eft_CanDev_Link[4].connect_status = COMP_NORMAL;
        Dev.Eft_CanDev_Link[4].recv_time = HAL_GetTick();
        if(set_eftdev_canid_flag == true)
            set_eftdev_canid_status = set_eftdev_canid_status | (1 << 6);
        break;
    case Nozzle4:
        Dev.Nozzle_Link.connect_status = COMP_NORMAL;
        Dev.Nozzle.facid = FAC_EFT;
        Dev.Nozzle_Link.recv_time = HAL_GetTick();

        Dev.Eft_CanDev_Link[5].connect_status = COMP_NORMAL;
        Dev.Eft_CanDev_Link[5].recv_time = HAL_GetTick();
        memcpy(&nozzle4,&data[0],sizeof(water_dev));
        if(set_eftdev_canid_flag == true)
            set_eftdev_canid_status = set_eftdev_canid_status | (1 << 7);
        break;
    //升级与版本信息回馈
    case Pump1 + 1:
        if(eft_update.update_flag == true)
            eft_dev_update_wait_ereseflash(data);
        break;
    case Pump1 + 2:
        if(data[5] == 0xF1 && data[6] == 0xF3)
        {
            switch (data[0])
            {
            case 0XF4:
                if(data[5] == 0xF1 && data[6] == 0xF3)
                {
                    for(uint8_t i = 0;i < 3;i++)
                    {
                        dev_pump1.sn[2 * i] = ((data[2+i] >> 4) & 0xf) + '0';
                        dev_pump1.sn[2 * i + 1] = (data[2+i] & 0xf )+ '0';
                    }
                    dev_pump1.sn[6] = ((data[7] >> 4) & 0xf) + '0';
                    dev_pump1.sn[7] = (data[7] & 0xf )+ '0';

                    regist_dev_info(&dev_pump1,DEVICE_LPUMP1,false,dev_pump1.sn,8,NULL,0,NULL,0,"eftpump1",9);
                }
                break;
            case 0XF5:
                for (uint8_t i = 1; i < 5; i++)
                {
                    if(data[i] < 10)
                    {
                        dev_pump1.soft_serial[2 * (i - 1)] = '0';
                        Int2String(data[i],&dev_pump1.soft_serial[2 * (i - 1) + 1],1);
                    }
                    else
                    {
                        Int2String(data[i],&dev_pump1.soft_serial[ (i - 1) * 2],2);
                    }

                }
                regist_dev_info(&dev_pump1,DEVICE_LPUMP1,false,NULL,0,dev_pump1.soft_serial,8,NULL,0,"eftpump1",9);
                break;
            case 0XF6:
                Dev.Pump.runtime1 = (data[1] + (data[2] + data[3] * 256) * 60) / 10;
                p1_runing_time = true;
                break;
            default:
                break;
            }
        }
        break;
    case Pump2 + 1:
        if(eft_update.update_flag == true)
            eft_dev_update_wait_ereseflash(data);
        break;
    case Pump2 + 2:
        if(data[5] == 0xF1 && data[6] == 0xF3)
        {
            switch (data[0])
            {
            case 0XF4:
                if(data[5] == 0xF1 && data[6] == 0xF3)
                {
                    for(uint8_t i = 0;i < 3;i++)
                    {
                        dev_pump2.sn[2 * i] = ((data[2+i] >> 4) & 0xf) + '0';
                        dev_pump2.sn[2 * i + 1] = (data[2+i] & 0xf )+ '0';
                    }
                    dev_pump2.sn[6] = ((data[7] >> 4) & 0xf) + '0';
                    dev_pump2.sn[7] = (data[7] & 0xf )+ '0';

                    regist_dev_info(&dev_pump2,DEVICE_LPUMP2,false,dev_pump2.sn,8,NULL,0,NULL,0,"eftpump2",9);
                }
                break;
            case 0XF5:
                for (uint8_t i = 1; i < 5; i++)
                {
                    if(data[i] < 10)
                    {
                        dev_pump2.soft_serial[2 * (i - 1)] = '0';
                        Int2String(data[i],&dev_pump2.soft_serial[2 * (i - 1) + 1],1);
                    }
                    else
                    {
                        Int2String(data[i],&dev_pump2.soft_serial[ (i - 1) * 2],2);
                    }

                }
                regist_dev_info(&dev_pump2,DEVICE_LPUMP2,false,NULL,0,dev_pump2.soft_serial,8,NULL,0,"eftpump2",9);
                break;
            case 0XF6:
                Dev.Pump.runtime2 = (data[1] + (data[2] + data[3] * 256) * 60) / 10;
                p2_runing_time = true;
                break;
            default:
                break;
            }
        }
        break;
    case Nozzle1 + 1:
        if(eft_update.update_flag == true)
            eft_dev_update_wait_ereseflash(data);
        break;
    case Nozzle1 + 2:
        if(data[5] == 0xF1 && data[6] == 0xF3)
        {
            switch (data[0])
            {
            case 0XF4:
                for(uint8_t i = 0;i < 3;i++)   
                {
                    dev_nozzle1.sn[2 * i] = ((data[2+i] >> 4) & 0xf) + '0';
                    dev_nozzle1.sn[2 * i + 1] = (data[2+i] & 0xf )+ '0';
                }
                dev_nozzle1.sn[6] = ((data[7] >> 4) & 0xf) + '0';
                dev_nozzle1.sn[7] = (data[7] & 0xf )+ '0';

                regist_dev_info(&dev_nozzle1,DEVICE_NOZZLE1,false,dev_nozzle1.sn,8,NULL,0,NULL,0,"eftnozzle1",11);
                break;
            case 0XF5:
                for (uint8_t i = 1; i < 5; i++)
                {
                    if(data[i] < 10)
                    {
                        dev_nozzle1.soft_serial[2 * (i - 1)] = '0';
                        Int2String(data[i],&dev_nozzle1.soft_serial[2 * (i - 1) + 1],1);
                    }
                    else
                    {
                        Int2String(data[i],&dev_nozzle1.soft_serial[ (i - 1) * 2],2);
                    }

                }
                regist_dev_info(&dev_nozzle1,DEVICE_NOZZLE1,false,NULL,0,dev_nozzle1.soft_serial,8,NULL,0,"eftnozzle1",11);
                break;
            case 0XF6:
                Dev.Nozzle.runtime1 = (data[1] + (data[2] + data[3] * 256) * 60) / 10;
                n1_runing_time = true;
                break;
            default:
                break;
            }
        }
        break;
    case Nozzle2 + 1:
        if(eft_update.update_flag == true)
            eft_dev_update_wait_ereseflash(data);
        break;
    case Nozzle2 + 2:
        if(data[5] == 0xF1 && data[6] == 0xF3)
        {
            switch (data[0])
            {
            case 0XF4:
                if(data[5] == 0xF1 && data[6] == 0xF3)
                {
                    for(uint8_t i = 0;i < 3;i++)
                    {
                        dev_nozzle2.sn[2 * i] = ((data[2+i] >> 4) & 0xf) + '0';
                        dev_nozzle2.sn[2 * i + 1] = (data[2+i] & 0xf )+ '0';
                    }
                    dev_nozzle2.sn[6] = ((data[7] >> 4) & 0xf) + '0';
                    dev_nozzle2.sn[7] = (data[7] & 0xf )+ '0';

                    regist_dev_info(&dev_nozzle2,DEVICE_NOZZLE2,false,dev_nozzle2.sn,8,NULL,0,NULL,0,"eftnozzle2",11);
                }
                break;
            case 0XF5:
                for (uint8_t i = 1; i < 5; i++)
                {
                    if(data[i] < 10)
                    {
                        dev_nozzle2.soft_serial[2 * (i - 1)] = '0';
                        Int2String(data[i],&dev_nozzle2.soft_serial[2 * (i - 1) + 1],1);
                    }
                    else
                    {
                        Int2String(data[i],&dev_nozzle2.soft_serial[ (i - 1) * 2],2);
                    }

                }
                regist_dev_info(&dev_nozzle2,DEVICE_NOZZLE2,false,NULL,0,dev_nozzle2.soft_serial,8,NULL,0,"eftnozzle2",11);
                break;
            case 0XF6:
                Dev.Nozzle.runtime2 = (data[1] + (data[2] + data[3] * 256) * 60) / 10;
                n2_runing_time = true;
                break;
            default:
                break;
            }
        }
        break;
    case Nozzle3 + 1:
        if(eft_update.update_flag == true)
            eft_dev_update_wait_ereseflash(data);
        break;
    case Nozzle3 + 2:
        if(data[5] == 0xF1 && data[6] == 0xF3)
        {
            switch (data[0])
            {
            case 0XF4:
                for(uint8_t i = 0;i < 3;i++)
                {
                    dev_nozzle3.sn[2 * i] = ((data[2+i] >> 4) & 0xf) + '0';
                    dev_nozzle3.sn[2 * i + 1] = (data[2+i] & 0xf )+ '0';
                }
                dev_nozzle3.sn[6] = ((data[7] >> 4) & 0xf) + '0';
                dev_nozzle3.sn[7] = (data[7] & 0xf )+ '0';
                regist_dev_info(&dev_nozzle3,DEVICE_NOZZLE3,false,dev_nozzle3.sn,8,NULL,0,NULL,0,"eftnozzle3",11);
                break;
            case 0XF5:
                for (uint8_t i = 1; i < 5; i++)
                {
                    if(data[i] < 10)
                    {
                        dev_nozzle3.soft_serial[2 * (i - 1)] = '0';
                        Int2String(data[i],&dev_nozzle3.soft_serial[2 * (i - 1) + 1],1);
                    }
                    else
                    {
                        Int2String(data[i],&dev_nozzle3.soft_serial[ (i - 1) * 2],2);
                    }

                }
                regist_dev_info(&dev_nozzle3,DEVICE_NOZZLE3,false,NULL,0,dev_nozzle3.soft_serial,8,NULL,0,"eftnozzle3",11);
                break;
            case 0XF6:
                Dev.Nozzle.runtime3 = (data[1] + (data[2] + data[3] * 256) * 60) / 10;
                n3_runing_time = true;
                break;
            default:
                break;
            }
        }
        break;
    case Nozzle4 + 1:
        if(eft_update.update_flag == true)
            eft_dev_update_wait_ereseflash(data);
        break;
    case Nozzle4 + 2:
        if(data[5] == 0xF1 && data[6] == 0xF3 )
        {
            switch (data[0])
            {
            case 0XF4:
                for(uint8_t i = 0;i < 3;i++)
                {
                    dev_nozzle4.sn[2 * i] = ((data[2+i] >> 4) & 0xf) + '0';
                    dev_nozzle4.sn[2 * i + 1] = (data[2+i] & 0xf )+ '0';
                }
                dev_nozzle4.sn[6] = ((data[7] >> 4) & 0xf) + '0';
                dev_nozzle4.sn[7] = (data[7] & 0xf )+ '0';
                regist_dev_info(&dev_nozzle4,DEVICE_NOZZLE4,false,dev_nozzle4.sn,8,NULL,0,NULL,0,"eftnozzle4",11);
                break;
            case 0XF5:
                for (uint8_t i = 1; i < 5; i++)
                {
                    if(data[i] < 10)
                    {
                        dev_nozzle4.soft_serial[2 * (i - 1)] = '0';
                        Int2String(data[i],&dev_nozzle4.soft_serial[2 * (i - 1) + 1],1);
                    }
                    else
                    {
                        Int2String(data[i],&dev_nozzle4.soft_serial[ (i - 1) * 2],2);
                    }

                }
                regist_dev_info(&dev_nozzle4,DEVICE_NOZZLE4,false,NULL,0,dev_nozzle4.soft_serial,8,NULL,0,"eftnozzle4",11);
                break;
            case 0XF6:
                Dev.Nozzle.runtime4 = (data[1] + (data[2] + data[3] * 256) * 60) / 10;
                n4_runing_time = true;
                break;
            default:
                break;
            }
        }

       break;
    case Spread:
        break;
    default:
        break;
     }
}

/**
  * @file    Z70_Pump_Nozzle_recieved_hookfuction
  * @brief   Z70转接板
  * @param    
  * @details 
  * @author  Zhang Sir 
 **/
Z70_tranfer z70_info;
weight70_dev z70weight;
uint16_t LiftingWeight_warning = 0;
void Z70_Pump_Nozzle_recieved_hookfuction(uint32_t CanID, uint8_t data[], uint8_t len)
{      
    //70吊运板称重
    if(CanID == CAN_EFT70_WEIGHT)
    {
        if(drone_type != Drone_Z70)
            drone_type = Drone_Z70;
        Z70_LiftingWeight_exist = true;
        fplate.weight = (data[0] + data[1] * 256);
        LiftingWeight_warning = data[2];
        Dev.Weight_Link.connect_status = COMP_NORMAL;
        Dev.Weight.facid = FAC_EFT;
        Dev.Weight_Link.recv_time = HAL_GetTick();
    }
    else if(CanID == CAN_EFT70_WEIGHT_ACK)
    {
        //称重回馈
        switch (data[0])
        {
        case 0xe1:
            pmu_set_ack(_MSGID_SET,25,Cal_Remove_Peel,0); 
            break;
        case 0xe2:
            pmu_set_ack(_MSGID_SET,25,Cal_Weight,0);
            break;
        case 0xe3:
            pmu_set_ack(_MSGID_SET,MSGID_SET_WEIGHT_K,0,0);
            break;
        case 0xe4:
            z70weight.info.k1 = data[1] + ((data[2] & 0x3f) << 8);
            z70weight.info.k2 = (data[2] >> 6) + (data[3] << 2) + ((data[4] & 0xf) << 10);
            z70weight.info.k3 = (data[4] >> 4) + (data[5] << 4)  + ((data[6] & 0x3) << 12);
            z70weight.info.k4 = (data[6] >> 2) + (data[7] << 6);
            break;
        case 0XF3:
            for(uint8_t i = 0;i < 3;i++)
            {
                dev_weight.sn[2 * i] = ((data[2+i] >> 4) & 0xf) + '0';
                dev_weight.sn[2 * i + 1] = (data[2+i] & 0xf )+ '0';
            }
            dev_weight.sn[6] = ((data[7] >> 4) & 0xf) + '0';
            dev_weight.sn[7] = (data[7] & 0xf )+ '0';

            regist_dev_info(&dev_weight,DEVICE_WEIGHT,false,dev_weight.sn,8,NULL,0,NULL,0,"eftweight",10);
            break;
        case 0xF4:
            for (uint8_t i = 2; i < 5; i++)
            {
                if(data[i] < 10)
                {
                    dev_weight.soft_serial[2 * (i - 2)] = '0';
                    Int2String(data[i],&dev_weight.soft_serial[2 * (i - 2) + 1],1);
                }
                else
                {
                    Int2String(data[i],&dev_weight.soft_serial[ (i - 2) * 2],2);
                }

            }
            if(data[7] < 10)
            {
                dev_weight.soft_serial[2 * (4 - 1)] = '0';
                Int2String(data[7],&dev_weight.soft_serial[2 * (4 - 1) + 1],1);
            }
            else
            {
                Int2String(data[7],&dev_weight.soft_serial[ (4 - 1) * 2],2);
            }

            regist_dev_info(&dev_weight,DEVICE_WEIGHT,false,NULL,0,dev_weight.soft_serial,8,NULL,0,"eftweight",10);
            break;
        case 0xF5:
            Dev.Weight.runtime = data[2] + (data[3] + data[4] * 256) * 60;
            weight_runing_time = true;
            break;
        case 0xF7:
            pmu_set_ack(_MSGID_SET,MSGID_SET_RESIWIRE_BLOWN,data[1],0);
            break;
        default:
            break;
        }
    }


    if(CanID == 0x7010)
    {
        memcpy(&z70_info, &data[0], len);

        if(drone_type != Drone_Z70)
            drone_type = Drone_Z70;

        switch (z70_info.dev_type)
        {
        case Z70_Public:
            if(Z70_LiftingWeight_exist == false)
                memcpy(&fplate.weight,&data[4],2); 
            mimo_lackloss.status = data[6];

            Dev.Weight_Link.connect_status = COMP_NORMAL;
            Dev.Weight.facid = FAC_EFT;
            Dev.Weight_Link.recv_time = HAL_GetTick();
            
            break;
        case Z70_Pump1:
            pump1.rpm = z70_info.rpm;
            pump1.error_status = z70_info.warning;

            Dev.L_pump1_Link.connect_status = COMP_NORMAL;
            Dev.L_pump1.facid = FAC_EFT;
            Dev.L_pump1_Link.recv_time = HAL_GetTick();
            Dev.Eft_CanDev_Link[0].connect_status = COMP_NORMAL;
            Dev.Eft_CanDev_Link[0].recv_time = HAL_GetTick();
            break;
        case Z70_Pump2:
            pump2.rpm = z70_info.rpm;
            pump2.error_status = z70_info.warning;

            Dev.L_pump2_Link.connect_status = COMP_NORMAL;
            Dev.L_pump2.facid = FAC_EFT;
            Dev.L_pump2_Link.recv_time = HAL_GetTick();
            Dev.Eft_CanDev_Link[1].connect_status = COMP_NORMAL;
            Dev.Eft_CanDev_Link[1].recv_time = HAL_GetTick();
            break;
        case Z70_Nozzle1:
            nozzle1.rpm = z70_info.rpm;
            nozzle1.error_status = z70_info.warning;

            Dev.Nozzle_Link.connect_status = COMP_NORMAL;
            Dev.Nozzle.facid = FAC_EFT;
            Dev.Nozzle_Link.recv_time = HAL_GetTick();
            Dev.Eft_CanDev_Link[2].connect_status = COMP_NORMAL;
            Dev.Eft_CanDev_Link[2].recv_time = HAL_GetTick();
            break;
        case Z70_Nozzle2:
            nozzle2.rpm = z70_info.rpm;
            nozzle2.error_status = z70_info.warning;

            Dev.Nozzle_Link.connect_status = COMP_NORMAL;
            Dev.Nozzle.facid = FAC_EFT;
            Dev.Nozzle_Link.recv_time = HAL_GetTick();
            Dev.Eft_CanDev_Link[3].connect_status = COMP_NORMAL;
            Dev.Eft_CanDev_Link[3].recv_time = HAL_GetTick();
            break;
        case Z70_Nozzle3:
            nozzle3.rpm = z70_info.rpm;
            nozzle3.error_status = z70_info.warning;

            Dev.Nozzle_Link.connect_status = COMP_NORMAL;
            Dev.Nozzle.facid = FAC_EFT;
            Dev.Nozzle_Link.recv_time = HAL_GetTick();
            Dev.Eft_CanDev_Link[4].connect_status = COMP_NORMAL;
            Dev.Eft_CanDev_Link[4].recv_time = HAL_GetTick();
            break;
        case Z70_Nozzle4:
            nozzle4.rpm = z70_info.rpm;
            nozzle4.error_status = z70_info.warning;

            Dev.Nozzle_Link.connect_status = COMP_NORMAL;
            Dev.Nozzle.facid = FAC_EFT;
            Dev.Nozzle_Link.recv_time = HAL_GetTick();
            Dev.Eft_CanDev_Link[5].connect_status = COMP_NORMAL;
            Dev.Eft_CanDev_Link[5].recv_time = HAL_GetTick();
            break;
        case Z70_Churn:
            churn.rpm = z70_info.rpm;
            churn.error_status = z70_info.warning;

            Dev.Seed_Link.connect_status = COMP_NORMAL;
            Dev.Seed_Link.recv_time = HAL_GetTick();
            Dev.Seed.facid = FAC_EFT;
            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_EFT;
            break;
        default:
            break;
        }
    }
    else if(CanID == 0x7011)
    {   
        if(eft_update.update_flag == true)
                eft_dev_update_wait_ereseflash(data);
        //称重回馈
        switch (data[0])
        {
        case 0xe1:
            pmu_set_ack(_MSGID_SET,25,Cal_Remove_Peel,0); 
            break;
        case 0xe2:
            pmu_set_ack(_MSGID_SET,25,Cal_Weight,0);
            break;
        case 0xe3:
            pmu_set_ack(_MSGID_SET,MSGID_SET_WEIGHT_K,0,0);
            break;
        case 0xe4:
            z70weight.info.k1 = data[1] + ((data[2] & 0x3f) << 8);
            z70weight.info.k2 = (data[2] >> 6) + (data[3] << 2) + ((data[4] & 0xf) << 10);
            z70weight.info.k3 = (data[4] >> 4) + (data[5] << 4)  + ((data[6] & 0x3) << 12);
            z70weight.info.k4 = (data[6] >> 2) + (data[7] << 6);
            break;
        default:
            break;
        }

        switch (data[1])
        {
        case Z70_Public:
            switch (data[0])
            {
            case 0XF3:
                for(uint8_t i = 0;i < 3;i++)
                {
                    dev_weight.sn[2 * i] = ((data[2+i] >> 4) & 0xf) + '0';
                    dev_weight.sn[2 * i + 1] = (data[2+i] & 0xf )+ '0';
                }
                dev_weight.sn[6] = ((data[7] >> 4) & 0xf) + '0';
                dev_weight.sn[7] = (data[7] & 0xf )+ '0';

                regist_dev_info(&dev_weight,DEVICE_WEIGHT,false,dev_weight.sn,8,NULL,0,NULL,0,"eftweight",10);
                break;
            case 0xF4:
                for (uint8_t i = 2; i < 5; i++)
                {
                    if(data[i] < 10)
                    {
                        dev_weight.soft_serial[2 * (i - 2)] = '0';
                        Int2String(data[i],&dev_weight.soft_serial[2 * (i - 2) + 1],1);
                    }
                    else
                    {
                        Int2String(data[i],&dev_weight.soft_serial[ (i - 2) * 2],2);
                    }

                }
                if(data[7] < 10)
                {
                    dev_weight.soft_serial[2 * (4 - 1)] = '0';
                    Int2String(data[7],&dev_weight.soft_serial[2 * (4 - 1) + 1],1);
                }
                else
                {
                    Int2String(data[7],&dev_weight.soft_serial[ (4 - 1) * 2],2);
                }

                regist_dev_info(&dev_weight,DEVICE_WEIGHT,false,NULL,0,dev_weight.soft_serial,8,NULL,0,"eftweight",10);
                break;
            case 0xF5:
                Dev.Weight.runtime = data[2] + (data[3] + data[4] * 256) * 60;
                weight_runing_time = true;
                break;
            default:
                break;
            }       
            break;
        case Z70_Pump1:
            switch (data[0])
            {
            case 0XF3:
                for(uint8_t i = 0;i < 3;i++)
                {
                    dev_pump1.sn[2 * i] = ((data[2+i] >> 4) & 0xf) + '0';
                    dev_pump1.sn[2 * i + 1] = (data[2+i] & 0xf )+ '0';
                }
                dev_pump1.sn[6] = ((data[7] >> 4) & 0xf) + '0';
                dev_pump1.sn[7] = (data[7] & 0xf )+ '0';

                regist_dev_info(&dev_pump1,DEVICE_LPUMP1,false,dev_pump1.sn,8,NULL,0,NULL,0,"eftpump1",9);
                break;
            case 0XF4:
                for (uint8_t i = 2; i < 5; i++)
                {
                    if(data[i] < 10)
                    {
                        dev_pump1.soft_serial[2 * (i - 2)] = '0';
                        Int2String(data[i],&dev_pump1.soft_serial[2 * (i - 2) + 1],1);
                    }
                    else
                    {
                        Int2String(data[i],&dev_pump1.soft_serial[ (i - 2) * 2],2);
                    }

                }
                if(data[7] < 10)
                {
                    dev_pump1.soft_serial[2 * (4 - 1)] = '0';
                    Int2String(data[7],&dev_pump1.soft_serial[2 * (4 - 1) + 1],1);
                }
                else
                {
                    Int2String(data[7],&dev_pump1.soft_serial[ (4 - 1) * 2],2);
                }
                regist_dev_info(&dev_pump1,DEVICE_LPUMP1,false,NULL,0,dev_pump1.soft_serial,8,NULL,0,"eftpump1",9);
                break;
            case 0XF5:
                Dev.Pump.runtime1 = (data[2] + (data[3] + data[4] * 256) * 60) / 10;
                p1_runing_time = true;
                break;
            default:
                break;
            }
            break;
        case Z70_Pump2:
            switch (data[0])
            {
            case 0XF3:
                    for(uint8_t i = 0;i < 3;i++)
                    {
                        dev_pump2.sn[2 * i] = ((data[2+i] >> 4) & 0xf) + '0';
                        dev_pump2.sn[2 * i + 1] = (data[2+i] & 0xf )+ '0';
                    }
                    dev_pump2.sn[6] = ((data[7] >> 4) & 0xf) + '0';
                    dev_pump2.sn[7] = (data[7] & 0xf )+ '0';

                    regist_dev_info(&dev_pump2,DEVICE_LPUMP2,false,dev_pump2.sn,8,NULL,0,NULL,0,"eftpump2",9);
                break;
            case 0XF4:
                for (uint8_t i = 2; i < 5; i++)
                {
                    if(data[i] < 10)
                    {
                        dev_pump2.soft_serial[2 * (i - 2)] = '0';
                        Int2String(data[i],&dev_pump2.soft_serial[2 * (i - 2) + 1],1);
                    }
                    else
                    {
                        Int2String(data[i],&dev_pump2.soft_serial[ (i - 2) * 2],2);
                    }

                }
                if(data[7] < 10)
                {
                    dev_pump2.soft_serial[2 * (4 - 1)] = '0';
                    Int2String(data[7],&dev_pump2.soft_serial[2 * (4 - 1) + 1],1);
                }
                else
                {
                    Int2String(data[7],&dev_pump2.soft_serial[ (4 - 1) * 2],2);
                }
                regist_dev_info(&dev_pump2,DEVICE_LPUMP2,false,NULL,0,dev_pump2.soft_serial,8,NULL,0,"eftpump2",9);
                break;
            case 0XF5:
                Dev.Pump.runtime2 = (data[2] + (data[3] + data[4] * 256) * 60) / 10;
                p2_runing_time = true;
                break;
            default:
                break;
            }         
            break;
        case Z70_Nozzle1:
            switch (data[0])
            {
            case 0XF3:
                for(uint8_t i = 0;i < 3;i++)   
                {
                    dev_nozzle1.sn[2 * i] = ((data[2+i] >> 4) & 0xf) + '0';
                    dev_nozzle1.sn[2 * i + 1] = (data[2+i] & 0xf )+ '0';
                }
                dev_nozzle1.sn[6] = ((data[7] >> 4) & 0xf) + '0';
                dev_nozzle1.sn[7] = (data[7] & 0xf )+ '0';

                regist_dev_info(&dev_nozzle1,DEVICE_NOZZLE1,false,dev_nozzle1.sn,8,NULL,0,NULL,0,"eftnozzle1",11);
                break;
            case 0XF4:
                for (uint8_t i = 2; i < 5; i++)
                {
                    if(data[i] < 10)
                    {
                        dev_nozzle1.soft_serial[2 * (i - 2)] = '0';
                        Int2String(data[i],&dev_nozzle1.soft_serial[2 * (i - 2) + 1],1);
                    }
                    else
                    {
                        Int2String(data[i],&dev_nozzle1.soft_serial[ (i - 2) * 2],2);
                    }

                }
                if(data[7] < 10)
                {
                    dev_nozzle1.soft_serial[2 * (4 - 1)] = '0';
                    Int2String(data[7],&dev_nozzle1.soft_serial[2 * (4 - 1) + 1],1);
                }
                else
                {
                    Int2String(data[7],&dev_nozzle1.soft_serial[ (4 - 1) * 2],2);
                }
                regist_dev_info(&dev_nozzle1,DEVICE_NOZZLE1,false,NULL,0,dev_nozzle1.soft_serial,8,NULL,0,"eftnozzle1",11);
                break;
            case 0XF5:
                Dev.Nozzle.runtime1 = (data[2] + (data[3] + data[4] * 256) * 60) / 10;
                n1_runing_time = true;
                break;
            default:
                break;
            }
            break;
        case Z70_Nozzle2:
            switch (data[0])
            {
            case 0XF3:
                if(data[5] == 0xF1 && data[6] == 0xF3)
                {
                    for(uint8_t i = 0;i < 3;i++)
                    {
                        dev_nozzle2.sn[2 * i] = ((data[2+i] >> 4) & 0xf) + '0';
                        dev_nozzle2.sn[2 * i + 1] = (data[2+i] & 0xf )+ '0';
                    }
                    dev_nozzle2.sn[6] = ((data[7] >> 4) & 0xf) + '0';
                    dev_nozzle2.sn[7] = (data[7] & 0xf )+ '0';

                    regist_dev_info(&dev_nozzle2,DEVICE_NOZZLE2,false,dev_nozzle2.sn,8,NULL,0,NULL,0,"eftnozzle2",11);
                }
                break;
            case 0XF4:
                for (uint8_t i = 2; i < 5; i++)
                {
                    if(data[i] < 10)
                    {
                        dev_nozzle2.soft_serial[2 * (i - 2)] = '0';
                        Int2String(data[i],&dev_nozzle2.soft_serial[2 * (i - 2) + 1],1);
                    }
                    else
                    {
                        Int2String(data[i],&dev_nozzle2.soft_serial[ (i - 2) * 2],2);
                    }

                }
                if(data[7] < 10)
                {
                    dev_nozzle2.soft_serial[2 * (4 - 1)] = '0';
                    Int2String(data[7],&dev_nozzle2.soft_serial[2 * (4 - 1) + 1],1);
                }
                else
                {
                    Int2String(data[7],&dev_nozzle2.soft_serial[ (4 - 1) * 2],2);
                }
                regist_dev_info(&dev_nozzle2,DEVICE_NOZZLE2,false,NULL,0,dev_nozzle2.soft_serial,8,NULL,0,"eftnozzle2",11);
                break;
            case 0XF5:
                Dev.Nozzle.runtime2 = (data[2] + (data[3] + data[4] * 256) * 60) / 10;
                n2_runing_time = true;
                break;
            default:
                break;
            }
            break;
        case Z70_Nozzle3:
            switch (data[0])
            {
            case 0XF3:
                for(uint8_t i = 0;i < 3;i++)
                {
                    dev_nozzle3.sn[2 * i] = ((data[2+i] >> 4) & 0xf) + '0';
                    dev_nozzle3.sn[2 * i + 1] = (data[2+i] & 0xf )+ '0';
                }
                dev_nozzle3.sn[6] = ((data[7] >> 4) & 0xf) + '0';
                dev_nozzle3.sn[7] = (data[7] & 0xf )+ '0';
                regist_dev_info(&dev_nozzle3,DEVICE_NOZZLE3,false,dev_nozzle3.sn,8,NULL,0,NULL,0,"eftnozzle3",11);
                break;
            case 0XF4:
                for (uint8_t i = 2; i < 5; i++)
                {
                    if(data[i] < 10)
                    {
                        dev_nozzle3.soft_serial[2 * (i - 2)] = '0';
                        Int2String(data[i],&dev_nozzle3.soft_serial[2 * (i - 2) + 1],1);
                    }
                    else
                    {
                        Int2String(data[i],&dev_nozzle3.soft_serial[ (i - 2) * 2],2);
                    }

                }
                if(data[7] < 10)
                {
                    dev_nozzle3.soft_serial[2 * (4 - 1)] = '0';
                    Int2String(data[7],&dev_nozzle3.soft_serial[2 * (4 - 1) + 1],1);
                }
                else
                {
                    Int2String(data[7],&dev_nozzle3.soft_serial[ (4 - 1) * 2],2);
                }
                regist_dev_info(&dev_nozzle3,DEVICE_NOZZLE3,false,NULL,0,dev_nozzle3.soft_serial,8,NULL,0,"eftnozzle3",11);
                break;
            case 0XF5:
                Dev.Nozzle.runtime3 = (data[2] + (data[3] + data[4] * 256) * 60) / 10;
                n3_runing_time = true;
                break;
            default:
                break;
            }
            break;
        case Z70_Nozzle4:
            switch (data[0])
            {
            case 0XF3:
                for(uint8_t i = 0;i < 3;i++)
                {
                    dev_nozzle4.sn[2 * i] = ((data[2+i] >> 4) & 0xf) + '0';
                    dev_nozzle4.sn[2 * i + 1] = (data[2+i] & 0xf )+ '0';
                }
                dev_nozzle4.sn[6] = ((data[7] >> 4) & 0xf) + '0';
                dev_nozzle4.sn[7] = (data[7] & 0xf )+ '0';
                regist_dev_info(&dev_nozzle4,DEVICE_NOZZLE4,false,dev_nozzle4.sn,8,NULL,0,NULL,0,"eftnozzle4",11);
                break;
            case 0XF4:
                for (uint8_t i = 1; i < 5; i++)
                {
                    if(data[i] < 10)
                    {
                        dev_nozzle4.soft_serial[2 * (i - 2)] = '0';
                        Int2String(data[i],&dev_nozzle4.soft_serial[2 * (i - 2) + 1],1);
                    }
                    else
                    {
                        Int2String(data[i],&dev_nozzle4.soft_serial[ (i - 2) * 2],2);
                    }

                }
                if(data[7] < 10)
                {
                    dev_nozzle4.soft_serial[2 * (4 - 1)] = '0';
                    Int2String(data[7],&dev_nozzle4.soft_serial[2 * (4 - 1) + 1],1);
                }
                else
                {
                    Int2String(data[7],&dev_nozzle4.soft_serial[ (4 - 1) * 2],2);
                }
                regist_dev_info(&dev_nozzle4,DEVICE_NOZZLE4,false,NULL,0,dev_nozzle4.soft_serial,8,NULL,0,"eftnozzle4",11);
                break;
            case 0XF5:
                Dev.Nozzle.runtime4 = (data[2] + (data[3] + data[4] * 256) * 60) / 10;
                n4_runing_time = true;
                break;
            default:
                break;
            }
            break;
        default:
            break;
        }

    }
}

/**
  * @file    can_sendmsg_eft_water
  * @brief   更新设备信息
  * @param    
  * @details 
  * @author  Zhang Sir 
 **/
water70_dev water70_info;
void can_sendmsg_eft_water(void)
{
    uint8_t can_buf[8] = {0};
    if(drone_type == Drone_Z20)
    {
        if(Dev.L_pump1_Link.connect_status == COMP_NORMAL || Dev.L_pump2_Link.connect_status == COMP_NORMAL )
        {
            
            static uint8_t life[6] = {0};

            can_buf[0] = 0xF1;
            can_buf[5] = 0xF1; 
            can_buf[6] = 0xF3; 

            //pmup1
            can_buf[1]= (pmu_pin.pump1 >> 8) & 0xff;
            can_buf[2]= pmu_pin.pump1 & 0xff;
            can_buf[7]= life[0];

            life[0]++;
            Can_Send_Msg_Func(CANID2, can_buf, 8, Pump1 + 1 , CAN_ID_EXT); 

            //pump2
            can_buf[1]= (pmu_pin.pump2 >> 8) & 0xff;
            can_buf[2]= pmu_pin.pump2 & 0xff;
            can_buf[7]= life[1];

            life[1]++;
            Can_Send_Msg_Func(CANID2, can_buf, 8, Pump2 + 1 , CAN_ID_EXT); 

            //nozzle1
            can_buf[1]= (pmu_pin.nozz1_fm >> 8) & 0xff;
            can_buf[2]= pmu_pin.nozz1_fm & 0xff;
            can_buf[7]= life[2];

            life[2]++;
            Can_Send_Msg_Func(CANID2, can_buf, 8, Nozzle1 + 1 , CAN_ID_EXT); 

            //nozzle2
            can_buf[1]= (pmu_pin.nozz1_fm >> 8) & 0xff;
            can_buf[2]= pmu_pin.nozz1_fm & 0xff;
            can_buf[7]= life[3];

            life[3]++;
            Can_Send_Msg_Func(CANID2, can_buf, 8, Nozzle2 + 1 , CAN_ID_EXT);

            //nozzle3
            can_buf[1]= (pmu_pin.nozz2_zp >> 8) & 0xff;
            can_buf[2]= pmu_pin.nozz2_zp & 0xff;
            can_buf[7]= life[4];

            life[4]++;
            Can_Send_Msg_Func(CANID2, can_buf, 8, Nozzle3 + 1 , CAN_ID_EXT);

            //nozzle4
            can_buf[1]= (pmu_pin.nozz2_zp >> 8) & 0xff;
            can_buf[2]= pmu_pin.nozz2_zp & 0xff;
            can_buf[7]= life[5];

            life[5]++;
            Can_Send_Msg_Func(CANID2, can_buf, 8, Nozzle4 + 1 , CAN_ID_EXT);
        }
    }
    else if(drone_type == Drone_Z70)
    {
        water70_info.dev_water70.flag = planep.lock_status == STA_LOCK ? 0x5 : 0xA;
        if(Dev.Seed_Link.connect_status != COMP_NOEXIST)
        {
            water70_info.dev_water70.pump1 = spread_par.pwm_valve - 1000;
            water70_info.dev_water70.pump2 = spread_par.pwm_turntable - 1000;
        }
        else
        {
            water70_info.dev_water70.pump1 = pmu_pin.pump1 - 1000;
            water70_info.dev_water70.pump2 = pmu_pin.pump2 - 1000;
        }
        water70_info.dev_water70.nozzle1 = pmu_pin.nozz1_fm - 1000;
        water70_info.dev_water70.nozzle2 = pmu_pin.nozz2_zp - 1000;
        water70_info.dev_water70.nozzle3 = pmu_pin.nozz3 - 1000;
        water70_info.dev_water70.nozzle4 = pmu_pin.nozz4 - 1000;

        memcpy(&can_buf, &water70_info.buf[0], sizeof(soft_water70));
        Can_Send_Msg_Func(CANID2, can_buf, 8, 0x7011 , CAN_ID_EXT);
    }

}

/**
  * @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    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){}
    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)
    {
        EscPtr = &EscMsg[0];
    }
    

    if ( EscPtr != NULL )
    {
        Dev.L_pump1_Link.connect_status = COMP_NORMAL;
        Dev.L_pump1_Link.recv_time = HAL_GetTick();
        Dev.L_pump1.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    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;
        }

        // 初始化并清零现场
        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_Func(CANID2, send_can_buf, 3, canID, CAN_ID_EXT);

                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_Func(CANID2, send_can_buf, 3, canID, CAN_ID_EXT);

                    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_Func(CANID2, send_can_buf, 3, canID, CAN_ID_EXT);

                SetEscidTail.HWTailBit.tranid++;

                setESCidInfo.reTxMaxCnt--;
            }
            else
            {
                //广播模式，总线上有其他同NODEID的ESC时也会收到反馈
                set_HWesc_ESCid = false;

                setESCidInfo.setESCidStep++;
                setESCidInfo.reTxMaxCnt = 5;

                //设置成功，发送ACK
            }

            //超时退出
            if ( setESCidInfo.reTxMaxCnt <= 0 )
            {
                set_HWesc_ESCid = false;

                setESCidInfo.setESCidStep = SETESCID_INIT;
                setESCidInfo.reTxMaxCnt = 5;
            }
        }
    }
}

/**
  * @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] = {1000,1000,1000,1000};

    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;
    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_Func(CANID2, send_thr_buf, 8, canID, CAN_ID_EXT);

    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)
    {
        if(Check_Timer_Ready(&time_1hz,_1_HZ_))
        {
            HW_CanGetESCInfomation();
            HW_CanSetESCInfomation();
        }

        if(set_HWesc_ESCid != true)
        {
            can_sendmsg_esc();
        }
    }

}