4d_ObstacleRadarDroneCAN/App/algorithm.c

#include <rtthread.h>
#include <stdio.h>
#include "gpio.h"
#include "iwdg.h"
#include "algorithm.h"
#include "log.h"
#include "drv_hwtimer.h"
#include "usart.h"
#include "dri_can.h"
#include "quaternion.h"
#include "euler.h"
#include "dri_Flash.h"
#include "stdlib.h"
#include <dronecan_msgs.h>


#define M_PI_ 3.1415926f
#define Rng_res  0.6958f                    //范围矫正值
#define Vel_res  0.3519f                    //速度校正值

#define MAX_FRAME_NUM 15                    //点云最大帧数
#define MAX_Z_AXIS_POINTS 70                    //点云最大帧数
bool filter_flag = 0;
bool terrain_flag = 0;

extern rt_sem_t cluster_sem_t;
static Point_Cloud_Frame radar_frame_buffer[MAX_FRAME_NUM];
static int frame_write_index = 0;
static FinalPointCloutStruct cluster;
static FinalPointCloutStruct TerrainPoints;

float q_now[4] = {1, 0, 0, 0};              //飞控传递的四元数
float p_now[3] = {0};                       //飞控坐标向量
uint8_t other[4] = {0};
int16_t euler[3] = {0};

static RegionResult ObstacleDistance = {0};

float min[2] = {0};
uint8_t can_obstacle[8] = {0};
uint8_t terrain_predict[50] = {0};
extern struct rt_mutex queue_lock;
extern uint8_t temp_buf[8192];
extern uint8_t temp_len;


typedef struct {
    uint32_t valid;
    float points_z[MAX_Z_AXIS_POINTS]; 
    uint32_t TerrainFlag;
    float TerrainHeight;

}TerrianBinStat;

TerrianBinStat terrain_bins[25] = {0};

/*KalmanFilter Parameters*/
uint32_t hitcnt = 0;
float cy_last = 0;
float cy_kfy = 0;
bool doReset = true;
uint32_t misscnt = 0;
uint32_t KFActive = 0;

//typedef struct {
//    float z[50];
//    uint32_t valid;
//    float z_axis;
//    uint32_t HeightFlag;
//}ObstacleHeight;
//ObstacleHeight ObstacleZ={0};
extern CanardInstance canard;
struct uavcan_equipment_range_sensor_Measurement radarMsg = {0};
struct uavcan_protocol_NodeStatus NodeStatusMsg = {0};
/**
  * @Name    ClusterTask
  * @brief
  * @param   parameter: [输入/出]
  * @retval
  * @author  zhutianyu
  * @Data    2025-10-11
 **/
void ClusterTask( void *parameter )
{
    while ( 1 )
    {
        rt_sem_take( cluster_sem_t, RT_WAITING_FOREVER );
//        rt_tick_t start = rt_tick_get();

        PointCloudFilter();
        PointCloudConvert();
        
        DetectObstacle_ByDynamicY();
//        if(confInfo.DeviceType == REAR_OBSTACLE_RADAR || confInfo.DeviceType == FRONT_OBSTACLE_RADAR)
//        {
//            TerrainPointsSegmentation();
//            TerrianPointsFilter();
//            TerrainPredict();   
//        }
//        
//        int16_t  tmp_x = 0, tmp_y = 0, tmp_z = 0;
        static float obstacleDistance = 10.0f;
        obstacleDistance += 1.0f;
        if(obstacleDistance >= 35 )
            obstacleDistance = 10;
//        if (ObstacleZ.HeightFlag == 1)
//        {
//            tmp_z = ( ( int16_t )( ObstacleZ.z_axis * 100 ) );
//            ObstacleZ.HeightFlag = 0;
//            ObstacleZ.z_axis = 0;
//        }
        
        if( DroneCAN_IsNodeIDAllocated() )
        {
            radarMsg.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_RADAR;
            radarMsg.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE;
            radarMsg.range = obstacleDistance;
            radarMsg.sensor_id = FRONT_OBSTACLE_RADAR;
            DroneCAN_SendRangeSensorMeasurement(&radarMsg);
        }

    }

}


void NodeStatusTask( void *parameter )
{
    ( void )parameter;

    while ( 1 )
    {
        if ( DroneCAN_IsNodeIDAllocated() )
        {
            NodeStatusMsg.uptime_sec = rt_tick_get() / RT_TICK_PER_SECOND;
            NodeStatusMsg.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;
            NodeStatusMsg.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;

            // 可以放你的雷达状态码，例如 bit0=雷达在线, bit1=测距有效, bit2=故障
            NodeStatusMsg.vendor_specific_status_code = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE;

            DroneCAN_SendNodeStatus( &NodeStatusMsg );
        }

        rt_thread_mdelay( UAVCAN_PROTOCOL_NODESTATUS_MAX_BROADCASTING_PERIOD_MS );
    }

}




