V9_PMU/Src/soft_seed_device.c

#include "soft_seed_device.h"
#include "common.h"
#include "string.h"
#include "soft_p_2_c.h"
#include "soft_eft.h"
#include "soft_flow.h"
#include "soft_water_device.h"
#include "soft_uart.h"
#include "soft_terrain.h"
#include "soft_obstacle.h"
#include "soft_bms.h"
#include "soft_okcell.h"
#include "soft_version.h"
#include "math.h"
#include "qingxie_bms.h"

weight weight_vkinfo;
seed seed_vkinfo;
send_seed_device seed_dev;

Weight_cal weight_order;

/**
  * @file    Weight_recieved_hookfuction
  * @brief   VK协议称重传感器
  * @param   none
  * @details 
  * @author  Zhang Sir 
 **/
void Weight_recieved_hookfuction(uint32_t CanID, uint8_t data[], uint8_t len)
{
    switch (CanID)
    {
    //称重传感器
    case 0x81320:
        memcpy(&weight_vkinfo.sensor_status,&data[0],8);
        if(weight_vkinfo.weight < 0)
        {
            weight_vkinfo.weight = abs(weight_vkinfo.weight);
        }

        Dev.Weight.facid = FAC_VK;
        Dev.Weight_Link.connect_status = COMP_NORMAL;
        Dev.Weight_Link.recv_time = HAL_GetTick();

        break;
    //称重传感器设置回馈
    case 0x81322:
        switch (data[0])
        {  
        //去皮校准ack
        case 0xf1:
            pmu_set_ack(_MSGID_SET,25,Cal_Remove_Peel,0);
            break;
        //称重校准ack
        case 0xf2:
            pmu_set_ack(_MSGID_SET,25,Cal_Weight,0);
            break;
        //恢复出厂设置ack
        case 0xf4:
            pmu_set_ack(_MSGID_SET,25,Cal_Seed_Back,0);
            break;
        case 0xf5:
            break;
        case 0xf6:
        {
            char ver_buf[8] = {0};
            int ver_temp32 = 0;
            memcpy(&ver_temp32,&data[1],4);
            Int2String(ver_temp32,&ver_buf[2],6);
            ver_buf[0] = '0';
            ver_buf[1] = data[7] + '0';

            regist_dev_info(&dev_weight,DEVICE_WEIGHT,false,NULL,0,(char *)&ver_buf[0],8,NULL,0,"weight",7);
            break;
        }
        //厂家编号
        case 0xf7:
            if(data[1] == 'Q' && data[2] == 'F') //0x 51 46
            {
                weight_vkinfo.serial_num = 3;
                Dev.Weight.facid = FAC_QIFEI;
            }
            else
            {
                weight_vkinfo.serial_num = 0;
            }
            weight_init_vk.step.fac_num_flag = 0;
            weight_init_count = 10;
            //memcpy(&weight_vkinfo.serial_num, &data[1],4);
            break;
        //读取传感器K值
        case 0xe1:
            if(data[7] == 0x1)
            {
                memcpy(&weight_vkinfo.senor_k[0],&data[1],4);
            }
            else if(data[7] == 0x2)
            {
                memcpy(&weight_vkinfo.senor_k[2],&data[1],4);
            }
            weight_init_vk.step.read_k_flag = 0;
            break;
        //设置传感器K值ack
        case 0xe2:
            pmu_set_ack(_MSGID_SET,Cal_Set_Sensor_K,0,0);
            weight_vkinfo.senor_k[data[7]] = data[1] + (data[2] << 8);
            break;
        //称重传感器模式读取
        case 0xe3:
            weight_vkinfo.mode = data[1];
            weight_init_vk.step.weight_mode_flag = 0;
            weight_init_count = 10;
            break;
        //称重传感器模式ack
        case 0xe4:
            pmu_set_ack(_MSGID_SET,Cal_Set_Sensor_Mode,0,0);
            weight_vkinfo.mode = data[1];
            break;
        //最大下药率读取
        case 0xe5:
            memcpy(&weight_vkinfo.dose_rate,&data[1],2);
            weight_init_vk.step.dose_rate_flag = 0;
            weight_init_count = 10;
            break;
        //最高下药率ack
        case 0xe6:
            pmu_set_ack(_MSGID_EFT_CON,Cal_Set_Doserate,0,0);
            weight_vkinfo.dose_rate = data[1] + (data[2] << 8);
            break;            
        default:
            break;
        }
        break;
    //播撒器
    case 0x81323:
        memcpy(&seed_vkinfo,&data[0],7);

        Dev.Seed_Link.connect_status = COMP_NORMAL;
        Dev.Seed_Link.recv_time = HAL_GetTick();
        break;
    //播撒器回馈
    case 0x81325:
        switch (data[0])
        {
        case 0xf5:
            break;
        case 0xf6:
            break;
        case 0xf7:
            if(data[1] == 'Q' && data[2] == 'F')
            {
                seed_vkinfo.serial_num = 3;
                Dev.Seed.facid = FAC_QIFEI;
            }
            break;
        default:
            break;
        }
        break;
    case 0x81326:
        memcpy(&weight_vkinfo.Single_sensor[0],&data[0],8);
        break;
    default:
        break;
    }
}

