#include "Command.h"
#include "zf_driver_uart.h"
#include "zf_common_interrupt.h"
#include "ti/driverlib/dl_uart_main.h"
#include "zf_common_fifo.h"

volatile fifo_struct command_fifo = {0};
volatile uint8_t command_fifo_buffer[COMMAND_FIFO_BUFFER_SIZE] = {0};
uint8_t command_sum(uint8_t* data,int len);

void command_callback(uint32 state, void *ptr)
{
    while(false == DL_UART_isRXFIFOEmpty(UART2))//读取到一个数据
    {
        fifo_write_element((fifo_struct*)&command_fifo,DL_UART_Main_receiveData(UART2));//写入软件FIFO
    }
}
void command_init()
{
    uart_init(UART_2,115200,UART2_TX_B15,UART2_RX_B16);
    uart_set_interrupt_config(UART_2,UART_INTERRUPT_CONFIG_RX_ENABLE);
    interrupt_set_priority(UART2_INT_IRQn,1);//设置为与主要任务平行的优先级

    //开启发送DMA

    //开启硬件FIFO
    DL_UART_Main_enableFIFOs(UART2);
    DL_UART_Main_setRXFIFOThreshold(UART2,DL_UART_MAIN_RX_FIFO_LEVEL_1_2_FULL);

    //设置软件FIFO
    fifo_init((fifo_struct *)&command_fifo,FIFO_DATA_8BIT,(void*)command_fifo_buffer,COMMAND_FIFO_BUFFER_SIZE);
    //设置中断回调
    uart_set_callback(UART_2,command_callback,NULL);
}
//从软件FIFO中解析数据包并执行
void command_task()
{

}
//DMA发送使用的地址
uint8_t angle_velocity_data[6] = {0};
//发送速度前馈
void command_send_angle_velocity(float* pointer_angle_velocity)
{
    //等待上一次DMA发送完成

    //填充数据
    uint8_t* p = (uint8_t*)pointer_angle_velocity;
    angle_velocity_data[0]=0xA1;
    angle_velocity_data[1]=*(p++);
    angle_velocity_data[2]=*(p++);
    angle_velocity_data[3]=*(p++);
    angle_velocity_data[4]=*(p++);
    angle_velocity_data[5]=command_sum(angle_velocity_data,5);
    //DMA发送（非阻塞）

}
uint8_t command_sum(uint8_t* data,int len)
{
    uint8_t sum = 0;
    for(int i=0;i<len;i++)
    {
        sum+=data[i];
    }
    return sum;
}