typedef struct usart_can
{
    uint8_t head[8];
    uint32_t length;
    uint32_t rt_pointds;   // 类型
    uint32_t FrameIdxy;    // 帧索引
    uint32_t Tick;         // 时间计数器
    uint32_t NumPointUsed; // 有效点数统计
    uint8_t data[0];
} usart_can_data;

struct Can2PMU_Data RadarRawData = {0};

static uint32_t  a1 = 0, a = 0, power;
static uint32_t  numl1 = 0, numl2 = 0, numl3 = 0, numl4 = 0;
static float range = 0, azi = 0, ele = 0;
//static uint32_t positive_num  = 0;
//static uint32_t negative_num  = 0;
//static float fmu_yaw, fmu_pitch, fmu_roll;

/**
 * @brief 接收射频单元的原始点云数据并进行转换
 *
 */
void PointCloudFilter( void )
{

    usart_can_data *point_data = ( usart_can_data * )temp_buf;
    uint8_t *usart_data_p = point_data->data;
    static Point_Cloud_Frame radar_frame;
    memset( &radar_frame, 0, sizeof( radar_frame ) );
    radar_frame.ValidFlag = 1;

    //补偿角度转换
    float compensate_angle = 0;
    if ( confInfo.CompensateAngle <= 150 && confInfo.CompensateAngle >= -150 )
    {
        compensate_angle = ( float )confInfo.CompensateAngle / 10.0f * M_PI_ / 180.0f;
    }

    //点云转换
    for ( int i = 0; i < point_data -> NumPointUsed; i++ )
    {

        azi = usart_data_p[i * 8 + 2] * 0.5f - 60.0f;
        if ( azi < -45.0f || azi > 45.0f )  // 滤除偏航角+-45之外的点
            continue;
        azi = azi * M_PI_ / 180.0f + compensate_angle; //角度补偿

        range = usart_data_p[i * 8 + 0];

        numl1 =  ( usart_data_p[i * 8 + 4] );
        numl2 =  ( usart_data_p[i * 8 + 5] << 8 );
        numl3 =  ( usart_data_p[i * 8 + 6] << 16 );
        numl4 =  ( usart_data_p[i * 8 + 7] << 24 );
        power = numl1 + numl2 + numl3 + numl4;

        if ( PowerFilter( range, power ) == 0 )
            continue;

        ele = usart_data_p[i * 8 + 3] - 90.0f;
        ele = -( ele * M_PI_ / 180.0f );    // 电目俯仰角标定错误，需补偿
        range = range * Rng_res;
        radar_point *pt = &radar_frame.vaild_point_arr[radar_frame.vaild_point_num++];
        pt->z = range * cosf( ele ) * sinf( azi );  // 电目传输错误，X值需取反
        if ( confInfo.DeviceType == REAR_OBSTACLE_RADAR )
        {
            pt->y = -range * cosf( ele ) * cosf( azi );
            pt->x = -range * sinf( ele );

        }
        else if ( confInfo.DeviceType == FRONT_OBSTACLE_RADAR )
        {
            pt->y = range * cosf( ele ) * cosf( azi );
            pt->x = range * sinf( ele );
        }
        else if ( confInfo.DeviceType == LEFT_OBSTACLE_RADAR )
        {
            pt->x = -range * cosf( ele ) * cosf( azi );
            pt->y = range * sinf( ele );
        }
        else if ( confInfo.DeviceType == RIGHT_OBSTACLE_RADAR )
        {
            pt->x = range * cosf( ele ) * cosf( azi );
            pt->y = -range * sinf( ele );
        }

        pt->pow = power;

        if ( radar_frame.vaild_point_num >= 99 )
        {
            log_info( "point_cloud_filter full\n" );
            break;
        }
    }


    if ( radar_frame.vaild_point_num > 0 )
    {
        
        //获取当前姿态四元数和相对起飞点向量(ENU)
        rt_enter_critical();
        
//          fmu_roll    = (float) euler[0] * (M_PI_ / 180.0f);
//          fmu_pitch   = (float) euler[1] * (M_PI_ / 180.0f);
//          fmu_yaw     = (float) euler[2] * (M_PI_ / 180.0f);
//          Quaternion_Enu_ByEuler(fmu_yaw, fmu_pitch, fmu_roll, q_now);
        memcpy( radar_frame.q, q_now, 16 );
        memcpy( radar_frame.p, p_now, 12 );
        rt_exit_critical();

        euler_angle_t euler_temp = {0};
        float q_new[4] = {0};
        memcpy ( q_new, radar_frame.q, 16 );
        Euler_ByQuaternionEnu( q_new, &euler_temp );
        
        Quaternion_Enu_ByEuler( euler_temp.yaw, 0, 0, q_new );
        float xyz[3] = {0};
        uint8_t new_count = 0;
        for ( int j = 0; j < radar_frame.vaild_point_num ; j++ )
        {
            //点云旋转
            quat_rotate_point_filter( q_new, radar_frame.q,
                                      radar_frame.vaild_point_arr[j].x,
                                      radar_frame.vaild_point_arr[j].y,
                                      radar_frame.vaild_point_arr[j].z,
                                      xyz );
            if ( confInfo.DeviceType == REAR_OBSTACLE_RADAR )
            {    
                xyz[1] = -xyz[1];
            }
            
            if ( confInfo.DeviceType == LEFT_OBSTACLE_RADAR )
            {
                float tempLeft = xyz[0];
                xyz[0] = xyz[1];
                xyz[1] = -tempLeft;
            }
            
            if ( confInfo.DeviceType == RIGHT_OBSTACLE_RADAR )
            {
                float tempRight = xyz[0];
                xyz[0] = -xyz[1];
                xyz[1] = tempRight;
            }
            
            //去除前三米的倍频影响
            if ( xyz[2] > 0 || xyz[1] >3  )
            {
                radar_frame.vaild_point_arr[new_count++] = radar_frame.vaild_point_arr[j];
            }
                
        }
        radar_frame.vaild_point_num = new_count;
       
        //留存点云 做处理
        memcpy( &radar_frame_buffer[frame_write_index], &radar_frame, sizeof( Point_Cloud_Frame ) );

        frame_write_index = ( frame_write_index + 1 ) % MAX_FRAME_NUM;

    }

}



