#include "LD_remote.h"
#include <string.h>

#include "bsp_usart.h"
#include "error_detect.h"

static uint8_t sbus_rx_buf_new[SBUS_RX_BUF_NUM]; // SBUS 数据缓冲区
static RC_Ctrl_New_t rc_ctrl_new; // SBUS 数据结构

/* SBUS 数据解析 */
static void sbus_to_rc_new(volatile const uint8_t *sbus_buf) {
    if (sbus_buf == NULL) return;

    rc_ctrl_new.ch[0] = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07FF;
    rc_ctrl_new.ch[1] = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07FF;
    rc_ctrl_new.ch[2] = ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) | (sbus_buf[5] << 10)) & 0x07FF;
    rc_ctrl_new.ch[3] = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07FF;

    rc_ctrl_new.sw[0] = ((sbus_buf[6] >> 4) | (sbus_buf[7] << 4)) & 0x07FF;
    rc_ctrl_new.sw[1] = ((sbus_buf[7] >> 7) | (sbus_buf[8] << 1) | (sbus_buf[9] << 9)) & 0x07FF;
}

/* 初始化函数 */
void RC_New_Init(void) {
    HAL_UARTEx_ReceiveToIdle_DMA(&huart3, sbus_rx_buf_new, SBUS_RX_BUF_NUM);
}

/* 获取数据指针 */
const RC_Ctrl_New_t* RC_New_GetData(void) {
    return &rc_ctrl_new;
}