4d_ObstacleRadarDroneCAN/App/application.c

/*****************************************************************************
 * 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 <rtthread.h>
#include <string.h>
#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 );
    }
}