/**
 * @brief 原始点点云过滤，以距离和强度共同作为过滤条件
 * @param fRangeValue 点云距离信息
 * @param uPowerValue 点云能量信息
 *
 */
bool PowerFilter( float fRangeValue, uint32_t uPowerValue )
{
    bool bFlag = 0;
    if ( fRangeValue < 10 && uPowerValue > 1200000 )
        bFlag = 1;
    else if ( fRangeValue >= 10 && fRangeValue < 20 && uPowerValue > 800000 )
        bFlag = 1;
    else if ( fRangeValue >= 20 && fRangeValue < 30 && uPowerValue > 500000 )
        bFlag = 1;
    else if ( fRangeValue >= 30 && fRangeValue < 40 && uPowerValue > 400000 )
        bFlag = 1;
    else if ( fRangeValue >= 40 && fRangeValue < 50 && uPowerValue > 300000 )
        bFlag = 1;

    return bFlag ;
}

/**
 * @brief 点云坐标系旋转
 *
 */
void PointCloudConvert( void )
{
    cluster.vaild_len = 0 ;
    TerrainPoints.vaild_len = 0;

    uint8_t index = frame_write_index;
    euler_angle_t euler_temp = {0};
    //判断最新帧的位置
    index = ( index == 0 ) ? ( MAX_FRAME_NUM - 1 ) : ( index - 1 );

    float q_new[4] = {0};
    memcpy ( q_new, radar_frame_buffer[index].q, 16 );
    Euler_ByQuaternionEnu( q_new, &euler_temp );
    Quaternion_Enu_ByEuler( euler_temp.yaw, 0, 0, q_new );
    float point_new[3] = {0};
    memcpy ( point_new, radar_frame_buffer[index].p, 12 );
    float xyz[3] = {0};

    //盲区距离
    float blind_distance = ( float )( confInfo.BlindDistance / 100.0f );
    
    uint16_t terrain_hight = 0;
    memcpy(&terrain_hight, &other[0], 2);
    uint8_t drone_status_flag = other[3];
    
    //高度过滤系数
    uint16_t hight_filter_value = 5;
    if ( confInfo.HeightFilterValue <= 10 )
    {
        // 10 为最高，0为最低 这里做取反，方便过滤计算
        hight_filter_value = 10 - confInfo.HeightFilterValue;
    }
    if ( terrain_hight > 0 )
    {
        if (terrain_hight <= 100 )
        {
            hight_filter_value = (hight_filter_value <= 1) ? hight_filter_value : 1;
        } else if ( terrain_hight <= 150 )
        {
            hight_filter_value = (hight_filter_value <= 2) ? hight_filter_value : 2;
        } else if ( terrain_hight <= 200 )
        {
            hight_filter_value = (hight_filter_value <= 3) ? hight_filter_value : 3;
        } else if ( terrain_hight <= 230 )
        {
            hight_filter_value = (hight_filter_value <= 4) ? hight_filter_value : 4;
        }
    }
    
    
    if (drone_status_flag == 0)
    {
        hight_filter_value = 0;
    }
    
    for ( int i = 0 ; i < MAX_FRAME_NUM ; i++ )
    {
        if ( radar_frame_buffer[i].ValidFlag == 0 )
            continue;
        for ( int j = 0; j < radar_frame_buffer[i].vaild_point_num ; j++ )
        {
            //点云旋转
            quat_rotate_point( q_new, radar_frame_buffer[i].q, point_new, radar_frame_buffer[i].p,
                               radar_frame_buffer[i].vaild_point_arr[j].x,
                               radar_frame_buffer[i].vaild_point_arr[j].y,
                               radar_frame_buffer[i].vaild_point_arr[j].z,
                               xyz );

            if ( confInfo.DeviceType == REAR_OBSTACLE_RADAR )
            {
                
                xyz[1] = -xyz[1];
            }
            
            if ( confInfo.DeviceType == LEFT_OBSTACLE_RADAR )
            {
                float tempLeft = xyz[0];
                xyz[0] = xyz[1];
                xyz[1] = -tempLeft;
            }
            
            if ( confInfo.DeviceType == RIGHT_OBSTACLE_RADAR )
            {
                float tempRight = xyz[0];
                xyz[0] = -xyz[1];
                xyz[1] = tempRight;
            }

            //根据Y值匹配Z轴过滤值
            filter_flag = 1;
            terrain_flag = 1;
            if ( xyz[1] <= blind_distance || xyz[1] >= 35.0f )
            {
                filter_flag = 0;
                terrain_flag = 0;
            }
            else if ( xyz[0] >= 5.0f || xyz[0] <= -5.0f )
            {
                filter_flag = 0;
                terrain_flag = 0;
            }
            else if ( xyz[1] < 5 )
            {
                if ( xyz[2] <= -0.1f * hight_filter_value )
                    filter_flag = 0;
            }
            else if ( xyz[1] >= 5.0f && xyz[1] < 10.0f )
            {
                if ( xyz[2] <= -0.12f * hight_filter_value )
                    filter_flag = 0;
            }
            else if ( xyz[1] >= 10 )
            {
                if ( xyz[2] <= -0.1f * hight_filter_value )
                    filter_flag = 0;
            }
            
            if ( filter_flag )
            {
                if (cluster.vaild_len < 1499 )
                {
                    cluster.all_point[cluster.vaild_len].x = xyz[0];
                    cluster.all_point[cluster.vaild_len].y = xyz[1];
                    cluster.all_point[cluster.vaild_len].z = xyz[2];
                    cluster.all_point[cluster.vaild_len].pow = radar_frame_buffer[i].vaild_point_arr[j].pow;
                    cluster.vaild_len++;
                } else {
                    log_info( "point_cloud_convert full\n" );
                    break;
                }
                
            }
            
            if( terrain_flag && TerrainPoints.vaild_len < 1499)
            {
                TerrainPoints.all_point[TerrainPoints.vaild_len].x = xyz[0];
                TerrainPoints.all_point[TerrainPoints.vaild_len].y = xyz[1];
                TerrainPoints.all_point[TerrainPoints.vaild_len].z = xyz[2];
                TerrainPoints.all_point[TerrainPoints.vaild_len].pow = radar_frame_buffer[i].vaild_point_arr[j].pow;
                TerrainPoints.vaild_len++;
                if (TerrainPoints.vaild_len >= 1499)
                {
                    log_info( "TerrainPoints is full\n" );
                    break;
                }
            }
            
            
        }

    }

}


