proximity_uavcan_converter/App/freertos_action.c

#include "freertos_action.h"

extern uint32_t last_rev_usart1_tick;
extern uint32_t last_rev_usart2_tick;

osStatus_t handleQueuePutErrorResource(osMessageQueueId_t mq_id, const void *msg_ptr, uint8_t msg_prio, uint32_t timeout);

void task_usart_rev_action(void)
{
    usart_data_t data = {0};
   // uint8_t data_uart;
    osStatus_t os_status;
    active_uasrt_irq(1);
    active_uasrt_irq(2);
    for(;;)
    {
        
        if (usart_rev_queueHandle != NULL)
        {
            os_status = osMessageQueueGet(usart_rev_queueHandle, &data, NULL, osWaitForever);
            if (os_status == osOK)
            {
                if (data.len < 6)
                {
                  deal_send_data(&data);
                }
                else
                {
                    parse_ar1271_data(&data);
                }
            }
        }
        else
        {
            osDelay(2);
        }
    }
}

void task_uavcan_rev_action(void)
{
    uavcan_rev_t uavcan_rev_data;
    osStatus_t os_status;
    
    /* Infinite loop */
    for (;;)
    {
        if (uavcan_rev_queueHandle != NULL)
        {
            os_status = osMessageQueueGet(uavcan_rev_queueHandle, &uavcan_rev_data, NULL, osWaitForever);
            if (os_status == osOK)
            {
                handle_uavcan_rx_frame(&uavcan_rev_data);              
            }
        }
        else
        {
            osDelay(2);
        }
    }
}

void task_uavcan_send_action(void)
{
    uavcan_send_data_t data = {0};
  //  float depth_data=0.0f;
    osStatus_t status;
    
    for(;;)
    {
        allocation_update();
        if (uavcan_send_queueHandle != NULL)
        {
            status = osMessageQueueGet(uavcan_send_queueHandle, &data, NULL, 5);
            if (status == osOK)
            {
                send_uavcan_data(&data);
            }
        }
        send_canard();   
         check_need_to_jump_boot();   
    }
}

void task_default_action(void)
{
    
    uavcan_init_by_allocation();
    uint32_t last_send_node_status_tick = 0;
    //uint32_t last_send_proximity_tick =0;
    /* Infinite loop */
    for (;;)
    {
        if (last_send_node_status_tick + 1000 < HAL_GetTick())
        {
            uavcan_cleanup_stale_transfers(HAL_GetTick() * 1000);
            last_send_node_status_tick = HAL_GetTick();
            if (uavcan_send_queueHandle != NULL)
            {
                uavcan_send_data_t send_data = {0};
                send_data.id = UAVCAN_NODE_STATUS_DATA_TYPE_ID;
                uavcan_node_status_t data = {0};
                memcpy(&send_data.content.node_state, &data, sizeof(uavcan_node_status_t));
                myOsMessageQueuePut(uavcan_send_queueHandle, &send_data, 0, 10);
            }
        }
        osDelay(200);
    }
}

void task_usart_send(void)
{
    osStatus os_status;
    usart_data_t data = {0};
    for (;;)
    {
        if (usart_send_queueHandle != NULL)
        {
            os_status = osMessageQueueGet(usart_send_queueHandle, &data, NULL, 10);
            if (os_status == osOK)
            {
                if (data.usart_instance == USART_INSTANCE_U1)
                {
                    HAL_UART_Transmit_DMA(&huart1, (uint8_t *)&data.data, data.len);
                }
                else
                {
                    HAL_UART_Transmit_DMA(&huart2, (uint8_t *)&data.data, data.len);
                }
            }
        }

        if ((last_rev_usart1_tick + 3000) < HAL_GetTick())
        {
            request_qoar1271_by_step(1, USART_INSTANCE_U1);
        }

        if ((last_rev_usart2_tick + 3000) < HAL_GetTick())
        {
            request_qoar1271_by_step(1, USART_INSTANCE_U2);
        }
    }
}

