#include "main.h"
#include "can.h"
#include "gpio.h"
#include "can2.h"
// 打包函数
void pack_rc_to_can(const rc_info_t *rc, can_packet_t *packet1, can_packet_t *packet2)
{
    // 第一个包：ch0-ch3 (共8字节)
    packet1->data[0] = (uint8_t)(rc->ch0 & 0xFF);
    packet1->data[1] = (uint8_t)((rc->ch0 >> 8) & 0xFF);
    packet1->data[2] = (uint8_t)(rc->ch1 & 0xFF);
    packet1->data[3] = (uint8_t)((rc->ch1 >> 8) & 0xFF);
    packet1->data[4] = (uint8_t)(rc->ch2 & 0xFF);
    packet1->data[5] = (uint8_t)((rc->ch2 >> 8) & 0xFF);
    packet1->data[6] = (uint8_t)(rc->ch3 & 0xFF);
    packet1->data[7] = (uint8_t)((rc->ch3 >> 8) & 0xFF);
    
    // 第二个包：roll + sw1 + sw2 (共4字节)
    packet2->data[0] = (uint8_t)(rc->roll & 0xFF);
    packet2->data[1] = (uint8_t)((rc->roll >> 8) & 0xFF);
    packet2->data[2] = rc->sw1;
    packet2->data[3] = rc->sw2;
    
    // 填充剩余字节（可以添加校验、计数器等）
    static uint8_t counter = 0;
    packet2->data[4] = counter++;  // 帧计数器
    packet2->data[5] = 0xAA;       // 校验标记
    packet2->data[6] = 0x55;
    packet2->data[7] = 0x00;
}

// CAN发送函数
static uint8_t send_can_frame(uint32_t id, uint8_t *data, uint8_t len)
{
    CAN_TxHeaderTypeDef tx_header;
    uint32_t tx_mailbox;
    
    // 配置CAN帧头
    tx_header.StdId = id;
    tx_header.ExtId = 0x00;
    tx_header.IDE = CAN_ID_STD;
    tx_header.RTR = CAN_RTR_DATA;
    tx_header.DLC = len;
    tx_header.TransmitGlobalTime = DISABLE;
    
    // 发送CAN帧
    if (HAL_CAN_AddTxMessage(&hcan2, &tx_header, data, &tx_mailbox) == HAL_OK)
    {
        return 1;  // 发送成功
    }
    
    return 0;  // 发送失败
}

// 发送遥控器数据
void send_rc_data(const rc_info_t *rc)
{
    can_packet_t packet1, packet2;
    
    // 打包数据
    pack_rc_to_can(rc, &packet1, &packet2);
    
    // 发送两个CAN帧
    if (!send_can_frame(0x100, packet1.data, 8))
    {
        // 发送失败处理
        // 可以重试或记录错误
    }
    
    // 延时一下，避免总线拥堵
    HAL_Delay(1);
    
    if (!send_can_frame(0x101, packet2.data, 8))
    {
        // 发送失败处理
    }
}