/**
  * @Name    quat_rotate_point_filter
  * @brief
  * @param   q_new[4]: [输入/出]
  * @retval
  * @author  zhutianyu
  * @Data    2025-10-11
 **/
void quat_rotate_point_filter( float q_new[4], float q_old[4], float x, float y, float z, float * xyz )
{
    float point_src[3] = {x, y, z};
    float point_dst[3] = {0};
    float temp[3] = {0};
    // 旋转公式：q * p * q_inv
    Quaternion_Conj( q_old, point_src, temp );

    Quaternion_ConjInv( q_new, temp, point_dst );


    xyz[0] = point_dst[0];
    xyz[1] = point_dst[1];
    xyz[2] = point_dst[2];
}

void TerrainPointsSegmentation (void)
{   
    
    for(uint8_t j=0; j < 25 ; j++)
    {
        terrain_bins[j].valid = 0;
        terrain_bins[j].TerrainHeight = 0;
        terrain_bins[j].TerrainFlag = 0;
    }

    for(int i=0; i < TerrainPoints.vaild_len; i++)
    {
        if (TerrainPoints.all_point[i].y <= 25)
        {
            for(uint8_t j=0; j < 25 ; j++)
            {
                if(TerrainPoints.all_point[i].y >= j && TerrainPoints.all_point[i].y < (j+1))
                {
                    if(terrain_bins[j].valid < MAX_Z_AXIS_POINTS - 1)
                        terrain_bins[j].points_z[terrain_bins[j].valid++] = TerrainPoints.all_point[i].z;
                }
            }
        }
    }
}

