/*
 * msp_steering_engine_new.c
 *
 *  Created on: 2025年10月14日
 *      Author: 13752
 */

#include "msp_steering_engine_new.h"

#define USART_RECV_BUF_SIZE 2048
#define USART_SEND_BUF_SIZE 2048
//#include <bsp_Log.h>
#include "BHBF_ROBOT.h"
//#include "bsp_Error_Detect.h"

//设置4个舵机速度
int32_t* steeringSetSpeed1;
int32_t* steeringSetSpeed2;
int32_t* steeringSetSpeed3;
int32_t* steeringSetSpeed4;

//设置4个舵机阻尼模式功率
int32_t* steeringSetDampingPower1;
int32_t* steeringSetDampingPower2;
int32_t* steeringSetDampingPower3;
int32_t* steeringSetDampingPower4;

//转向舵机原点位置
int32_t* steeringInitAngle3;
int32_t* steeringInitAngle4;

//转向舵机实时角度
int32_t* steeringRealAngle3;
int32_t* steeringRealAngle4;

void steering_control();
void decode_steering_angle(uint8_t *buffer, uint16_t length);
static uint8_t calc_servo_checksum(const uint8_t *cmd_buf, uint16_t cmd_len);
static void build_single_turn_angle_cmd(uint8_t *cmd_buf, uint8_t id,
                                        float target_angle, uint16_t move_interval,
                                        uint16_t exec_power);
static void build_speed_ctrl_cmd(uint8_t *cmd_buf, uint8_t id, uint16_t speed,
                                 uint16_t accel_time, uint16_t decel_time, bool is_clockwise);
static void build_stop_ctrl_cmd(uint8_t *cmd_buf, uint8_t id, uint16_t stop_power);
uint8_t steering_velocity_control(void);
uint8_t steering_homing_control(void);

struct UARTHandler *steering_engine_UART_Handler;
DispacherController* steering_dispacher;

void steering_engine_intialize(struct UARTHandler *Handler)
{
	steering_engine_UART_Handler = Handler;
	steering_engine_UART_Handler->Wait_time = 10; //等待10ms 最低不要低于4；
	steering_engine_UART_Handler->UART_Decode = decode_steering_angle;
	steering_dispacher=Handler->dispacherController;
	steering_dispacher->Dispacher_Enable = 1;
	steering_dispacher->DispacherCallTime = 20;
	steering_dispacher->Add_Dispatcher_List(steering_dispacher,read_steering_Angle);

	steering_dispacher->Add_Dispatcher_List(steering_dispacher,steering_control);
}

void steering_control()
{
	if(GV.P_MK32.CH5_SB == -1000)
	{
		steering_homing_control();
		return;
	}
	else if(GV.P_MK32.CH5_SB == 1000)
	{
		update_steering_state();
		return;
	}
	else {
		steering_velocity_control();
	}
}

// 全局配置表（通过初始化函数填充，避免静态初始化常量问题）
static SteeringConfigType steering_configs[4];
static bool config_initialized = false;  // 配置初始化标志（符合MISRA C:2012 Rule 8.13）

// 配置初始化函数（在系统启动阶段调用，确保参数就绪）
static void SteeringConfig_Init(void) {
    if (!config_initialized) {
        // 初始化舵机0配置（阻尼模式）
        steering_configs[0] = (SteeringConfigType){
            .id = 0U,
            .speed_ref = steeringSetSpeed1,
            .damping_power_ref = steeringSetDampingPower1,
            .mode = STEERING_MODE_DAMPING,
            .accel_time_ms = DEFAULT_ACCEL_DECEL,
            .decel_time_ms = DEFAULT_ACCEL_DECEL
        };

        // 初始化舵机1配置（阻尼模式）
        steering_configs[1] = (SteeringConfigType){
            .id = 1U,
            .speed_ref = steeringSetSpeed2,
            .damping_power_ref = steeringSetDampingPower2,
            .mode = STEERING_MODE_DAMPING,
            .accel_time_ms = DEFAULT_ACCEL_DECEL,
            .decel_time_ms = DEFAULT_ACCEL_DECEL
        };

        // 初始化舵机2配置（锁力模式）
        steering_configs[2] = (SteeringConfigType){
            .id = 2U,
            .speed_ref = steeringSetSpeed3,
            .damping_power_ref = NULL,
            .mode = STEERING_MODE_LOCKING,
            .accel_time_ms = DEFAULT_ACCEL_DECEL,
            .decel_time_ms = DEFAULT_ACCEL_DECEL
        };

        // 初始化舵机3配置（锁力模式）
        steering_configs[3] = (SteeringConfigType){
            .id = 3U,
            .speed_ref = steeringSetSpeed4,
            .damping_power_ref = NULL,
            .mode = STEERING_MODE_LOCKING,
            .accel_time_ms = DEFAULT_ACCEL_DECEL,
            .decel_time_ms = DEFAULT_ACCEL_DECEL
        };

        config_initialized = true;
    }
}