//上电初始化发送的参数
//device_type : 1 称重
//            : 2 播撒
/**
  * @file    seed_init_send_info
  * @brief   上电初始化发送的参数
  * @param    1 称重
              2 播撒
  * @details 
  * @author  Zhang Sir 
 **/
Dev_weight_init weight_init_eft = {.step.read_k_flag = 1};
Dev_weight_init weight_init_vk = {.step.dose_rate_flag = 1, 
                                    .step.weight_mode_flag = 1, 
                                    .step.read_k_flag = 1,
                                    .step.verison = 1,
                                    .step.fac_num_flag = 1};
Dev_seed_init seed_init;
bool recv_fmu_seed_info = false;
uint8_t weight_init_count = 10;
void seed_init_send_info(uint8_t device_type,char *factory)
{
    uint8_t vk_canbuf[8] = {0,0,0,0,0,0XFE,0XFE,0};
    static uint8_t spread_init_count = 10;
    int vk_can_id = 0;

    if(strcmp(factory,"VK") == 0)
    {
        switch (device_type)
        {
        //称重
        case WEIGHT_DEVICE:
            vk_can_id = 0x81321;
            if(weight_init_vk.status != 0)
            {
                if(weight_init_vk.step.read_k_flag != 0)
                {
                    vk_canbuf[0] = 0xE1;
                }
                else if(weight_init_vk.step.fac_num_flag != 0 && weight_init_count > 0)
                {
                    vk_canbuf[0] = 0xF7;
                    weight_init_count--;
                    if(weight_init_count == 0)
                    {
                        weight_init_count = 10;
                        weight_init_vk.step.fac_num_flag = 0;
                    }
                }
                else if(weight_init_vk.step.verison != 0 && weight_init_count > 0)
                {
                    vk_canbuf[0] = 0xF6;
                    weight_init_count--;
                    if(weight_init_count == 0)
                    {
                        weight_init_count = 10;
                        weight_init_vk.step.verison = 0;
                    }
                }
                else if (weight_init_vk.step.weight_mode_flag != 0 && weight_init_count > 0)
                {
                    vk_canbuf[0] = 0xE3;
                    weight_init_count--;
                    if(weight_init_count == 0)
                    {
                        weight_init_count = 10;
                        weight_init_vk.step.weight_mode_flag = 0;
                    }
                }
                else if (weight_init_vk.step.dose_rate_flag != 0 && weight_init_count > 0)
                {
                    vk_canbuf[0] = 0xE5;
                    weight_init_count--;
                    if(weight_init_count == 0)
                    {
                        weight_init_count = 10;
                        weight_init_vk.step.dose_rate_flag = 0;
                    }
                }
                // else if (weight_init_vk.step.dose_rate_flag != 0) //序列号和固件版本
                // {
                //     vk_canbuf[0] = 0xf6;;
                // }
                // else if (weight_init_vk.step.dose_rate_flag != 0)
                // {
                //     vk_canbuf[0] = 0xf5;
                // }
                can_send_msg_normal((unsigned char *)&vk_canbuf, 8, vk_can_id);
            }
            break;
        //播撒
        case SEED_DEVICE:
            vk_can_id = 0x81324;
            if(spread_init_count > 0)
            {
                //获取厂家信息
                vk_canbuf[0] = 0xf7;
                spread_init_count--;
                can_send_msg_normal((unsigned char *)&vk_canbuf, 8, vk_can_id);
            }
            break;
        default:
            break;
        }
    }   
    else if (strcmp(factory,"EFT") == 0)
    {     
        vk_can_id = SEND_EFT_INFO;
        switch (device_type)
        {
        case WEIGHT_DEVICE:
            if(weight_init_eft.status != 0)
            {
                if(weight_init_eft.step.read_k_flag != 0)
                {
                    vk_canbuf[0] = 0xFD;
                    vk_canbuf[5] = 0xf1;
                    vk_canbuf[6] = 0xf3;
                }
                // else if(weight_init.step.sn_flag != 0) //EFT前板没有反馈。SN和版本
                // {
                //     vk_canbuf[0] = 0xF8;
                //     vk_canbuf[5] = 0xf1;
                //     vk_canbuf[6] = 0xf3;
                // }
                // else if(weight_init.step.version_flag != 0)
                // {
                //     vk_canbuf[0] = 0xf9;
                //     vk_canbuf[5] = 0xf1;
                //     vk_canbuf[6] = 0xf3;
                // }
                can_send_msg_normal((unsigned char *)&vk_canbuf, 8, vk_can_id);
            }
            break;
        case SEED_DEVICE:
            if(seed_init.status != 0)
            {
                if(seed_init.step.sn_flag != 0)
                {

                }
                else if(seed_init.step.version_flag != 0)
                {

                }
                //can_send_msg_normal((unsigned char *)&vk_canbuf, 8, vk_can_id);
            }

            break;
        default:
            break;
        }
    }
}


/**
  * @file    Set_Seed_Weight_Par
  * @brief   设置称重播撒参数
  * @param    1 称重
              2 播撒
  * @details 
  * @author  Zhang Sir 
 **/