static int Z_ValueCompare(const void * a, const void *b)
{
    float a_value = *(const float *) a;
    float b_value = *(const float *) b;
    return (a_value > b_value) - (a_value < b_value);
    
}

static float prctile( float * data, uint32_t vaild_len, float precentile)
{
    if ( vaild_len <= 0)
        return 0;
    
    qsort(data, vaild_len, sizeof(float), Z_ValueCompare);
    
    if (precentile <= 0)
    { 
        float value = data[0];
        return value;
    }
    
    if (precentile >= 100)
    {
        float value = data[vaild_len - 1];
        return value;
    }
    
    float pos = (precentile / 100.0f) * (vaild_len - 1);
    int idx = (int)pos;
    float frac = pos - idx;
    
    float result = 0;
    if (idx >= vaild_len - 1)
    {
        result = data[vaild_len - 1];
        
    } else {
        result = data[idx] + frac * (data[idx+1] - data[idx]);
        
    }
    
    return result;
        
}

#define MIN_Points 10

void TerrianPointsFilter (void )
{
    float value_25prctile = 0;
    float value_75prctile = 0;
    float IQR = 0;
    float lowerFence = 0;
    float upperFence = 0;
    uint32_t valid_temp = 0;
//    uint32_t ObstacleFlag = 0;
    // ObstacleZ.valid = 0;
    // uint16_t hight_filter_value = 5;
    // if ( ObstacleDistance.valid == 1)    
    // {
    //     ObstacleDistance.valid = 0;
    //     ObstacleFlag = (uint32_t)ObstacleDistance.y;
    //     uint16_t terrain_hight = 0;
    //     memcpy(&terrain_hight, &other[0], 2);
    //     uint8_t drone_status_flag = other[3];
        
    //     //高度过滤系数
        
    //     if ( confInfo.HeightFilterValue <= 10 )
    //     {
    //         // 10 为最高，0为最低 这里做取反，方便过滤计算
    //         hight_filter_value = 10 - confInfo.HeightFilterValue;
    //     }
        
    //     if (terrain_hight <= 100 )
    //     {
    //         hight_filter_value = (hight_filter_value <= 1) ? hight_filter_value : 1;
    //     } else if ( terrain_hight <= 150 )
    //     {
    //         hight_filter_value = (hight_filter_value <= 2) ? hight_filter_value : 2;
    //     } else if ( terrain_hight <= 200 )
    //     {
    //         hight_filter_value = (hight_filter_value <= 3) ? hight_filter_value : 3;
    //     } else if ( terrain_hight <= 230 )
    //     {
    //         hight_filter_value = (hight_filter_value <= 4) ? hight_filter_value : 4;
    //     }
        
    //     if (drone_status_flag == 0)
    //     {
    //         hight_filter_value = 0;
    //     }
        
    // }
    
    for(uint32_t i = 0; i < 25; i++)
    {
        if(terrain_bins[i].valid >= MIN_Points)
        {
            value_25prctile = prctile(terrain_bins[i].points_z, terrain_bins[i].valid, 25);
            value_75prctile = prctile(terrain_bins[i].points_z, terrain_bins[i].valid, 75);
            IQR = value_75prctile - value_25prctile;
            lowerFence = value_25prctile - 1.5f * IQR;
            upperFence = value_75prctile + 1.5f * IQR;
            for(int j =0; j < terrain_bins[i].valid; j++)
            {
                if(terrain_bins[i].points_z[j] >= lowerFence && terrain_bins[i].points_z[j] <= upperFence)
                    terrain_bins[i].points_z[valid_temp++] = terrain_bins[i].points_z[j];
            }
            terrain_bins[i].valid = valid_temp;
            int index_40prc = ( int ) ceilf(0.5f * valid_temp);
            int index_80prc = ( int ) floorf(0.9f * valid_temp);
            
            // /*计算障碍物z轴高度*/
            // if (i == ObstacleFlag && ObstacleFlag != 0)
            // {
            //     for (uint32_t k = 0; k < terrain_bins[i].valid; k++)
            //     {
            //         filter_flag = 1;
            //         if (  i < 5 )
            //         {
            //             if ( terrain_bins[i].points_z[k] <= -0.1f * hight_filter_value )
            //                 filter_flag = 0;
            //         }
            //         else if ( i >= 5.0f && i < 10.0f )
            //         {
            //             if ( terrain_bins[i].points_z[k] <= -0.12f * hight_filter_value )
            //                 filter_flag = 0;
            //         }
            //         else if ( i >= 10 )
            //         {
            //             if ( terrain_bins[i].points_z[k] <= -0.1f * hight_filter_value )
            //                 filter_flag = 0;
            //         }
                    
            //         if(filter_flag == 1)
            //         {
            //             ObstacleZ.z[ObstacleZ.valid++] = terrain_bins[i].points_z[k];
            //             if (ObstacleZ.valid >= 49 )
            //                 break;
            //         } 
            //     }
                
            //     float height_25prctile = prctile(ObstacleZ.z, ObstacleZ.valid, 25);
            //     float height_75prctile = prctile(ObstacleZ.z, ObstacleZ.valid, 75);
            //     float height_IQR = height_75prctile - height_25prctile;
            //     float height_lowerFence = value_25prctile - 1.5f * IQR;
            //     float height_upperFence = value_75prctile + 1.5f * IQR;
            //     uint32_t height_valid_temp = 0;    
            //     for(int j =0; j < ObstacleZ.valid; j++)
            //     {
            //         if(ObstacleZ.z[j] >= height_lowerFence && ObstacleZ.z[j] <= height_upperFence)
            //             ObstacleZ.z[height_valid_temp++] = ObstacleZ.z[j];
            //     }
            //     ObstacleZ.valid = height_valid_temp;
            //     int height_index_80prc = ( int ) ceilf(0.8f * height_valid_temp);
            //     int height_index_90prc = ( int ) floorf(0.9f * height_valid_temp);
                
            //     for(int k = height_index_80prc; k <height_index_90prc; k++)
            //     {
            //         ObstacleZ.z_axis += ObstacleZ.z[k];
    
            //     }
            //     ObstacleZ.z_axis /= (height_index_90prc - height_index_80prc);
            //     ObstacleZ.HeightFlag = 1;
            // }
            // /*障碍物z轴高度计算结束*/
            
            
            if (terrain_bins[i].valid > (MIN_Points / 2))
            {
                for(int k = index_40prc; k <index_80prc; k++)
                {
                    terrain_bins[i].TerrainHeight += terrain_bins[i].points_z[k];
    
                }
                terrain_bins[i].TerrainHeight /= (index_80prc - index_40prc);
                terrain_bins[i].TerrainFlag = 1;
                
            }
            
            valid_temp = 0;
            
        }
    }
}