void task_proximity(void)
{
    proximity_data_t proximity1 = {0};
    proximity_data_t proximity2 = {0};
    osStatus_t os_p1 = osError;
    osStatus_t os_p2 = osError;
    for (;;)
    {
        if (proximity1_queueHandle != NULL && proximity2_queueHandle != NULL)
        {
            os_p1 = osMessageQueueGet(proximity1_queueHandle, &proximity1, NULL, 120);
            os_p2 = osMessageQueueGet(proximity2_queueHandle, &proximity2, NULL, 120);
            // if((os_p1 == osOK) || (os_p2 == osOK)){
            //     uavcan_proximity_t data = {0};
            //     data.d0 = proximity1.d0;
            //     data.d45 = proximity1.d45;
            //     data.d90 = proximity1.d90;
            //     data.d135 = proximity1.d135;
            //     data.d180 = proximity1.d180;
            //     data.d225 = proximity1.d225;
            //     data.d270 = proximity1.d270;
            //     data.d315 = proximity1.d315;
            //     send_uavcan_proximity(&data);
            // }
            // if ((os_p1 == osOK) && (os_p2 == osOK))
            // {
            //     uavcan_proximity_t data = {0};
            //     data.d0 = min(proximity1.d0, proximity2.d0);
            //     data.d90 = proximity1.d90;
            //     data.d45 = proximity2.d45;
            //     send_uavcan_proximity(&data);
            // }
            // else
            // {
                uavcan_proximity_t data = {0};
                data.d0 = min(proximity1.d0, proximity2.d0);
                if (os_p1 == osOK)  //左边的雷达是反的
                {
                    // uavcan_proximity_t data = {0};
                    data.d0 = proximity1.d0;
                    // data.d45 = proximity1.d90;
                    data.d90 = proximity1.d45;
                    send_uavcan_proximity(&data);
                }
                else if (os_p2 == osOK)
                {
                    // uavcan_proximity_t data = {0};
                    data.d0 = proximity2.d0;
                    data.d45 = proximity2.d45;
                    // data.d90 = proximity2.d90;
                    send_uavcan_proximity(&data);
                }
            // }
        }
    }
}

uint16_t min(uint16_t one, uint16_t two)
{
    return one < two ? one : two;
}

osStatus_t myOsMessageQueuePut(osMessageQueueId_t mq_id, const void *msg_ptr, uint8_t msg_prio, uint32_t timeout)
{
    osStatus_t osStatus = osMessageQueuePut(mq_id, msg_ptr, msg_prio, timeout);
    switch (osStatus)
    {
    case osOK:
        return osOK;
    case osErrorTimeout:
        return osErrorTimeout;
    case osErrorResource:
        return handleQueuePutErrorResource(mq_id, msg_ptr, msg_prio, timeout);
    case osErrorParameter:
        return osMessageQueuePut(mq_id, msg_ptr, msg_prio, 0);
    default:
        return osError;
    }
}

osStatus_t handleQueuePutErrorResource(osMessageQueueId_t mq_id, const void *msg_ptr, uint8_t msg_prio, uint32_t timeout)
{
    uint32_t size = osMessageQueueGetMsgSize(mq_id);
    if (size <= 0)
    {
        size = 100;
    }
    uint8_t *temp = pvPortMalloc(size);
    osStatus_t osStatus = osMessageQueueGet(mq_id, temp, &msg_prio, timeout);
    osStatus = myOsMessageQueuePut(mq_id, temp, msg_prio, timeout);
    vPortFree(temp);
    // osMemoryPoolId_t mpid_MemPool = osMemoryPoolNew(1, size, NULL);
    // uint8_t *temp = osMemoryPoolAlloc(mpid_MemPool, 0);
    // osStatus_t osStatus = osMessageQueueGet(mq_id, temp, &msg_prio, timeout);
    // osStatus = myOsMessageQueuePut(mq_id, temp, msg_prio, timeout);
    // osStatus = osMemoryPoolFree(mpid_MemPool, temp);
    // osStatus = osMemoryPoolDelete(mpid_MemPool);
    return osStatus;
}