// 全局轮询ID（带volatile修饰符，确保多任务环境下的可见性）
static volatile uint8_t steering_id = 0U;

/**
 * @brief 舵机速度控制调度函数
 * @note 符合ISO 13849-1 PLd安全等级要求，支持在线参数校验
 * @return 执行状态（0=成功，非0=错误代码）
 */
uint8_t steering_velocity_control(void) {
    // 确保配置已初始化（符合功能安全要求的初始化检查）
    if (!config_initialized) {
        SteeringConfig_Init();
        if (!config_initialized) {
            return 0x01U;  // 配置初始化失败错误码
        }
    }

    // 计算配置数量（避免硬编码，符合MISRA C:2012 Rule 13.5）
    const uint8_t config_count = sizeof(steering_configs) / sizeof(steering_configs[0]);

    // 边界校验（防止越界访问，符合ISO 26262 ASIL B要求）
    if (steering_id >= config_count) {
        steering_id = 0U;  // 故障安全处理：重置为初始ID
        return 0x02U;      // 返回越界错误码
    }

    // 获取当前配置（使用const指针防止意外修改）
    const SteeringConfigType* current_config = &steering_configs[steering_id];

    // 参数有效性校验（符合功能安全参数校验要求）
    if ((current_config->speed_ref == NULL) ||
        (current_config->mode >= STEERING_MODE_MAX) ||
        (current_config->accel_time_ms > 10000U) ||  // 最大加速时间限制（10秒）
        (current_config->decel_time_ms > 10000U)) {   // 最大减速时间限制（10秒）
        return 0x03U;  // 参数无效错误码
    }

    // 阻尼模式额外参数校验
    if ((current_config->mode == STEERING_MODE_DAMPING) &&
        (current_config->damping_power_ref == NULL)) {
        return 0x04U;  // 阻尼功率参数缺失错误码
    }

    // 根据模式调用对应控制函数（分离关注点，符合SOLID原则）
    uint8_t result = 0U;
    if (current_config->mode == STEERING_MODE_DAMPING) {
        steering_set_velocity_with_damping(
            current_config->id,
            *current_config->speed_ref,
            current_config->accel_time_ms,
            current_config->decel_time_ms,
            *current_config->damping_power_ref
        );
    } else {
        steering_set_velocity_with_locking_force(
            current_config->id,
            *current_config->speed_ref,
            current_config->accel_time_ms,
            current_config->decel_time_ms
        );
    }

    // 轮询ID更新（原子操作，防止多任务环境下的数据竞争）
    steering_id = (steering_id + 1U) % config_count;

    return result;  // 返回执行结果
}


uint8_t steering_id2 = 0;           // 当前轮询的舵机ID索引
uint8_t status_counts = 0;          // 周期计数器

/**
 * @brief 更新舵机状态，周期性轮询清除各舵机圈数
 */
void update_steering_state(void) {
    // 定义需要轮询的舵机ID列表（可扩展至更多舵机）
    static const uint8_t steering_ids[MAX_STEERING_NUM] = {0, 1};

    // 1. 清除当前舵机的圈数（使用列表中的实际ID）
    clear_current_laps(steering_ids[steering_id2]);

    // 2. 周期计数：达到设定周期后切换到下一个舵机
    if (++status_counts >= CLEAR_LAPS_PERIOD) {
        // 循环切换到下一个舵机ID索引（0→1→0...）
        steering_id2 = (steering_id2 + 1) % MAX_STEERING_NUM;
        status_counts = 0;  // 重置计数器
    }
}