rt_tick_t Terrain_time_old = 0, Terrain_time_new = 0;
void TerrainPredict ( void )
{
    memset(terrain_predict, 0, 50);
    int16_t temp = 0;
    for(int i = 0 ; i < 25 ; i++)
    {

        if (terrain_bins[i].TerrainFlag == 1)
        {
            temp = ((int16_t)(terrain_bins[i].TerrainHeight * 100));
        } else {
            temp = 0;
        }
        if( i<3 )
        {
            temp = 0;
        }
        memcpy(&terrain_predict[2*i],&temp,2); 
     
    }
    Terrain_time_new = rt_tick_get();

    uint16_t Can_Crc = Get_Crc16( terrain_predict, 50 );
    FlagUnion TerrainFlag = {0};
    uint8_t FrameNum = 10;
    uint8_t uFlag = 0;
    uint8_t send_buf[8] = {0};
    for (int i = 0; i< FrameNum; i++)
    {
        memset(send_buf, 0, 8);
        uFlag = calculate_flag( i+1, FrameNum, &TerrainFlag );
        if (i == 0)
        {
            send_buf[0] = 25;
            send_buf[1] = Terrain_time_new ;
            send_buf[2] = Terrain_time_new >> 8;
            send_buf[3] = Can_Crc;
            send_buf[4] = Can_Crc >> 8;

            send_buf[7] = uFlag;
        } else {
            if ( i != 9 )
            {
                send_buf[0] = 0xFE;
                memcpy( &send_buf[1], &terrain_predict[6*(i-1)], 6);
                send_buf[7] = uFlag;   
            }
            else{
                send_buf[0] = 0xFE;
                memcpy( &send_buf[1], &terrain_predict[6*(i-1)], 2);
                send_buf[7] = uFlag; 
            }
            
        }
        
        //发送Can数据
        MyCAN_Transmit( TerrainHeightCanID, 8, send_buf, 0 );
    }

}

