/***************************************************************************** * Copyright (c) 2019, Nations Technologies Inc. * * All rights reserved. * **************************************************************************** * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * - Redistributions of source code must retain the above copyright notice, * this list of conditions and the disclaimer below. * * Nations' name may not be used to endorse or promote products derived from * this software without specific prior written permission. * * DISCLAIMER: THIS SOFTWARE IS PROVIDED BY NATIONS "AS IS" AND ANY EXPRESS OR * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE * DISCLAIMED. IN NO EVENT SHALL NATIONS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /** * @file application.c * @author Nations * @version v1.0.0 * * @copyright Copyright (c) 2019, Nations Technologies Inc. All rights reserved. */ #include #include #include "gpio.h" #include "iwdg.h" #include "application.h" #include "log.h" #include "drv_hwtimer.h" #include "usart.h" #include "algorithm.h" #include "dri_Flash.h" #include "dri_can.h" ///////////////////////////////////////////////////////////////////// ALIGN( RT_ALIGN_SIZE ) #define RADAR_SOFTWARE_VERSION 260623 #define FRONT_OBSTACLE_ACK_ID (0xA81302) //4D前避障雷达参数配置回复 Can ID #define REAR_OBSTACLE_ACK_ID (0xB81302) //4D后避障雷达参数配置回复 Can ID #define LEFT_OBSTACLE_ACK_ID (0xC81302) //4D左避障雷达参数配置回复 Can ID #define RIGHT_OBSTACLE_ACK_ID (0xD81302) //4D右避障雷达参数配置回复 Can ID #define FRONT_OBSTACLE_RECEIVE_ID (0xA81300) //4D前避障雷达参数配置接收 Can ID #define REAR_OBSTACLE_RECEIVE_ID (0xB81300) //4D后避障雷达参数配置接收 Can ID #define LEFT_OBSTACLE_RECEIVE_ID (0xC81300) //4D左避障雷达参数配置接收 Can ID #define RIGHT_OBSTACLE_RECEIVE_ID (0xD81300) //4D右避障雷达参数配置接收 Can ID static rt_uint8_t InitStack[1024]; static rt_uint8_t NodeStatusStack[512]; static rt_uint8_t ParameterStack[1024]; static rt_uint8_t AppStack[2048]; static rt_uint8_t ClusterStack[8192]; static struct rt_thread InitHandle; static struct rt_thread ParameterHandle; static struct rt_thread AppHandle; static struct rt_thread ClusterHandle; static struct rt_thread NodeStatusHandle; // 信号量 rt_sem_t uart_sem_t = RT_NULL; rt_sem_t cluster_sem_t = RT_NULL; rt_sem_t parameter_sem_t = RT_NULL; ////////////////////////////////////////////////////////////////// static void AppTask( void *parameter ); static void ParameterTask( void *parameter ); ////////////////////////////////////////////////////////////////// uint8_t ParameterData[8] = {0}; uint32_t ParameterAckCanID = 0; uint32_t ParameterReceiveCanID = 0; uint32_t OstacleDistanceCanID = 0; uint32_t TerrainHeightCanID = 0; uint32_t RawPointCloudCanID = 0; uint32_t RceveiveCanID = 0; extern uint16_t canBitrate; /** * @brief Init任务 * * @param parameter */ static void InitTask( void *parameter ) { read_iap_info(); if ( iap_info.ValidFlag == IAP_VFLAG && iap_info.UpgradeFlag == IAP_FLAG ) { iap_info.UpgradeFlag = IAP_UN_FLAG; write_iap_info(); } // confInfo.DeviceType = THIS_DEVICE; // confInfo.MaxDist = 2500; // confInfo.RawDataFlag = 0; // confInfo.HeightFilterValue = 5; // confInfo.CompensateAngle = 0; // confInfo.PowerFilterLevel = 0; // confInfo.BlindDistance = 100; // write_conf_info(); read_conf_info(); canBitrate = confInfo.canBitrate; can_init(); DroneCAN_Start(); check_conf_info(); read_dev_info(); devInfo.soft_ver = RADAR_SOFTWARE_VERSION; write_dev_info(); read_boot_info(); if ( confInfo.DeviceType == FRONT_OBSTACLE_RADAR ) { TerrainHeightCanID = FRONT_TERRAIN_CAN_ID; ParameterReceiveCanID = FRONT_OBSTACLE_RECEIVE_ID; ParameterAckCanID = FRONT_OBSTACLE_ACK_ID; OstacleDistanceCanID = FRONT_OBSTACLE_CAN_ID; RawPointCloudCanID = FRONT_OBSTACLE_OUT; } if ( confInfo.DeviceType == REAR_OBSTACLE_RADAR ) { TerrainHeightCanID = REAR_TERRAIN_CAN_ID; ParameterReceiveCanID = REAR_OBSTACLE_RECEIVE_ID; ParameterAckCanID = REAR_OBSTACLE_ACK_ID; OstacleDistanceCanID = REAR_OBSTACLE_CAN_ID; RawPointCloudCanID = REAR_OBSTACLE_OUT; } if ( confInfo.DeviceType == LEFT_OBSTACLE_RADAR ) { ParameterReceiveCanID = LEFT_OBSTACLE_RECEIVE_ID; ParameterAckCanID = LEFT_OBSTACLE_ACK_ID; OstacleDistanceCanID = LEFT_OBSTACLE_CAN_ID; } if ( confInfo.DeviceType == RIGHT_OBSTACLE_RADAR ) { ParameterReceiveCanID = RIGHT_OBSTACLE_RECEIVE_ID; ParameterAckCanID = RIGHT_OBSTACLE_ACK_ID; OstacleDistanceCanID = RIGHT_OBSTACLE_CAN_ID; } read_dev_info(); rt_err_t result = RT_EOK; result = rt_thread_init( &ParameterHandle, "parameter", ParameterTask, RT_NULL, ( rt_uint8_t * )&ParameterStack[0], sizeof( ParameterStack ), 4, 1 ); if ( result == RT_EOK ) { rt_thread_startup( &ParameterHandle ); } result = rt_thread_init( &NodeStatusHandle, "nodestatus", NodeStatusTask, RT_NULL, ( rt_uint8_t * )&NodeStatusStack[0], sizeof( NodeStatusStack ), 10, 1 ); if ( result == RT_EOK ) { rt_thread_startup( &NodeStatusHandle ); } result = rt_thread_init( &AppHandle, "app", AppTask, RT_NULL, ( rt_uint8_t * )&AppStack[0], sizeof( AppStack ), 2, 1 ); if ( result == RT_EOK ) { rt_thread_startup( &AppHandle ); } rt_thread_init( &ClusterHandle, "cluster", ClusterTask, RT_NULL, ( rt_uint8_t * )&ClusterStack[0], sizeof( ClusterStack ), 3, 1 ); if ( result == RT_EOK ) { rt_thread_startup( &ClusterHandle ); } } //串口数据帧头、帧尾 uint8_t head_data[8] = {'R', 'a', 'd', 'a', 'r', 'E', 'y', 'e'}; uint8_t tail_data[4] = {'R', 'E', 'N', 'D'}; static uint8_t t_cnt = 0; uint8_t temp_buf[8192] = {0}; uint32_t temp_len = 0; struct rt_mutex queue_lock; /** * @brief 原始点云处理 * * @param parameter */ static void AppTask( void *parameter ) { while ( 1 ) { rt_sem_take( uart2_rdata.sem, RT_WAITING_FOREVER ); uint16_t current_len = uart2_rdata.recv_len; uint8_t *current_buf = uart2_rdata.buff; if ( ( memcmp( current_buf, head_data, 8 ) == 0 ) && ( memcmp( current_buf + current_len - 4, tail_data, 4 ) == 0 ) ) { t_cnt++; if ( ( t_cnt % 2 ) == 0 ) { LED0_OFF(); } else { LED0_ON(); } if ( confInfo.RawDataFlag == 1 ) parse_radar_response( current_buf, current_len ); if ( rt_mutex_take( &queue_lock, 0 ) == RT_EOK ) { memcpy( temp_buf, uart2_rdata.buff, 8192 ); temp_len = uart2_rdata.recv_len; rt_mutex_release( &queue_lock ); rt_sem_release( cluster_sem_t ); } } memset( uart2_rdata.buff, 0, 8192 ); uart2_rdata.recv_len = 0; } } uint8_t ParameterAck[8] = {0}; FlagUnion ParameterFlag = {0}; uint8_t uFlag = 0; static void ParameterTask( void *parameter ) { while ( 1 ) { rt_sem_take( parameter_sem_t, RT_WAITING_FOREVER ); if ( RceveiveCanID != ParameterReceiveCanID ) continue; if ( ParameterData[0] == 1 ) //飞控获取雷达版本信息 { read_dev_info(); ParameterAck[0] = 1; memcpy( &ParameterAck[1], &devInfo.sn, sizeof( devInfo.sn ) ); uFlag = calculate_flag( 1, 2, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); memcpy( &ParameterAck[1], &devInfo.soft_ver, sizeof( devInfo.soft_ver ) ); memcpy( &ParameterAck[5], &bootInfo.boot_version, sizeof( bootInfo.boot_version ) ); uFlag = calculate_flag( 2, 2, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); } else if ( ParameterData[0] == 2 ) //设置雷达SN号 { memcpy( &devInfo.sn, &ParameterData[1], sizeof( devInfo.sn ) ); write_dev_info(); read_dev_info(); ParameterAck[0] = 2; memcpy( &ParameterAck[1], &devInfo.sn, sizeof( devInfo.sn ) ); uFlag = calculate_flag( 1, 1, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); } else if ( ParameterData[0] == 4 ) //设置雷达设备类型 { memcpy( &confInfo.DeviceType, &ParameterData[1], sizeof( confInfo.DeviceType ) ); write_conf_info(); read_conf_info(); ParameterAck[0] = 4; memcpy( &ParameterAck[1], &confInfo.DeviceType, sizeof( confInfo.DeviceType ) ); uFlag = calculate_flag( 1, 1, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); if ( confInfo.DeviceType == FRONT_OBSTACLE_RADAR ) { TerrainHeightCanID = FRONT_TERRAIN_CAN_ID; ParameterReceiveCanID = FRONT_OBSTACLE_RECEIVE_ID; ParameterAckCanID = FRONT_OBSTACLE_ACK_ID; OstacleDistanceCanID = FRONT_OBSTACLE_CAN_ID; RawPointCloudCanID = FRONT_OBSTACLE_OUT; } if ( confInfo.DeviceType == REAR_OBSTACLE_RADAR ) { TerrainHeightCanID = REAR_TERRAIN_CAN_ID; ParameterReceiveCanID = REAR_OBSTACLE_RECEIVE_ID; ParameterAckCanID = REAR_OBSTACLE_ACK_ID; OstacleDistanceCanID = REAR_OBSTACLE_CAN_ID; RawPointCloudCanID = REAR_OBSTACLE_OUT; } if ( confInfo.DeviceType == LEFT_OBSTACLE_RADAR ) { ParameterReceiveCanID = LEFT_OBSTACLE_RECEIVE_ID; ParameterAckCanID = LEFT_OBSTACLE_ACK_ID; OstacleDistanceCanID = LEFT_OBSTACLE_CAN_ID; } if ( confInfo.DeviceType == RIGHT_OBSTACLE_RADAR ) { ParameterReceiveCanID = RIGHT_OBSTACLE_RECEIVE_ID; ParameterAckCanID = RIGHT_OBSTACLE_ACK_ID; OstacleDistanceCanID = RIGHT_OBSTACLE_CAN_ID; } } else if ( ParameterData[0] == 5 ) //设置距离雷达盲区距离 { memcpy( &confInfo.BlindDistance, &ParameterData[1], sizeof( confInfo.BlindDistance ) ); write_conf_info(); read_conf_info(); ParameterAck[0] = 5; memcpy( &ParameterAck[1], &confInfo.BlindDistance, sizeof( confInfo.BlindDistance ) ); uFlag = calculate_flag( 1, 1, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); } else if ( ParameterData[0] == 6 ) //获取雷达UID { read_uid(uid); ParameterAck[0] = 6; memcpy( &ParameterAck[1], &uid[0], 6 ); uFlag = calculate_flag( 1, 2, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); memcpy( &ParameterAck[1], &uid[6], 6 ); uFlag = calculate_flag( 2, 2, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); } else if ( ParameterData[0] == 8 ) //获取雷达盲区距离 { read_conf_info(); ParameterAck[0] = 8; memcpy( &ParameterAck[1], &confInfo.BlindDistance, sizeof( confInfo.BlindDistance ) ); uFlag = calculate_flag( 1, 1, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); } else if ( ParameterData[0] == 10 ) //设置雷达点云数据开关 { memcpy( &confInfo.RawDataFlag, &ParameterData[1], sizeof( confInfo.RawDataFlag ) ); write_conf_info(); read_conf_info(); ParameterAck[0] = 10; memcpy( &ParameterAck[1], &confInfo.RawDataFlag, sizeof( confInfo.RawDataFlag ) ); uFlag = calculate_flag( 1, 1, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); } else if ( ParameterData[0] == 11 ) //获取雷达点云数据开关 { read_conf_info(); ParameterAck[0] = 11; memcpy( &ParameterAck[1], &confInfo.RawDataFlag, sizeof( confInfo.RawDataFlag ) ); uFlag = calculate_flag( 1, 1, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); } else if ( ParameterData[0] == 12 ) //设置雷达安装补偿角度 { memcpy( &confInfo.CompensateAngle, &ParameterData[1], sizeof( confInfo.CompensateAngle ) ); write_conf_info(); read_conf_info(); ParameterAck[0] = 12; memcpy( &ParameterAck[1], &confInfo.CompensateAngle, sizeof( confInfo.CompensateAngle ) ); uFlag = calculate_flag( 1, 1, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); } else if ( ParameterData[0] == 13 ) //获取雷达安装补偿角度 { read_conf_info(); ParameterAck[0] = 13; memcpy( &ParameterAck[1], &confInfo.CompensateAngle, sizeof( confInfo.CompensateAngle ) ); uFlag = calculate_flag( 1, 1, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); } else if ( ParameterData[0] == 14 ) //设置雷达高度过滤系数 { memcpy( &confInfo.HeightFilterValue, &ParameterData[1], sizeof( confInfo.HeightFilterValue ) ); write_conf_info(); read_conf_info(); ParameterAck[0] = 14; memcpy( &ParameterAck[1], &confInfo.HeightFilterValue, sizeof( confInfo.HeightFilterValue ) ); uFlag = calculate_flag( 1, 1, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); } else if ( ParameterData[0] == 15 ) //获取雷达高度过滤系数 { read_conf_info(); ParameterAck[0] = 15; memcpy( &ParameterAck[1], &confInfo.HeightFilterValue, sizeof( confInfo.HeightFilterValue ) ); uFlag = calculate_flag( 1, 1, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); } else if ( ParameterData[0] == 16 ) //设置雷达障碍反馈距离 { memcpy( &confInfo.MaxDist, &ParameterData[1], sizeof( confInfo.MaxDist ) ); write_conf_info(); read_conf_info(); ParameterAck[0] = 16; memcpy( &ParameterAck[1], &confInfo.MaxDist, sizeof( confInfo.MaxDist ) ); uFlag = calculate_flag( 1, 1, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); } else if ( ParameterData[0] == 17 ) //获取雷达障碍反馈距离 { read_conf_info(); ParameterAck[0] = 17; memcpy( &ParameterAck[1], &confInfo.MaxDist, sizeof( confInfo.MaxDist ) ); uFlag = calculate_flag( 1, 1, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); } else if ( ParameterData[0] == 18 ) //设置雷达升级模式 { memcpy( &confInfo.upgradeMode, &ParameterData[1], sizeof( confInfo.upgradeMode ) ); if (ParameterData[1] == 0) confInfo.canBitrate = 6; if (ParameterData[1] == 1) confInfo.canBitrate = 12; write_conf_info(); read_conf_info(); ParameterAck[0] = 18; memcpy( &ParameterAck[1], &confInfo.upgradeMode, sizeof( confInfo.upgradeMode ) ); uFlag = calculate_flag( 1, 1, &ParameterFlag ); ParameterAck[7] = uFlag; MyCAN_Transmit( ParameterAckCanID, 8, ParameterAck, 0 ); } memset( &ParameterAck[0], 0, 8 ); } } /** * @brief init application */ void rt_application_init( void ) { rt_err_t result = RT_EOK; log_info( "app init\r\n" ); result = rt_mutex_init( &queue_lock, "queue", RT_IPC_FLAG_PRIO ); if ( result != RT_EOK ) { log_info( "queue_lock init failed!\n" ); } rx_cache.sem = rt_sem_create( "Uart_r", 0, RT_IPC_FLAG_FIFO ); log_assert( rx_cache.sem, RT_NULL, 1 ); uart_sem_t = rt_sem_create( "Uart_t", 1, RT_IPC_FLAG_FIFO ); log_assert( uart_sem_t, RT_NULL, 1 ); uart2_rdata.sem = rt_sem_create( "Uart2_r", 0, RT_IPC_FLAG_FIFO ); log_assert( uart2_rdata.sem, RT_NULL, 1 ); cluster_sem_t = rt_sem_create( "Cluster_t", 0, RT_IPC_FLAG_FIFO ); log_assert( cluster_sem_t, RT_NULL, 1 ); parameter_sem_t = rt_sem_create( "Parameter_t", 0, RT_IPC_FLAG_FIFO ); log_assert( parameter_sem_t, RT_NULL, 1 ); result = rt_thread_init( &InitHandle, "Init", InitTask, RT_NULL, ( rt_uint8_t * )&InitStack[0], sizeof( InitStack ), 2, 5 ); if ( result == RT_EOK ) { rt_thread_startup( &InitHandle ); } }