void Set_Seed_Weight_Par(uint8_t device_type,char *factory)
{   
    uint8_t can_buf[8] = {0};
    if(strcmp(factory,"VK") == 0)
    {
        switch (device_type)
        {
        case WEIGHT_DEVICE:
            if(weight_order.type != 0)
            {
                can_buf[1] = 0x00;  //D1 - D4
                can_buf[2] = 0x00;
                can_buf[3] = 0x00;
                can_buf[4] = 0x00;
                can_buf[5] = 0xFE;
                can_buf[6] = 0xFE;
                can_buf[7] = 0x00;  //D5

                switch (weight_order.type)
                {
                //去皮校准
                case Weight_Peer:
                    can_buf[0] = 0xF1;
                    break;
                //重量校准
                case Weight_Kg:
                    can_buf[0] = 0xF2;
                    memcpy(&can_buf[1], &weight_order.order_con2,2);
                    break;
                case Weight_Reserve:
                    break;
                //恢复出厂设置
                case Weight_Bcak:
                    can_buf[0] = 0xF4;
                    break;
                //称重传感器K值
                case Weight_Set_K:
                    can_buf[0] = 0xE2;
                    can_buf[1] = weight_order.order_con2 & 0xff;
                    can_buf[2] = (weight_order.order_con2 >> 8) & 0xff;

                    can_buf[7] = weight_order.order_con1 - 1; //VK协议传感器编号是0-3
                    break;
                //称重传感器模式
                case Weight_Mode:
                    can_buf[0] = 0xE4;
                    can_buf[1] = weight_order.order_con1 & 0xff;
                    break;
                //最高下药率
                case Weight_Drug_Rate:
                    can_buf[0] = 0xE6;
                    can_buf[1] = weight_order.order_con1 & 0xff;
                    can_buf[2] = (weight_order.order_con1 >> 8) & 0xff;
                    break;
                default:
                    break;
                }
                can_send_msg_normal((unsigned char *)&can_buf, 8, 0x81321);
                weight_order.type = 0;
            }    
            break;
        case SEED_DEVICE:
            //更换信号输出方式 0 PWM 1 CAN
            if((seed_vkinfo.output != seed_output_mode) && recv_fmu_seed_info == true)
            {
                can_buf[0] = 0xF2;
                can_buf[1] = seed_output_mode;
                can_buf[2] = 0x00;
                can_buf[3] = 0x00;
                can_buf[4] = 0x00;
                can_buf[5] = 0xFE;
                can_buf[6] = 0xFE;
                can_buf[7] = 0;

                can_send_msg_normal((unsigned char *)&can_buf, 8, 0x81324);
            }
            break;
        default:
            break;
        }
    }
    else if (strcmp(factory,"EFT") == 0)
    {
        switch (device_type)
        {
        case SEED_DEVICE:
            //更换信号输出方式 0 PWM 1 CAN
            if((eft_info.can_enable != seed_output_mode) && recv_fmu_seed_info == true)
            {
                can_buf[0] = 0xF2;
                can_buf[1] = seed_output_mode;
                can_buf[2] = 0x00;
                can_buf[3] = 0x00;
                can_buf[4] = 0x00;
                can_buf[5] = 0xf1;
                can_buf[6] = 0xf3;
                can_buf[7] = 0;

                can_send_msg_normal((unsigned char *)&can_buf, 8, SEND_EFT_INFO);
                HAL_Delay(10);
            }        
            break;
        case WEIGHT_DEVICE:
            if(weight_order.type != 0)   
            {
                can_buf[1] = 0x00;
                can_buf[2] = 0x00;
                can_buf[3] = 0x00;
                can_buf[4] = 0x00;
                can_buf[5] = 0xf1;
                can_buf[6] = 0xf3;
                can_buf[7] = 0;

                switch (weight_order.type)
                {
                case Weight_Peer:
                    can_buf[0] = 0xF6;
                    break;
                case Weight_Kg:
                    can_buf[0] = 0xF7;
                    can_buf[1] = (weight_order.order_con2 >> 8) & 0xff;
                    can_buf[2] = weight_order.order_con2 & 0xff;
                    break;
                case Weight_Reserve:
                    break;
                case Weight_Bcak:
                    can_buf[0] = 0xF5;
                    break;
                case Weight_Set_K:
                    can_buf[0] = 0xFC;
                    can_buf[1] = eft_info.seed_k[0];
                    can_buf[2] = eft_info.seed_k[1];
                    can_buf[3] = eft_info.seed_k[2];

                    can_buf[weight_order.order_con1] = weight_order.order_con2 & 0xff;
                      
                    break;
                default:
                    break;
                }
                can_send_msg_normal((unsigned char *)&can_buf, 8, SEND_EFT_INFO);
                HAL_Delay(10);
                weight_order.type = 0;
            }
            break;
        default:
            break;
        }
    }
}


/**
  * @file    can_sendmsg_VK
  * @brief   播撒常发数据包
  * @param    
  * @details 
  * @author  Zhang Sir 
 **/