#define MAX_RANGE_Y  50.0f   // 最大检测距离
#define MAX_BINS     35      // 最大可能bin数
typedef struct
{
    uint16_t count;
    float sum_pow;
    float sum_y;
    float sum_x;
    float sum_z;
    float avg_y;
    float avg_pow;
    float avg_x;
    float avg_z;
    uint8_t valid;
} BinStat;


void DetectObstacle_ByDynamicY( void )
{
    BinStat bins[MAX_BINS] = {0};
    int bin_count = 0;
    float y_step = 0;
    ObstacleDistance.valid = 1;

    // 初始化动态bin边界
    float bin_edges[MAX_BINS + 1];
    float y = 0;
    while ( y < MAX_RANGE_Y && bin_count < MAX_BINS )
    {
        if ( y < 10.0f ) y_step = 1.0f;
        else if ( y < 16.0f ) y_step = 1.5f;
        else y_step = 2.0f;
        bin_edges[bin_count] = y;
        y += y_step;
        bin_count++;
    }

    
    for ( int i = 0; i < cluster.vaild_len; i++ )
    {
        //float x = cluster.all_point[i].x;
        float y = cluster.all_point[i].y;
        //float z = cluster.all_point[i].z;
        //float pow = cluster.all_point[i].pow;

        // 找到对应的Y区间bin
        int8_t idx = -1;
        for ( uint8_t b = 0; b < bin_count; b++ )
        {
            if ( y >= bin_edges[b] && y < bin_edges[b + 1] )
            {
                idx = b;
                break;
            }
        }
        if ( idx < 0 || idx >= MAX_BINS ) continue;

        bins[idx].count++;
        //bins[idx].sum_pow += pow;
        bins[idx].sum_y += y;
    }

    // 计算平均值并判断障碍条件
    int valid_bins[MAX_BINS];
    int valid_cnt = 0;

    for ( uint8_t i = 0; i < bin_count; i++ )
    {
        if ( bins[i].count == 0 ) continue;

        bins[i].avg_pow = bins[i].sum_pow / bins[i].count;
        bins[i].avg_y   = bins[i].sum_y   / bins[i].count;

        // 动态最小点数阈值
        uint8_t min_points = 0;
        if ( bins[i].avg_y <= 10.0f ) min_points = 8; //10
        else if ( bins[i].avg_y <= 20.0f ) min_points = 5; //7
        else min_points = 4; //5

        if ( bins[i].count >= min_points )
        {
            bins[i].valid = 1;
            valid_bins[valid_cnt++] = i;
        }
    }
    
    float DetectDist = confInfo.MaxDist / 100.0f;
    
    float y1 = 0;
    // 找出最近有效区间
    if ( valid_cnt != 0 ) 
    {
        int idx1 = valid_bins[0];

        //障碍距离
        y1 = bins[idx1].avg_y; 
    }

    
    if ( y1 <= 30.0f ) //25
    {
        ObstacleDistance.y = y1;
        ObstacleDistance.valid = 1;
    }
    else
    {
        ObstacleDistance.y = 0;
        ObstacleDistance.valid = 0;
    }
    
    if (ObstacleDistance.y > 0 )
    {
        misscnt = 0;
        hitcnt  += 1;
        if (hitcnt < 3)
        {
            cy_kfy = 0;
            doReset = true;
            KFActive = 0;
            cy_last = ObstacleDistance.y;
            
        }
        else
        {   
            if (KFActive == 0)
                doReset = true;
            cy_kfy = KelmanFilter_1Dimension(ObstacleDistance.y, doReset);
            doReset = false;
            KFActive = 1;
            cy_last = cy_kfy;
        }
 
    }
    else
    {
        hitcnt = 0;
        
        if (KFActive == 1)
        {
            misscnt += 1;
            if(misscnt<=3)
            {
                cy_kfy = KelmanFilter_1Dimension(cy_last, doReset);
                cy_last = cy_kfy;
                doReset = false;
            }
            else 
            {
                cy_kfy = 0;
                cy_last = 0;
                doReset = false;
                KFActive = 0;
                misscnt = 0;
    
            }

        }
        else 
        {
            cy_kfy = ObstacleDistance.y;
            doReset = true;
            misscnt = 0;
        }
 
    }

}