/**
 * @brief 清除舵机当前圈数计数
 * @param id: 目标舵机ID
 * @return 无（通过UART句柄发送指令）
 */
void clear_current_laps(uint8_t id) {
    uint8_t cmd_buf[CMD_LEN_CLEAR_LAPS] = {0};  // 局部指令缓冲区

    // 步骤1：填充帧头
    cmd_buf[0] = SERVO_REQ_HEADER_0;
    cmd_buf[1] = SERVO_REQ_HEADER_1;

    // 步骤2：填充指令ID和内容长度
    cmd_buf[2] = CMD_CLEAR_CURRENT_LAPS;  // 清除圈数指令ID
    cmd_buf[3] = 0x01;                    // 内容长度（1字节）

    // 步骤3：填充舵机ID
    cmd_buf[4] = id;

    // 步骤4：计算并填充校验和（第5字节）
    cmd_buf[5] = calc_servo_checksum(cmd_buf, CMD_LEN_CLEAR_LAPS - 1);

    // 指令发送
    memcpy(steering_engine_UART_Handler->Tx_Buf, cmd_buf, CMD_LEN_CLEAR_LAPS);
    steering_engine_UART_Handler->TxCount = CMD_LEN_CLEAR_LAPS;
    steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
}

/**
 * @brief 构建锁力模式停止控制指令
 * @param cmd_buf: 输出指令缓冲区
 * @param id: 舵机ID
 */
static void build_lock_force_stop_cmd(uint8_t *cmd_buf, uint8_t id) {
    if (cmd_buf == NULL) return;

    // 步骤1：填充帧头
    cmd_buf[0] = SERVO_REQ_HEADER_0;
    cmd_buf[1] = SERVO_REQ_HEADER_1;

    // 步骤2：填充指令ID和内容长度（停止指令固定）
    cmd_buf[2] = CMD_STOP_CTRL;  // 停止指令ID
    cmd_buf[3] = 0x04;           // 内容长度（4字节，协议要求）

    // 步骤3：填充舵机ID、锁力模式、固定功率参数
    cmd_buf[4] = id;
    cmd_buf[5] = PARAM_STOP_MODE_LOCK;  // 锁力停止模式
    cmd_buf[6] = LOCK_POWER_LOW;        // 锁力功率低字节
    cmd_buf[7] = LOCK_POWER_HIGH;       // 锁力功率高字节

    // 步骤4：计算并填充校验和（第8字节）
    cmd_buf[8] = calc_servo_checksum(cmd_buf, CMD_LEN_STOP_CTRL - 1);
}

/**
 * @brief 舵机速度控制（停止后进入锁力模式，支持加减速时间配置）
 * @param id: 舵机ID（0~254，255为广播地址）
 * @param Steering_Speed: 舵机速度（>0=顺时针，<0=逆时针，=0=停止并锁力）
 * @param accel_time: 加速时间（ms，范围0~4095，默认100ms）
 * @param decel_time: 减速时间（ms，范围0~4095，默认100ms）
 * @return 无（通过UART句柄发送指令）
 */
void steering_set_velocity_with_locking_force(uint8_t id, int32_t Steering_Speed,
                                              uint16_t accel_time, uint16_t decel_time) {
    uint8_t servo_cmd_buf[MAX_CMD_BUF_LEN] = {0};
    uint16_t cmd_len = 0;

    // 加减速时间范围修正
    uint16_t actual_accel = (accel_time > 4095) ? 4095 : accel_time;
    uint16_t actual_decel = (decel_time > 4095) ? 4095 : decel_time;

    // 分支1：顺时针转动
    if (Steering_Speed > 0) {
        uint16_t speed_proto = (uint16_t)(Steering_Speed * 10);
        build_speed_ctrl_cmd(servo_cmd_buf, id, speed_proto, actual_accel, actual_decel, true);
        cmd_len = CMD_LEN_SPEED_CTRL;

    // 分支2：逆时针转动
    } else if (Steering_Speed < 0) {
        uint16_t speed_proto = (uint16_t)((-Steering_Speed) * 10);
        build_speed_ctrl_cmd(servo_cmd_buf, id, speed_proto, actual_accel, actual_decel, false);
        cmd_len = CMD_LEN_SPEED_CTRL;

    // 分支3：停止并进入锁力模式
    } else {
        build_lock_force_stop_cmd(servo_cmd_buf, id);
        cmd_len = CMD_LEN_STOP_CTRL;
    }

    // 指令发送
    memcpy(steering_engine_UART_Handler->Tx_Buf, servo_cmd_buf, cmd_len);
    steering_engine_UART_Handler->TxCount = cmd_len;
    steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
}