void can_sendmsg_VK(void)
{
    static uint8_t vk_count = 0;
    uint8_t vk_canbuf[8] = {0};

    if(Dev.Weight_Link.connect_status == COMP_NORMAL && (Dev.Weight.facid == FAC_VK ||
         Dev.Weight.facid == FAC_QIFEI) && HAL_GetTick() > 3000)
    {
        seed_init_send_info(WEIGHT_DEVICE,"VK");
        Set_Seed_Weight_Par(WEIGHT_DEVICE,"VK");
    }

    if(Dev.Seed_Link.connect_status == COMP_NORMAL && (Dev.Seed.facid == FAC_VK ||
         Dev.Seed.facid == FAC_QIFEI) && HAL_GetTick() > 2000)
    {

        seed_init_send_info(SEED_DEVICE,"VK");
        Set_Seed_Weight_Par(SEED_DEVICE,"VK");

        if(seed_output_mode == 1 )
        {
            if(HAL_GetTick() < 5000) //初始化自检
            {
                tppwm_value = 1000;
                sow_rotate_value = 1000;
            }
            //发送转速 和阀门
            vk_canbuf[0] = 0xf1;
            vk_canbuf[1] = tppwm_value & 0xff;
            vk_canbuf[2] = (tppwm_value >> 8) & 0xff;
            vk_canbuf[3] = sow_rotate_value & 0xff;
            vk_canbuf[4] = (sow_rotate_value >> 8) & 0xff;
            vk_canbuf[5] = 0xfe;
            vk_canbuf[6] = 0xfe;
            vk_canbuf[7] = vk_count;

            vk_count++;
            can_send_msg_normal((unsigned char *)&vk_canbuf, 8, 0x81324);
        }

        if(vk_count >= 255)
        {
            vk_count = 0;
        }
        
    }
}

Device_type Dev;
comp_status seed_dev_link_status = COMP_NOEXIST;

void check_dev_type_link(void )
{
    int error_time = 0;

    error_time = HAL_GetTick() - Dev.Seed_Link.recv_time;

    
    if(Dev.Seed_Link.connect_status == COMP_NORMAL && error_time > 5000)
    {
        Dev.Seed_Link.connect_status = COMP_LOST;
        memset(&Dev.Seed.facid, 0, 7);
    }

    error_time = HAL_GetTick() - Dev.Weight_Link.recv_time;

    if(Dev.Pump_Link.connect_status == COMP_NORMAL && error_time > 5000)
    {
        Dev.Pump_Link.connect_status = COMP_LOST;
        memset(&Dev.Weight.facid, 0, 16);
    }

    

    error_time = HAL_GetTick() - Dev.Pump_Link.recv_time;

    if(Dev.Pump_Link.connect_status == COMP_NORMAL && error_time > 5000)
    {
        Dev.Pump_Link.connect_status = COMP_LOST;
        memset(&Dev.Pump.facid, 0, 11);
    }

    //

    error_time = HAL_GetTick() - Dev.Nozzle_Link.recv_time;


    if(Dev.Nozzle_Link.connect_status == COMP_NORMAL && error_time > 5000)
    {
        Dev.Nozzle_Link.connect_status = COMP_LOST;
        memset(&Dev.Nozzle.facid, 0, 11);
    }

    //

    error_time = HAL_GetTick() - Dev.Arm_Link.recv_time;

    if(Dev.Arm_Link.connect_status == COMP_NORMAL && error_time > 5000)
    {
        Dev.Arm_Link.connect_status = COMP_LOST;
        memset(&Dev.Arm.facid, 0, 2);
    }

}

void DM_obs_test( void )
{
    static uint32_t time_50hz = 0;
    static uint32_t time_49hz = 0;
    static uint32_t time_48hz = 0;
    static uint32_t time_47hz = 0;
    uint8_t can_buf[8] = {0};
    if(Check_Timer_Ready(&time_50hz,20))
    {
        memcpy(&can_buf[0],&planep.QuaterQ0,4);
        memcpy(&can_buf[4],&planep.QuaterQ1,4);
        can_send_msg_normal((unsigned char *)&can_buf, 8, 0x2345);   
    }

    if(Check_Timer_Ready(&time_49hz,20))
    {
        memcpy(&can_buf[0],&planep.QuaterQ2,4);
        memcpy(&can_buf[4],&planep.QuaterQ3,4);
        can_send_msg_normal((unsigned char *)&can_buf, 8, 0x2346);
    }

    if(Check_Timer_Ready(&time_48hz,50))
    {
        memcpy(&can_buf[0],&planep.pos_x,4);
        memcpy(&can_buf[4],&planep.pos_y,4);
        can_send_msg_normal((unsigned char *)&can_buf, 8, 0x2347);
    }

    if(Check_Timer_Ready(&time_47hz,50))
    {
        memcpy(&can_buf[0],&planep.pos_z,4);
        memcpy(&can_buf[4],&planep.pos_flag,4);
        can_send_msg_normal((unsigned char *)&can_buf, 8, 0x2348);
    }


}
/**
  * @file    update_device_type_data
  * @brief   更新设备信息
  * @param    
  * @details 
  * @author  Zhang Sir 
 **/
