#include #include #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 #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 (MIN_Points / 2)) { for(int k = index_40prc; k > 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 ); } }