/**
 * @brief 舵机速度控制（带阻尼停止功能，新增加减速时间参数）
 * @param id: 舵机ID（0~254，255为广播地址，协议文档6.3.1）
 * @param Steering_Speed: 舵机速度（>0=顺时针，<0=逆时针，=0=停止）
 * @param accel_time: 加速时间（ms，范围0~4095，默认100ms，超出自动截断）
 * @param decel_time: 减速时间（ms，范围0~4095，默认100ms，超出自动截断）
 * @param Steering_Power: 停止时功率参数（仅停止模式生效，单位：mW）
 * @return 无（通过UART句柄发送指令）
 */
void steering_set_velocity_with_damping(uint8_t id, int32_t Steering_Speed,
                                        uint16_t accel_time, uint16_t decel_time,
                                        int32_t Steering_Power) {
    // 局部指令缓冲区（替代全局变量，避免多任务冲突，提升可移植性）
    uint8_t servo_cmd_buf[MAX_CMD_BUF_LEN] = {0};
    uint16_t cmd_len = 0;

    // 加减速时间范围修正（协议隐含要求：16位值最大4095，超出自动截断，新增逻辑）
    uint16_t actual_accel = (accel_time > 4095) ? 4095 : accel_time;
    uint16_t actual_decel = (decel_time > 4095) ? 4095 : decel_time;

    // 分支1：顺时针转动（Steering_Speed > 0，仅新增加减速参数传入）
    if (Steering_Speed > 0) {
        // 速度转换：原逻辑（Steering_Speed * 10），转为协议要求的16位无符号值
        uint16_t speed_proto = (uint16_t)(Steering_Speed * 10);
        // 构建顺时针速度指令（新增传入加减速时间参数）
        build_speed_ctrl_cmd(servo_cmd_buf, id, speed_proto, actual_accel, actual_decel, true);
        cmd_len = CMD_LEN_SPEED_CTRL;  // 速度指令长度18字节

    // 分支2：逆时针转动（Steering_Speed < 0，仅新增加减速参数传入）
    } else if (Steering_Speed < 0) {
        // 速度转换：原逻辑（-Steering_Speed * 10），取绝对值后转为16位无符号值
        uint16_t speed_proto = (uint16_t)((-Steering_Speed) * 10);
        // 构建逆时针速度指令
        build_speed_ctrl_cmd(servo_cmd_buf, id, speed_proto, actual_accel, actual_decel, false);
        cmd_len = CMD_LEN_SPEED_CTRL;  // 速度指令长度18字节，保持原逻辑

    // 分支3：停止（Steering_Speed == 0，保持原逻辑完全不变）
    } else {
        // 功率转换：原逻辑直接转为16位无符号值（确保不超出舵机功率范围）
        uint16_t stop_power_proto = (uint16_t)Steering_Power;
        // 构建停止指令（阻尼模式）
        build_stop_ctrl_cmd(servo_cmd_buf, id, stop_power_proto);
        cmd_len = CMD_LEN_STOP_CTRL;  // 停止指令长度9字节，保持原逻辑
    }

    // 指令发送：拷贝到UART发送缓冲区，触发发送（与原逻辑一致，无修改）
    memcpy(steering_engine_UART_Handler->Tx_Buf, servo_cmd_buf, cmd_len);
    steering_engine_UART_Handler->TxCount = cmd_len;
    steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
}

/**
 * @brief 计算舵机指令校验和（保持原逻辑完全不变）
 * @param cmd_buf: 指令缓冲区（未含校验和的原始指令）
 * @param cmd_len: 指令长度（未含校验和）
 * @return 校验和值（1字节）
 */