void  update_device_type_data(void)
{   
    //50hz更新 5hz发送
    static uint32_t time_1hz = 0;
    static uint32_t time_330hz = 0;
    static uint32_t time_5hz = 0;
    static uint32_t time_50hz = 0;

    if(Check_Timer_Ready(&time_1hz,_1_HZ_))
    {
        Check_dev_link(&Dev.Seed_Link,5000,(char *)&Dev.Seed.facid,sizeof(Seed_info));
        Check_dev_link(&Dev.Weight_Link,5000,(char *)&Dev.Weight.facid,sizeof(Weight_info));
        Check_dev_link(&Dev.Pump_Link,5000,(char *)&Dev.Pump.facid,sizeof(Pump_info));
        Check_dev_link(&Dev.Nozzle_Link,5000,(char *)&Dev.Nozzle.facid,sizeof(Nozzle_info));
        Check_dev_link(&Dev.Arm_Link,5000,(char *)&Dev.Arm.facid,sizeof(Arm_info));
        Check_dev_link(&Dev.Flow_Link1,5000,(char *)&Dev.Flow.facid,sizeof(Flow_info));
        Check_dev_link(&Dev.Checklow_Link,5000,(char *)&Dev.Checklow.facid,sizeof(CheckLow_info));
        Check_dev_link(&Distributor_Link,5000,(char *)&distributor.arm_sensor1,sizeof(distributor_info));
        Check_dev_link(&Dev.Current_Link,5000,(char *)&Dev.Current.facid,sizeof(Current_info));
        Check_dev_link(&Dev.Lackloss_Link,5000,NULL,0);
        Check_dev_link(&Dev.L_pump1_Link,5000,(char *)&Dev.L_pump1.facid,sizeof(Linear_pump_info));
        Check_dev_link(&Dev.L_pump2_Link,5000,(char *)&Dev.L_pump2.facid,sizeof(Linear_pump_info));
        
        check_radar_link_status();
    }

    if(Check_Timer_Ready(&time_330hz,330))
    {
        //EFT播撒称重
        can_sendmsg_eft();

    }
    if(Check_Timer_Ready(&time_5hz,_5_HZ_))
    {
        //VK播撒称重
        can_sendmsg_VK();
        //流量计
        can_sendmsg_flow();
        //分电板
        Can_sendmsg_to_distributor();
        
    } 

    if(Check_Timer_Ready(&time_50hz,_50_HZ_))
    {
        //tes_mmm();
        //好盈电调水泵
        Hobbywing_esc_func();
        //播撒器
        if(Dev.Seed_Link.connect_status == COMP_NORMAL)
        {
            switch (Dev.Seed.facid)
            {
            case FAC_VK:
                Dev.Seed.angle = 0;
                Dev.Seed.speed = 0;
                Dev.Seed.caplow = 0;
                Dev.Seed.warn = 0;
                break;
            case FAC_EFT:
                Dev.Seed.angle = eft_info.valve_angel;
                Dev.Seed.speed = eft_info.speed_rpm * 10;
                Dev.Seed.caplow = eft_info.warn_status & 0x1;
                Dev.Seed.warn = eft_info.warn_status >> 1;
                break;
            case FAC_QIFEI:
                Dev.Seed.angle = 0;
                Dev.Seed.speed = 0;
                Dev.Seed.caplow = 0;
                Dev.Seed.warn = 0;//weight_vkinfo.warn_status;
                break;

            default:
                break;
            }
        }

        //称重
        if(Dev.Weight_Link.connect_status == COMP_NORMAL)
        {
            switch (Dev.Weight.facid)
            {
            case FAC_VK:
                Dev.Weight.mode = weight_vkinfo.mode;
                Dev.Weight.kg = weight_vkinfo.weight / 10;
                Dev.Weight.rate = weight_vkinfo.weight_rate;
                Dev.Weight.k1 = weight_vkinfo.senor_k[0];
                Dev.Weight.k2 = weight_vkinfo.senor_k[1];
                Dev.Weight.k3 = weight_vkinfo.senor_k[2];
                Dev.Weight.k4 = weight_vkinfo.senor_k[3];
                Dev.Weight.warn = weight_vkinfo.warn_status;
                Dev.Weight.single_sensor[0] = weight_vkinfo.Single_sensor[0];
                Dev.Weight.single_sensor[1] = weight_vkinfo.Single_sensor[1];
                Dev.Weight.single_sensor[2] = weight_vkinfo.Single_sensor[2];
                Dev.Weight.single_sensor[3] = weight_vkinfo.Single_sensor[3];
                break;
            case FAC_EFT:
                Dev.Weight.mode = 0;
                Dev.Weight.kg = eft_info.weight / 10;
                Dev.Weight.rate = 0;
                Dev.Weight.k1 = eft_info.seed_k[0];
                Dev.Weight.k2 = eft_info.seed_k[1];
                Dev.Weight.k3 = eft_info.seed_k[2];
                Dev.Weight.k4 = 0;
                Dev.Weight.warn = eft_info.watering_warn_status >> 4 & 0xff;
                break;
            case FAC_QIFEI:
                Dev.Weight.mode = weight_vkinfo.mode;
                Dev.Weight.kg = weight_vkinfo.weight / 10;
                Dev.Weight.rate = weight_vkinfo.weight_rate;
                Dev.Weight.k1 = weight_vkinfo.senor_k[0];
                Dev.Weight.k2 = weight_vkinfo.senor_k[1];
                Dev.Weight.k3 = weight_vkinfo.senor_k[2];
                Dev.Weight.k4 = weight_vkinfo.senor_k[3];
                Dev.Weight.warn = weight_vkinfo.warn_status;
                Dev.Weight.dose_rate  = weight_vkinfo.dose_rate;
                break;
            default:
                break;
            }
        }

        //水泵
        if(Dev.Pump_Link.connect_status == COMP_NORMAL)
        {
            switch (Dev.Pump.facid)
            {
            case FAC_VK:
                
                break;
            case FAC_EFT:
                Dev.Pump.rpm1 = eft_info.pump1_rpm * 60;
                Dev.Pump.rpm2 = eft_info.pump2_rpm * 60;
                Dev.Pump.rpm3 = 0;
                Dev.Pump.rpm4 = 0;

                if( (eft_info.watering_warn_status & 0x3) != 0)
                {
                    Dev.Pump.warn = (eft_info.watering_warn_status & 0x3) + (eft_info.watering_warn_status & 0xf0);
                }
                else if((eft_info.watering_warn_status & 0x3) == 0)
                {
                    Dev.Pump.warn = 0;
                }
                Dev.Pump.warn = 0; //屏蔽报警
                break;

            // case FAC_HW_ESC:
            //     Dev.Pump.rpm1 = EscMsg[1].motorRPM;
            //     Dev.Pump.rpm2 = EscMsg[2].motorRPM;
            //     Dev.Pump.rpm3 = 0;
            //     Dev.Pump.rpm4 = 0;
            //     Dev.Pump.warn = EscMsg[1].warn_flag;
                // break;
            default:
                break;
            }
        }

        //离心喷头
        if(Dev.Nozzle_Link.connect_status == COMP_NORMAL)
        {
            switch (Dev.Nozzle.facid)
            {
            case FAC_VK:
                
                break;
            case FAC_EFT:
                Dev.Nozzle.rpm1 = eft_info.cent1_rpm * 60;
                Dev.Nozzle.rpm2 = eft_info.cent2_rpm * 60;
                Dev.Nozzle.rpm3 = 0;
                Dev.Nozzle.rpm4 = 0;

                if( (eft_info.watering_warn_status & 0xc) != 0)
                {
                    Dev.Nozzle.warn = ((eft_info.watering_warn_status >> 2) & 0x3) + (eft_info.watering_warn_status & 0xf0);
                }
                else if((eft_info.watering_warn_status & 0xc) == 0)
                {
                    Dev.Nozzle.warn = 0;
                }
                Dev.Nozzle.warn = 0; //屏蔽报警
                break;
            default:
                break;
            }
        }    

        //机臂传感器
        if(Dev.Arm_Link.connect_status == COMP_NORMAL)
        {
            switch (Dev.Arm.facid)
            {
            case FAC_VK:
                break;
            case FAC_EFT:
                Dev.Arm.status = eft_info.enginearm_lock;
                break;
            case FAC_VK_ALLINONE:

                Dev.Arm.status = (((uint8_t)HAL_GPIO_ReadPin( GPIOB, GPIO_PIN_12 ) & 0x1) << 3) | \
                                    (((uint8_t)HAL_GPIO_ReadPin( GPIOB, GPIO_PIN_13 ) & 0x1) << 2) | \
                                    (((uint8_t)HAL_GPIO_ReadPin( GPIOB, GPIO_PIN_14 ) & 0x1)) | \
                                    (((uint8_t)HAL_GPIO_ReadPin( GPIOB, GPIO_PIN_15 ) & 0x1) << 1);
                break;  
            default:
                break;
            }
        }

        //线性水泵
        if(Dev.L_pump1_Link.connect_status == COMP_NORMAL)
        {
            switch (Dev.L_pump1.facid)
            {
            case FAC_VK:
                break;
            case FAC_EFT:
                break;
            case FAC_HW_ESC:
                Dev.L_pump1.warn = EscMsg[1].warn_flag;
                Dev.L_pump1.rpm = EscMsg[1].motorRPM;
                break;
            default:
                break;
            }
        }
        if(Dev.L_pump2_Link.connect_status == COMP_NORMAL)
        {
            switch (Dev.L_pump2.facid)
            {
            case FAC_VK:
                break;
            case FAC_EFT:
                break;
            case FAC_HW_ESC:
                Dev.L_pump2.warn = EscMsg[2].warn_flag;
                Dev.L_pump2.rpm = EscMsg[2].motorRPM;
                break;
            default:
                break;
            }    
        }

        

        // if(Dev.L_pump2_Link.connect_status == COMP_NORMAL)
        // {
        //     switch (Dev.L_pump2.facid)
        //     {
        //     case FAC_VK:
        //         break;
        //     case FAC_HW_ESC:
        //         Dev.L_pump2.warn = EscMsg[1].warn_flag;
        //         Dev.L_pump2.rpm =  EscMsg[1].motorRPM;        
        //         break;
        //     default:
        //         break;
        //     }    

        // }

        //智能电池
        if(Dev.Bms_Link.connect_status == COMP_NORMAL)
        {
            Dev.Bms.index = 0;

            Dev.Bms.facid = bms_data.serial_num;

            bms_data.bms_version[0] = 'V';
            bms_data.bms_version[1] = 'K';
            bms_data.bms_version[2] = '3';

            bms_data.bms_group = group_num;

            memcpy(&Dev.Bms.buf[Dev.Bms.index],&bms_data.bms_version,5 + bms_data.bms_num * 2);
            Dev.Bms.index += (5 + bms_data.bms_num * 2);

            memcpy(&Dev.Bms.buf[Dev.Bms.index],&bms_data.bms_ids,43);
            Dev.Bms.index += 43;

            memset(&Dev.Bms.buf[Dev.Bms.index],0,2); //EFT协议新加插头问题
            Dev.Bms.index += 2;

            if(bms_data.bms_group == 2)
            {
                memcpy(&Dev.Bms.buf[Dev.Bms.index],&bms_data2.bms_num,1 + bms_data2.bms_num * 2);
                Dev.Bms.index += (1 + bms_data2.bms_num * 2);

                memcpy(&Dev.Bms.buf[Dev.Bms.index],&bms_data2.bms_ids,43);
                Dev.Bms.index += 43;

                memset(&Dev.Bms.buf[Dev.Bms.index],0,2); //EFT协议新加插头问题
                Dev.Bms.index += 2;
            }
        }

        if(_Aqx_bms_link.connect_status == COMP_NORMAL)
        {   
            Dev.Bms.facid = FAC_QX_BMS;
            Dev.Bms.buf[0] = 0;//电池串数
            Dev.Bms.index = 0;
			Dev.Bms.index++;
            for(uint8_t i = 0; i < 4; i++)
            {
                if(_qx_bms_link[i].connect_status == COMP_NORMAL)
                {
                    Dev.Bms.buf[0]++;
                    memcpy(&Dev.Bms.buf[Dev.Bms.index],&_qx_bms[i].bat_voltage,sizeof(qingxie_bms_status_t));
                    Dev.Bms.index += sizeof(qingxie_bms_status_t);
                }
            }
        }


        //流量计、断料记、雷达数据一直发
        if(true/*Dev.Flow_Link1.connect_status == COMP_NORMAL*/) 
        {
            switch (Dev.Flow.facid)
            {
            case FAC_VK:
                Dev.Flow.speed1 = flow_dev1.speed;
                Dev.Flow.speed2 = flow_dev2.speed;
                Dev.Flow.ml1 = flow_dev1.irq_count;
                Dev.Flow.ml2 = flow_dev2.irq_count;
                Dev.Flow.warn1 = 0;
                Dev.Flow.warn2 = 0;
                Dev.Flow.k1 = 0;
                Dev.Flow.k2 = 0;
                break;
            case FAC_MIMO_SIG:// APP需要识别单双管
                Dev.Flow.speed1 = flow_mimo1.flow_speed;
                Dev.Flow.speed2 = flow_mimo2.flow_speed;
                Dev.Flow.ml1 = (flow_mimo1.total_ml + 65535 * flow_mimo1.overturn_count) * FlOW_KP * 60 / 1000;
                Dev.Flow.ml2 = (flow_mimo2.total_ml + 65535 * flow_mimo2.overturn_count) * FlOW_KP * 60 / 1000;
                Dev.Flow.warn1 = flow_mimo1.status;
                Dev.Flow.warn2 = flow_mimo2.status;
                Dev.Flow.k1 = flow_mimo1.flow_k;
                Dev.Flow.k2 = flow_mimo2.flow_k;
                break;
            case FAC_MIMO_DOU:
                Dev.Flow.speed1 = flow_mimo1.flow_speed;
                Dev.Flow.speed2 = flow_mimo2.flow_speed;
                Dev.Flow.ml1 = (flow_mimo1.total_ml + 65535 * flow_mimo1.overturn_count) * FlOW_KP * 60 / 1000;
                Dev.Flow.ml2 = (flow_mimo2.total_ml + 65535 * flow_mimo2.overturn_count) * FlOW_KP * 60 / 1000;
                Dev.Flow.warn1 = flow_mimo1.status;
                Dev.Flow.warn2 = flow_mimo2.status;
                Dev.Flow.k1 = flow_mimo1.flow_k;
                Dev.Flow.k2 = flow_mimo2.flow_k;
                break;
            case FAC_QIFEI_SIG:
                Dev.Flow.speed1 = flow_inf.ch1.speed;
                Dev.Flow.speed2 = flow_inf.ch2.speed;
                Dev.Flow.ml1 = (flow_inf.ch1.ml + 65535 * flow_inf.ch1.overturn) * FlOW_KP * 60 / 1000;
                Dev.Flow.ml2 = (flow_inf.ch2.ml + 65535 * flow_inf.ch2.overturn) * FlOW_KP * 60 / 1000;
                Dev.Flow.warn1 = flow_inf.ch1.warn;
                Dev.Flow.warn2 = flow_inf.ch2.warn;
                Dev.Flow.k1 = flow_inf.ch1.k;
                Dev.Flow.k2 = flow_inf.ch2.k;
                break;
            case FAC_QIFEI_DOU: // 12做个调换
                Dev.Flow.speed1 = flow_inf.ch2.speed;
                Dev.Flow.speed2 = flow_inf.ch1.speed;
                Dev.Flow.ml1 = (flow_inf.ch2.ml + 65535 * flow_inf.ch2.overturn) * FlOW_KP * 60 / 1000;
                Dev.Flow.ml2 = (flow_inf.ch1.ml + 65535 * flow_inf.ch1.overturn) * FlOW_KP * 60 / 1000;
                Dev.Flow.warn1 = flow_inf.ch2.warn;
                Dev.Flow.warn2 = flow_inf.ch1.warn;
                Dev.Flow.k1 = flow_inf.ch2.k;
                Dev.Flow.k2 = flow_inf.ch1.k;
                break;
            case FAC_HW_ESC:
                Dev.Flow.speed1 = EscMsg[1].motorRPM / FlOW_KP;
                Dev.Flow.speed2 = EscMsg[2].motorRPM / FlOW_KP;
                Dev.Flow.ml1 = 0;//(flow_inf.ch1.ml + 65535 * flow_inf.ch1.overturn) * FlOW_KP * 60 / 1000;
                Dev.Flow.ml2 = 0;//(flow_inf.ch2.ml + 65535 * flow_inf.ch2.overturn) * FlOW_KP * 60 / 1000;
                Dev.Flow.warn1 = 0;
                Dev.Flow.warn2 = 0;
                Dev.Flow.k1 = 0;
                Dev.Flow.k2 = 0;
                break;
            default:
                break;
            }

            switch (Dev.Checklow.facid)
            {
            case FAC_VK:
                Dev.Checklow.L1_status = (uint16_t)L3_status;
                Dev.Checklow.L2_status = (uint16_t)L4_status;
                Dev.Checklow.liquid_persent = decode_liquid_info();
                Dev.Checklow.seed_lackloss = mimo_lackloss.status;
                Dev.Checklow.warn = 0;
                Dev.Checklow.lack_distance = mimo_lackloss.distance / 10; //cm
                Dev.Checklow.lack_power = mimo_lackloss.strength;
                break;
            case FAC_DM_CHECKLOW:
                Dev.Checklow.L1_status = (uint16_t)L3_status;
                Dev.Checklow.L2_status = (uint16_t)L4_status;
                Dev.Checklow.liquid_persent = decode_liquid_info();
                Dev.Checklow.seed_lackloss = DM_lackloss.status;
                Dev.Checklow.warn = 0;
                Dev.Checklow.lack_distance = DM_lackloss.distance;
                Dev.Checklow.lack_power = DM_lackloss.strength;
                break;
            default:
                break;
            }

            //雷达
            Dev.Radar.height_T = get_radar_info(MSGID_SET_T_RADAR,0);
            
            Dev.Radar.distance_F_Y = get_radar_info(MSGID_SET_F_RADAR,OBS_Y);
            Dev.Radar.distance_F_Y_ori = Dev.Radar.distance_F_Y;
            // if((Dev.Radar.distance_F_Y > 0) && (Fobs_handle_function() == false))
            //     Dev.Radar.distance_F_Y = 0;
            Dev.Radar.distance_F_X = get_radar_info(MSGID_SET_F_RADAR,OBS_X);
            Dev.Radar.signal_F_qulity = mimo_f_info.signal_qulity;
            
            
            Dev.Radar.distance_B_Y = get_radar_info(MSGID_SET_B_RADAR,OBS_Y);
            Dev.Radar.distance_B_Y_ori = Dev.Radar.distance_B_Y;
            // if((Dev.Radar.distance_B_Y > 0) && (Bobs_handle_function() == false))
            //     Dev.Radar.distance_B_Y = 0;
            Dev.Radar.distance_B_X  = get_radar_info(MSGID_SET_B_RADAR,OBS_X);
            Dev.Radar.signal_B_qulity = mimo_b_info.signal_qulity;
            

            terrain_is_link = Dev.Radar.height_T  > -1? true:false;
            obs_f_is_link = Dev.Radar.distance_F_Y  > -1? true:false;
            obs_b_is_link = Dev.Radar.distance_B_Y > -1? true:false;

        }

        if(Dev.Current_Link.connect_status == COMP_NORMAL)
        {
           Dev.Current.facid = FAC_VK;

            Dev.Current.tempture = power_BatteryInfo.temperature * 10;
            Dev.Current.voltage = power_BatteryInfo.voltage * 10;
            Dev.Current.current = power_BatteryInfo.current * 10;
        }


        if(Dev.Part_Tradar_Link.connect_status == COMP_NORMAL)
        {
            Dev.Part_radarT.height_part1 = T_radar[0].Distance * 0.05f * 100 * cos(T_radar[0].Amuzith * 0.1f / RAD);
            Dev.Part_radarT.height_part2 = T_radar[1].Distance * 0.05f * 100 * cos(T_radar[1].Amuzith * 0.1f / RAD);
            Dev.Part_radarT.height_part3 = T_radar[2].Distance * 0.05f * 100 * cos(T_radar[2].Amuzith * 0.1f / RAD);
        }

        if(Dev.Temp_Sensor_Link.connect_status == COMP_NORMAL)
        {
            switch (Dev.Temp_sensor.facid)
            {
            case FAC_EFT:
                Dev.Temp_sensor.Positive_pole = Temp_sensor.Positive_pole;
                Dev.Temp_sensor.Negative_pole = Temp_sensor.Negative_pole;
                break;
            
            default:
                break;
            }
        }
    }
}

comp_status QIFEI_link_status = COMP_NOEXIST;
uint32_t get_QIFEI_time = 0;
volatile char get_QIFEI_time_sem = 0;
unsigned int get_QIFEI_time_err = 0;


void QIFEI_recieved_hookfuction(uint32_t CanID, uint8_t data[], uint8_t len)
{
    uint8_t id = 0;

    QIFEI_link_status = COMP_NORMAL;
    get_QIFEI_time = HAL_GetTick();

    id = data[0];
    switch (CanID)
    {
    case 0x0AD2:
        break;
    case 0x0AD0:
        switch (id)
        {
        case 0x01:
            break;
        
        default:
            break;
        }
        break;
    default:
        break;
    }
}