EFTZ_PMU/user_src/soft_device.c

#include "soft_device.h"
#include "main_task.h"
#include "soft_obstacle.h"
#include "soft_terrain.h"
#include "usart_data_handle.h"
#include "soft_seed_weight.h"
#include "soft_bms.h"
#include "soft_flow.h"
#include "soft_radar_handle.h"
#include "string.h"
#include "soft_water.h"
#include "math.h"
#include "soft_update.h"
#include "config.h"
#include "usart_data_handle.h" 
#include "soft_can.h"
Device_type Dev = { .Flow.facid = 0xff};
Weight_cal weight_order;
uint8_t weight_init_count;

bool recv_fmu_seed_info;
Dev_seed_init seed_init;
/**
  * @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(&Dev.Current_Link,5000,(char *)&Dev.Current.facid,sizeof(Current_info));
        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_dev_link(&Dev.Eft_CanDev_Link[2],5000,(char *)&nozzle1,sizeof(water_dev));
        Check_dev_link(&Dev.Eft_CanDev_Link[3],5000,(char *)&nozzle2,sizeof(water_dev));
        Check_dev_link(&Dev.Eft_CanDev_Link[4],5000,(char *)&nozzle3,sizeof(water_dev));
        Check_dev_link(&Dev.Eft_CanDev_Link[5],5000,(char *)&nozzle4,sizeof(water_dev));
        Check_dev_link(&Dev.DM_Part_Fradar_Link,5000,(char *)&Dev.DM_Part_radarF.facid,sizeof(Part_Fradar));
        Check_dev_link(&Dev.DM_Part_Bradar_Link,5000,(char *)&Dev.DM_Part_radarB.facid,sizeof(Part_Fradar));
        Check_dev_link(&Dev.Temperature_Sensor_Link,5000,(char *)Dev.Temperature_Sensor.facid,sizeof(Temperature_Sensor_info));
        check_radar_link_status();
    }

    if(Check_Timer_Ready(&time_330hz,330) && eft_update.update_flag == false)
    {
        //EFT播撒称重
        can_sendmsg_eft();
        //EFT水泵离心
        can_sendmsg_eft_water();
        //EFT上电配置CANID
        set_eft_dev_canid_func();
    }
    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_))
    {
        //好盈电调水泵
        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 = weight_vkinfo.warn_status;
                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;
                if(spread_type == SPREAD_JIAOLONG)
                {
                    Dev.Seed.speed = turntable.rpm;
                    Dev.Seed.churn_rpm = churn.rpm;
                    Dev.Seed.warn = churn.error_status;
                    Dev.Seed.churn_warn = churn.reserve;
                }
                break;
            case FAC_QIFEI:
                Dev.Seed.angle = 0;
                Dev.Seed.speed = 0;
                Dev.Seed.caplow = 0;
                Dev.Seed.warn = weight_vkinfo.warn_status;
                break;
            case FAC_CHURN_SEED:
                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;
                if(spread_type == SPREAD_JIAOLONG)
                {
                    Dev.Seed.speed = turntable.rpm;
                    Dev.Seed.churn_rpm = churn.rpm;
                    Dev.Seed.warn = churn.error_status ;
                    Dev.Seed.churn_warn = churn.reserve;
                }
                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;
                break;
            case FAC_EFT:

                Dev.Weight.mode = 0;
                Dev.Weight.kg = fplate.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 = 0;
                if(weight_type == WEIGHT_TRANFER)
                {
                    Dev.Weight.kg = fplate.weight;//20单位g  70单位10g
                    Dev.Weight.k1 = z70weight.info.k1;
                    Dev.Weight.k2 = z70weight.info.k2;
                    Dev.Weight.k3 = z70weight.info.k3;
                    Dev.Weight.k4 = z70weight.info.k4;
                    if(Z70_LiftingWeight_exist == true)
                        Dev.Weight.warn = (LiftingWeight_warning << 8) & 0xffff;
                }
                break;
            case FAC_LIFTWEIGHT:
                Dev.Weight.mode = 0;
                Dev.Weight.kg = fplate.weight;
                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 = (LiftingWeight_warning << 8) & 0xff00;  
                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 = pump1.rpm;
                Dev.Pump.rpm2 = pump2.rpm;
                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:
                if(Dev.Eft_CanDev_Link[2].connect_status != COMP_NORMAL)
                    nozzle1.rpm = -1;
                if(Dev.Eft_CanDev_Link[3].connect_status != COMP_NORMAL)
                    nozzle2.rpm = -1;
                if(Dev.Eft_CanDev_Link[4].connect_status != COMP_NORMAL)
                    nozzle3.rpm = -1;
                if(Dev.Eft_CanDev_Link[5].connect_status != COMP_NORMAL)
                    nozzle4.rpm = -1;

                Dev.Nozzle.rpm1 = nozzle1.rpm;
                Dev.Nozzle.rpm2 = nozzle2.rpm;
                Dev.Nozzle.rpm3 = nozzle3.rpm;
                Dev.Nozzle.rpm4 = nozzle4.rpm;
                if(nozzle_type == NOZZLE_NORMAL)
                {
                    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 = fplate.arm1 | (fplate.arm2 << 1) | (fplate.arm3 << 2) | (fplate.arm4 << 3);
                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:
                Dev.L_pump1.warn = pump1.error_status;
                Dev.L_pump1.rpm =  pump1.rpm;   
                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:
                Dev.L_pump2.warn = pump2.error_status;
                Dev.L_pump2.rpm =  pump2.rpm;   
                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;
            
            memcpy(&Dev.Bms.buf[Dev.Bms.index],&fplate.bms_plug_tempture,1);
            Dev.Bms.index += 2;


            if(bms_data.bms_group == 2)
            {
                memcpy(&Dev.Bms.buf[Dev.Bms.index],&bms_data2.bms_num,1 + bms_data.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;

                memcpy(&Dev.Bms.buf[Dev.Bms.index],&fplate.bms_plug_tempture,1);
                Dev.Bms.index += 2;
            }
        }


        //流量计、断料记、雷达数据一直发
        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:
                if(spary_type == SPARY_TRANSFER)
                {
                    Dev.Checklow.L1_status = !mimo_lackloss.status;
                }
                else 
                {
                    Dev.Checklow.L1_status = (uint16_t)L3_status;
                }

                Dev.Checklow.L2_status = (uint16_t)L4_status;
                Dev.Checklow.seed_lackloss = mimo_lackloss.status;
                Dev.Checklow.liquid_persent = decode_liquid_info();
                Dev.Checklow.warn = 0;
                Dev.Checklow.lack_distance = mimo_lackloss.distance / 10;
                Dev.Checklow.lack_power = mimo_lackloss.strength;
                break;
            default:
                break;
            }

            //雷达
            Dev.Radar.height_T = get_radar_info(T_RADAR,0);
            
            Dev.Radar.distance_F_Y = get_radar_info(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(F_RADAR,OBS_X);
            Dev.Radar.signal_F_qulity = mimo_f_info.signal_qulity;
            
            
            Dev.Radar.distance_B_Y = get_radar_info(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(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);
            // Dev.Part_radarT.raw_height_part1 = T_radar[0].Distance * 0.05f * 100;
            // Dev.Part_radarT.raw_height_part2 = T_radar[1].Distance * 0.05f * 100;
            // Dev.Part_radarT.raw_height_part3 = T_radar[2].Distance * 0.05f * 100;
            // Dev.Part_radarT.angle_part1 = T_radar[0].Amuzith; //0.1
            // Dev.Part_radarT.angle_part2 = T_radar[1].Amuzith;
            // Dev.Part_radarT.angle_part3 = T_radar[2].Amuzith;            

            for(uint8_t i = 0; i < 5; i++)
            {
                Dev.Part_radarT.height_part[i] = T_radar[i].Distance * 0.05f * 100 * cos(T_radar[i].Amuzith * 0.1f / RAD);
                Dev.Part_radarT.raw_height_part[i] = T_radar[i].Distance * 0.05f * 100;
                Dev.Part_radarT.angle_part[i] = T_radar[i].Amuzith;
            }
        }

        if(Dev.Part_Fradar_Link.connect_status == COMP_NORMAL)
        {
            for(uint8_t i = 0; i < 3; i++)
            {
                Dev.Part_radarF.height_part[i] = F_radar[i].Distance * 0.05f * 100 * cos(F_radar[i].Amuzith * 0.1f / RAD);
                Dev.Part_radarF.raw_height_part[i] = F_radar[i].Distance * 0.05f * 100;
                Dev.Part_radarF.angle_Horpart[i] = F_radar[i].Amuzith;
                Dev.Part_radarF.angle_Velpart[i] = F_radar[i].Elevation;
            }
        }

        if(Dev.Part_Bradar_Link.connect_status == COMP_NORMAL)
        {
            for(uint8_t i = 0; i < 3; i++)
            {
                Dev.Part_radarB.height_part[i] = B_radar[i].Distance * 0.05f * 100 * cos(B_radar[i].Amuzith * 0.1f / RAD);
                Dev.Part_radarB.raw_height_part[i] = B_radar[i].Distance * 0.05f * 100;
                Dev.Part_radarB.angle_Horpart[i] = B_radar[i].Amuzith;
                Dev.Part_radarB.angle_Velpart[i] = B_radar[i].Elevation;
            }
        }
        if(Dev.DM_Part_Fradar_Link.connect_status == COMP_NORMAL && Dev.DM_Part_radarF.facid == FAC_DM_RF_4D)
        {
            Dev.DM_Part_radarF.X1 = F_4DRadar[0][0];
            Dev.DM_Part_radarF.Y1 = F_4DRadar[0][1];
            Dev.DM_Part_radarF.Z1 = F_4DRadar[0][2];
            Dev.DM_Part_radarF.X2 = F_4DRadar[1][0];
            Dev.DM_Part_radarF.Y2 = F_4DRadar[1][1];
            Dev.DM_Part_radarF.Z2 = F_4DRadar[1][2];
            Dev.DM_Part_radarF.X3 = F_4DRadar[2][0];
            Dev.DM_Part_radarF.Y3 = F_4DRadar[2][1];
            Dev.DM_Part_radarF.Z3 = F_4DRadar[2][2];
        }
        if(Dev.DM_Part_Bradar_Link.connect_status == COMP_NORMAL && Dev.DM_Part_radarB.facid == FAC_DM_RB_4D)
        {
             Dev.DM_Part_radarB.X1 = B_4DRadar[0][0];
            Dev.DM_Part_radarB.Y1 = B_4DRadar[0][1];
            Dev.DM_Part_radarB.Z1 = B_4DRadar[0][2];
            Dev.DM_Part_radarB.X2 = B_4DRadar[1][0];
            Dev.DM_Part_radarB.Y2 = B_4DRadar[1][1];
            Dev.DM_Part_radarB.Z2 = B_4DRadar[1][2];
            Dev.DM_Part_radarB.X3 = B_4DRadar[2][0];
            Dev.DM_Part_radarB.Y3 = B_4DRadar[2][1];
            Dev.DM_Part_radarB.Z3 = B_4DRadar[2][2];
        }
        if(Dev.Temperature_Sensor_Link.connect_status == COMP_NORMAL)
        {
            Dev.Temperature_Sensor.Battery_positive_temperature = fplate.bms_plug_tempture;
            Dev.Temperature_Sensor.Battery_negative_temperature = fplate.bms_battery_tempture;
        }
        
    }
}

bool set_eftdev_canid_flag = false;
uint32_t set_eftdev_canid_time = 0;
uint16_t set_eftdev_canid_status = 0;
uint8_t auto_set_canid_flag = 0; // 0 自检，1设置ID，2结束
uint32_t auto_eft_dev_status = 0;

void set_eft_dev_canid_func( void )
{
    if(set_eftdev_canid_flag == true && HAL_GetTick() - set_eftdev_canid_time < 3000)
    {
        static uint16_t last_status = 0;
        if(last_status != set_eftdev_canid_status)
        {
            last_status = set_eftdev_canid_status;
            pmu_set_ack(_MSGID_SET,2,set_eftdev_canid_status,0);
        }
    }
    else if(set_eftdev_canid_flag == true && HAL_GetTick() - set_eftdev_canid_time > 3000)
    {
        set_eftdev_canid_flag = false;
        pmu_set_ack(_MSGID_SET,3,set_eftdev_canid_status,0);
    }

    if(HAL_GetTick() > 3000 && auto_set_canid_flag == 0)
    {
        for(uint8_t i = 0; i < 6; i++)
        {
            if(Dev.Eft_CanDev_Link[i].connect_status == COMP_NORMAL)
            {
                auto_eft_dev_status = auto_eft_dev_status | (1 << i);
            }
        }

        //6个设备全识别
        if((auto_eft_dev_status & 0xff) == 0x3f) //两水泵4喷头
            auto_set_canid_flag = NO_NEED_SET;
        //2水泵2喷头
        else if((auto_eft_dev_status & 0xff) == 0x33) //两水泵 后两个喷头
            auto_set_canid_flag = NO_NEED_SET;
        else if((auto_eft_dev_status & 0xff) == 0x31) //单水泵 后俩喷头
            auto_set_canid_flag = NO_NEED_SET;
        else
        {
            auto_set_canid_flag = NEED_SET;
            set_eftdev_canid_time = HAL_GetTick();
        }
    }

    //设置5S ID
    if(auto_set_canid_flag == NEED_SET && HAL_GetTick() - set_eftdev_canid_time > 5000)
        auto_set_canid_flag = NO_NEED_SET;
}
void DM_obs_test( void )
{
    if(((Dev.DM_Part_Fradar_Link.connect_status != COMP_NORMAL) || (Dev.DM_Part_radarF.facid != FAC_DM_RF_4D)) && 
        ((Dev.DM_Part_Bradar_Link.connect_status != COMP_NORMAL) || (Dev.DM_Part_radarB.facid != FAC_DM_RB_4D)))
        return;

    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_Func(CANID1, can_buf, 8, 0x2345, CAN_ID_EXT);
        //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_Func(CANID1, can_buf, 8, 0x2346, CAN_ID_EXT);
    }

    if(Check_Timer_Ready(&time_48hz,19))
    {
        memcpy(&can_buf[0],&planep.pos_x,4);
        memcpy(&can_buf[4],&planep.pos_y,4);
        Can_Send_Msg_Func(CANID1, can_buf, 8, 0x2347, CAN_ID_EXT);
    }

    if(Check_Timer_Ready(&time_47hz,19))
    {
        memcpy(&can_buf[0],&planep.pos_z,4);
        memcpy(&can_buf[4],&planep.pos_flag,4);
        Can_Send_Msg_Func(CANID1, can_buf, 8, 0x2348, CAN_ID_EXT);
    }
}