static uint8_t calc_servo_checksum(const uint8_t *cmd_buf, uint16_t cmd_len) {
    uint32_t sum = 0;
    if (cmd_buf == NULL || cmd_len == 0) return 0;

    // 累加除校验和外的所有指令字节
    for (uint16_t i = 0; i < cmd_len; i++) {
        sum += cmd_buf[i];
    }
    return (uint8_t)(sum % 256);  // 按协议取模256
}

/**
 * @brief 构建速度控制指令
 * @param cmd_buf: 输出指令缓冲区
 * @param id: 舵机ID
 * @param speed: 速度值
 * @param accel_time: 加速时间（ms，转16位无符号值）
 * @param decel_time: 减速时间（ms，转16位无符号值）
 * @param is_clockwise: 是否顺时针（true=顺时针，false=逆时针）
 */
static void build_speed_ctrl_cmd(uint8_t *cmd_buf, uint8_t id, uint16_t speed,
                                 uint16_t accel_time, uint16_t decel_time, bool is_clockwise) {
    if (cmd_buf == NULL) return;

    // 步骤1：填充帧头（所有请求包通用，保持原逻辑）
    cmd_buf[0] = SERVO_REQ_HEADER_0;
    cmd_buf[1] = SERVO_REQ_HEADER_1;

    // 步骤2：填充指令ID和内容长度
    cmd_buf[2] = CMD_MULTI_SPEED_CTRL;  // 多圈速度控制指令ID
    cmd_buf[3] = 0x0D;                  // 内容长度（13字节，协议要求）

    // 步骤3：填充舵机ID和固定参数（区分顺时针/逆时针，保持原逻辑）
    cmd_buf[4] = id;
    cmd_buf[5] = PARAM_FIXED_00;
    if (is_clockwise) {  // 顺时针专属固定参数
        cmd_buf[6] = PARAM_CW_40;
        cmd_buf[7] = PARAM_CW_38;
    } else {  // 逆时针专属固定参数
        cmd_buf[6] = PARAM_CCW_C0;
        cmd_buf[7] = PARAM_CCW_C7;
    }
    cmd_buf[8] = is_clockwise ? PARAM_FIXED_00 : PARAM_CCW_FF;

    // 步骤4：填充速度值（小端字节序，保持原逻辑）
    cmd_buf[9] = (uint8_t)(speed & 0xFF);    // 速度低字节
    cmd_buf[10] = (uint8_t)((speed >> 8) & 0xFF);  // 速度高字节

    // 步骤5：填充加减速时间
    cmd_buf[11] = (uint8_t)(accel_time & 0xFF);  // 加速时间低字节
    cmd_buf[12] = (uint8_t)((accel_time >> 8) & 0xFF);  // 加速时间高字节
    cmd_buf[13] = (uint8_t)(decel_time & 0xFF);  // 减速时间低字节
    cmd_buf[14] = (uint8_t)((decel_time >> 8) & 0xFF);  // 减速时间高字节

    // 步骤6：填充后续通用固定参数
    cmd_buf[15] = PARAM_FIXED_00;
    cmd_buf[16] = PARAM_FIXED_00;

    // 步骤7：计算并填充校验和
    cmd_buf[17] = calc_servo_checksum(cmd_buf, CMD_LEN_SPEED_CTRL - 1);
}


/**
 * @brief 构建停止控制指令（阻尼模式）
 * @param cmd_buf: 输出指令缓冲区
 * @param id: 舵机ID
 * @param stop_power: 停止时功率参数
 */
static void build_stop_ctrl_cmd(uint8_t *cmd_buf, uint8_t id, uint16_t stop_power) {
    if (cmd_buf == NULL) return;

    // 步骤1：填充帧头
    cmd_buf[0] = SERVO_REQ_HEADER_0;
    cmd_buf[1] = SERVO_REQ_HEADER_1;

    // 步骤2：填充指令ID和内容长度（停止指令固定）
    cmd_buf[2] = CMD_STOP_CTRL;  // 停止指令ID
    cmd_buf[3] = 0x04;           // 内容长度（4字节，协议要求）

    // 步骤3：填充舵机ID、停止模式、功率参数
    cmd_buf[4] = id;
    cmd_buf[5] = PARAM_STOP_MODE_DAMP;  // 阻尼停止模式
    cmd_buf[6] = (uint8_t)(stop_power & 0xFF);    // 功率低字节（小端）
    cmd_buf[7] = (uint8_t)((stop_power >> 8) & 0xFF);  // 功率高字节（小端）

    // 步骤4：计算并填充校验和（第8字节）
    cmd_buf[8] = calc_servo_checksum(cmd_buf, CMD_LEN_STOP_CTRL - 1);
}