void quat_rotate_point( float q_new[4], float q_old[4], float p_enu_new[3], float p_enu_old[3], float x, float y, float z, float * xyz )
{
    float point_src[3] = {x, y, z};
    float point_dst[3] = {0};
    float temp[3] = {0};
    uint8_t i = 0;
    // 旋转公式：q * p * q_inv
    Quaternion_Conj( q_old, point_src, temp );

    //坐标向量计算，对历史点云做偏移
    for ( i = 0 ; i < 3 ; i++ )
    {
        temp[i] = temp[i] + p_enu_old[i] - p_enu_new[i];
    }

    Quaternion_ConjInv( q_new, temp, point_dst );


    xyz[0] = point_dst[0];
    xyz[1] = point_dst[1];
    xyz[2] = point_dst[2];
}


// 计算并设置FlagUnion的原始字节值
uint8_t calculate_flag( int index, int pack, FlagUnion* _f )
{
    _f->flags.seq = index ;                    // 帧序号（从1开始）
    _f->flags.sof = ( index == 1 ) ? 1 : 0;    // 首帧SOF置1
    _f->flags.eof = ( index == pack ) ? 1 : 0; // 末帧EOF置1
    return _f->u8flag;
}
rt_tick_t can_old = 0, can_new = 0;

uint8_t calculate_sequence( int pack, FlagUnion * _f){
    uint8_t sequence = 0;
    if ( _f ->flags.sof == 1)
        sequence = 1;
    
    return sequence;
}

/**
 * @brief can口输出数据用于飞控记录雷达原始点云数据、四元数及位置向量
 *
 */
void parse_radar_response( const uint8_t *usart_data, uint32_t len )
{
    usart_can_data *can_data = ( usart_can_data * )usart_data;
    memset( &RadarRawData, 0, sizeof( RadarRawData ) );
    uint8_t send_buff[8] = {0};
    RadarRawData.head[0] = can_data->NumPointUsed;
    uint8_t *radar_info_data = &( RadarRawData.data[0] );
    memset( send_buff, 0, 8 );
    uint8_t *usart_data_p = can_data->data;
    uint16_t num = 0;
    uint16_t data_len = 0;
    for ( int i = 0; i < ( can_data->NumPointUsed ) * 5; i++ )
    {

        switch ( ( i + 1 ) % 5 )
        {
            case 1:
                // 获取d1idx
                radar_info_data[data_len++] = usart_data_p[num * 8];
                break;
            case 2:
                // 获取d2idx
                radar_info_data[data_len++] = usart_data_p[num * 8 + 1];
                break;
            case 3:
                // 获取d3idx
                radar_info_data[data_len++] = usart_data_p[num * 8 + 2];
                break;
            case 4:
                // 获取d4idx
                radar_info_data[data_len++] = usart_data_p[num * 8 + 3];
                break;
            case 0:
                // 获取power
                numl1 =     ( usart_data_p[num * 8 + 4] );
                numl2 =     ( usart_data_p[num * 8 + 5] << 8 );
                numl3 =     ( usart_data_p[num * 8 + 6] << 16 );
                numl4 =     ( usart_data_p[num * 8 + 7] << 24 );
                a = numl1 + numl2 + numl3 + numl4;
                a1 = a / 10000;
                if ( a1 >= 255 )   a1 = 255;
                radar_info_data[data_len++] = a1;
                num++;
                break;
            default:
                break;
        }

    }
    can_new = rt_tick_get();
    rt_tick_t can_time = can_new - can_old;
    can_old = can_new;
    uint16_t Can_Crc = Get_Crc16( radar_info_data, data_len );
    uint8_t FrameNum = ( data_len % 7 ) == 0 ? ( data_len / 7 ) + 1 : ( data_len / 7 ) + 1 + 1;

    FlagUnion Can_Flag = {0};
    RadarRawData.head[1] = can_time;
    RadarRawData.head[2] = can_time >> 8;
    RadarRawData.head[3] = Can_Crc;
    RadarRawData.head[4] = Can_Crc >> 8;

    uint8_t uFlag = 0;
    uint16_t offset = 0, remain = 0;
    for ( int i = 1 ; i <= FrameNum ; i++ )
    {
        memset( send_buff, 0, 8 );
        uFlag = calculate_flag( i, FrameNum, &Can_Flag );
        if ( i == 1 )   // 帧头
            memcpy( &send_buff[0], &RadarRawData.head[0], 7 );
        else
        {
            offset = ( i - 2 ) * 7;
            remain = data_len - offset;
            if ( remain >= 7 ) remain = 7;
            memcpy( send_buff, &RadarRawData.data[offset], remain );
        }
        send_buff[7] = uFlag;

        //发送Can数据
        MyCAN_Transmit( RawPointCloudCanID, 8, send_buff, 0 );

    }

}