// 回零配置表（通过初始化函数填充，避免静态初始化问题）
static HomingConfigType homing_configs[HOMING_STATE_MAX];
static bool homing_config_initialized = false;  // 配置初始化标志

// 回零状态变量（volatile确保多任务环境下的可见性）
static volatile HomingStateType homing_state = HOMING_STATE_STEERING2;
static volatile uint8_t homing_exec_counter = 0U;  // 执行计数器

// 配置初始化函数（在首次调用时执行，符合功能安全初始化要求）
static void HomingConfig_Init(void) {
    if (!homing_config_initialized) {
        // 舵机2回零配置
        homing_configs[HOMING_STATE_STEERING2] = (HomingConfigType){
            .steering_id = 2U,
            .target_angle_ptr = steeringInitAngle3,
            .required_exec_count = 5U  // 需要执行5次指令
        };

        // 舵机3回零配置
        homing_configs[HOMING_STATE_STEERING3] = (HomingConfigType){
            .steering_id = 3U,
            .target_angle_ptr = steeringInitAngle4,
            .required_exec_count = 5U  // 需要执行5次指令
        };

        homing_config_initialized = true;
    }
}

/**
 * @brief 舵机回零控制函数
 * @note 符合ISO 13849-1 PLd安全等级，支持故障检测和状态校验
 * @return 执行状态码（0=成功，1=未初始化，2=参数错误）
 */
uint8_t steering_homing_control(void) {
    // 确保配置已初始化（功能安全要求：防止使用未初始化数据）
    if (!homing_config_initialized) {
        HomingConfig_Init();
        if (!homing_config_initialized) {
            return 1U;  // 配置初始化失败
        }
    }

    // 状态有效性校验（防止无效状态值，符合ISO 26262）
    if (homing_state >= HOMING_STATE_MAX) {
        homing_state = HOMING_STATE_STEERING2;      // 故障安全：重置为初始状态
        homing_exec_counter = 0U;
        return 2U;  // 状态参数错误
    }

    // 获取当前状态配置（使用const指针防止意外修改）
    const HomingConfigType* current_config = &homing_configs[homing_state];

    // 目标角度指针有效性校验（防止空指针访问）
    if (current_config->target_angle_ptr == NULL) {
        return 2U;  // 参数错误
    }

    // 执行回零指令（增加冗余发送机制，提高可靠性）
    (void)steering_set_single_turn_angle(
        current_config->steering_id,
        *(current_config->target_angle_ptr),
        DEFAULT_MOVE_INTERVAL,
        DEFAULT_EXEC_POWER
    );

    // 计数器累加与状态切换判断
    homing_exec_counter++;
    if (homing_exec_counter >= current_config->required_exec_count) {
        // 循环切换到下一个状态（使用枚举确保状态合法性）
        homing_state = (homing_state + 1U) % HOMING_STATE_MAX;
        homing_exec_counter = 0U;  // 重置计数器
    }

    return 0U;  // 执行成功
}



/**
 * @brief 单圈角度控制：设置舵机目标角度（自定义运动时间和功率）
 * @param id: 舵机ID
 * @param target_angle: 目标角度（单位：°）
 * @param move_interval: 到达目标角度的时间（ms）
 * @param exec_power: 执行功率（mW）
 * @return 无（通过UART发送指令）
 */
void steering_set_single_turn_angle(uint8_t id, float target_angle,
                                    uint16_t move_interval, uint16_t exec_power) {
    uint8_t cmd_buf[CMD_LEN_SINGLE_TURN_SET] = {0};  // 局部指令缓冲区

    // 构建单圈角度控制指令
    build_single_turn_angle_cmd(cmd_buf, id, target_angle, move_interval, exec_power);

    // 发送指令到UART缓冲区
    memcpy(steering_engine_UART_Handler->Tx_Buf, cmd_buf, CMD_LEN_SINGLE_TURN_SET);
    steering_engine_UART_Handler->TxCount = CMD_LEN_SINGLE_TURN_SET;
    steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
}

/**
 * @brief 构建单圈角度控制指令（设置目标角度）
 * @param cmd_buf: 输出指令缓冲区（需至少12字节）
 * @param id: 舵机ID（0~254，255为广播地址）
 * @param target_angle: 目标角度（单位：°，需符合舵机硬件范围，如0~360°）
 * @param move_interval: 到达目标角度的时间（ms，范围0~65535）
 * @param exec_power: 执行功率（mW，0~舵机额定功率）
 */
static void build_single_turn_angle_cmd(uint8_t *cmd_buf, uint8_t id,
                                        float target_angle, uint16_t move_interval,
                                        uint16_t exec_power) {
    if (cmd_buf == NULL) return;

    // 1. 填充帧头（协议固定格式：0x12 + 0x4C）
    cmd_buf[0] = SERVO_REQ_HEADER_0;
    cmd_buf[1] = SERVO_REQ_HEADER_1;

    // 2. 填充指令ID和内容长度（协议文档6.3.8：内容长度7字节）
    cmd_buf[2] = CMD_SINGLE_TURN_ANGLE_SET;
    cmd_buf[3] = 0x07;  // 内容长度=7字节（ID1 + 角度2 + 时间2 + 功率2）

    // 3. 填充指令内容（按协议顺序，小端字节序）
    cmd_buf[4] = id;  // 舵机ID（1字节）

    // 角度转换：实际角度→协议单位（例：90° → 90*10=900（0.1°单位））
    int16_t angle_proto = (int16_t)(target_angle * ANGLE_UNIT_SCALE);
    cmd_buf[5] = (uint8_t)(angle_proto & 0xFF);    // 角度低字节
    cmd_buf[6] = (uint8_t)((angle_proto >> 8) & 0xFF);  // 角度高字节

    // 运动时间（小端字节序，协议要求）
    cmd_buf[7] = (uint8_t)(move_interval & 0xFF);    // 时间低字节
    cmd_buf[8] = (uint8_t)((move_interval >> 8) & 0xFF);  // 时间高字节

    // 执行功率（小端字节序，协议要求）
    cmd_buf[9] = (uint8_t)(exec_power & 0xFF);    // 功率低字节
    cmd_buf[10] = (uint8_t)((exec_power >> 8) & 0xFF);  // 功率高字节

    // 4. 计算并填充校验和（协议规则：所有字节求和后对256取余）
    cmd_buf[11] = calc_servo_checksum(cmd_buf, CMD_LEN_SINGLE_TURN_SET - 1);
}


uint8_t read_steering_2_Angle_command[6] = {0x12,0x4c,0x0a,0x01,0x02,0x6b};
uint8_t read_steering_3_Angle_command[6] = {0x12,0x4c,0x0a,0x01,0x03,0x6c};
uint8_t read_steering_Angle_flag = 0;
/**
 * @brief 读取舵机角度
 */
void read_steering_Angle()
{
	switch(read_steering_Angle_flag)
	{
		case 0:
			memcpy(&steering_engine_UART_Handler->Tx_Buf,&read_steering_2_Angle_command,6);
			steering_engine_UART_Handler->TxCount = 6;
			steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
			read_steering_Angle_flag = 1;
			break;
		case 1:
			memcpy(&steering_engine_UART_Handler->Tx_Buf,&read_steering_3_Angle_command,6);
			steering_engine_UART_Handler->TxCount = 6;
			steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
			read_steering_Angle_flag = 0;
			break;
	}
}

/**
 * @brief 解析舵机角度数据
 */
void decode_steering_angle(uint8_t *buffer, uint16_t length)
{
	if((buffer[0] == 0x05) && (buffer[1] == 0x1c) && (buffer[2] == 0x0a) && (buffer[3] == 0x03))
	{
		switch(buffer[4])
		{
			case 0x02:
				*steeringRealAngle3 = ((buffer[6] << 8) | buffer[5]) / 10;
				break;
			case 0x03:
				*steeringRealAngle4 = ((buffer[6] << 8) | buffer[5]) / 10;
				break;
		}
	}
}