engineering/application/gimbal/gimbal.cpp

#ifdef __cplusplus
extern "C" {
#endif
#include "gimbal.h"
#include "robot_def.h"
#include "dji_motor.h"
#include "dmmotor.h"
#include "ins_task.h"
#include "message_center.h"
#include "general_def.h"
#include "bmi088.h"
#include "user_lib.h"
#include "bsp_log.h"
#include "referee_VT.h"
#ifdef __cplusplus
}
#endif
#include "matrix.h"
#include "robotics.h"


static attitude_t *gimba_IMU_data; // 云台IMU数据
static DMMotorInstance *yaw_motor, *pitch_motor_l,*pitch_motor_r, *roll_motor;
static DMMotorInstance *diff_r_motor,*diff_l_motor;
//pitch轴双6006 双环pid算出力矩 一人一半
static PIDInstance pitch_spd_loop,pitch_angle_loop;

static PIDInstance diff_pitch_loop,diff_roll_loop;
static PIDInstance diff_pitch_spd_loop,diff_roll_spd_loop;

float arm_gravity_feedforward = 0;
float GRAVITY_COMP = 6.0;

static Publisher_t *gimbal_pub;                   // 云台应用消息发布者(云台反馈给cmd)
static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息

static gimbal_mode_e last_gimbal_mode;     //上次模式  用于模式切换数据过渡

first_order_filter_type_t pitch_spd_filter,yaw_spd_filter,roll_spd_filter;
static float pitch_spd,yaw_spd,roll_spd;

first_order_filter_type_t diff_r_spd_filter,diff_l_spd_filter;
static float diff_r_spd,diff_l_spd;

first_order_filter_type_t Qset_filter[3]; //关节目标值滤波

float q_set[5] ; //五个关节的目标值

//机械臂参数初始化
//float arm_q[5] = {0}; // 机械臂各关节位置
robotics::Link link[5];
robotics::Serial_Link<5> engineer_arm(link);
Matrixf<4, 4> fk_T; //正运动学  末端变换矩阵
Matrixf<3, 1> fk_p; //正运动学  末端位置向量
Matrixf<3, 1> fk_rpy; //正运动学  末端欧拉角
Matrixf<5, 1> arm_q;  //正运动学  末端关节位置


Matrixf<4, 4> T_cmd; //拟运动学  期望末端变换矩阵
Matrixf<5, 2> ik_q;    //逆运动学  关节位置
Matrixf<5, 1> ik_q_cmd;//逆运动学 期望关节位置
Matrixf<3, 1> cmd_xyz; //逆运动学  末端期望位置
Matrixf<3, 3> cmd_R; //逆运动学  末端期望姿态
Matrixf<4, 1> cmd_quat; //逆运动学  末端期望姿态
Matrixf<3, 2> ik_q3;    //逆运动学  球形手腕关节位置
Matrixf<3, 1> ik_q3_cmd;//逆运动学  球形手腕期望关节位置
const float l1 = 0.151 ,l3 = 0.350, l4 = 0;//0.139;
void Arm_Init()
{
    link[0] = robotics::Link(0,l1,0,PI/2);
    link[1] = robotics::Link(0,0,l3,PI/2);
    link[2] = robotics::Link(0,0,0,-PI/2);
    link[3] = robotics::Link(0,0,0,PI/2);
    link[4] = robotics::Link(0,l4,0,0);

    engineer_arm = robotics::Serial_Link<5>(link);
    engineer_arm.ikine_analytic = robotics::my_analytic_ikine;
    engineer_arm.ikine_analytic_check = robotics::check_ikine;
}

void GimbalInit()
{
    Arm_Init();
    Motor_Init_Config_s yaw_motor_config = {
            .controller_param_init_config = {
                    .other_speed_feedback_ptr = &yaw_spd,
                    .speed_PID = {
                            .Kp = 0.0f,//3.5f,//5,
                            .Ki = 0.0f,
                            .Kd = 0,//0.02f,
                            .MaxOut = 10,
                            .Improve = static_cast<PID_Improvement_e>(PID_Integral_Limit | PID_Derivative_On_Measurement),
                            .IntegralLimit = 10.0F,
                    },
                    .angle_PID = {
                            .Kp = 10.0f,//13.0f,
                            .Ki = 0.0f,
                            .Kd = 0.0f,
                            .MaxOut = 10,
                            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
                            .IntegralLimit = 10.0F,
                    },
            },
            .controller_setting_init_config = {
                    .outer_loop_type = ANGLE_LOOP,
                    .close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
                    .angle_feedback_source = MOTOR_FEED,
                    .speed_feedback_source = OTHER_FEED,
                    .feedforward_flag = CURRENT_FEEDFORWARD,
            },
            .motor_type = DM6006,
    };



    Motor_Init_Config_s pitch_motor_config = {
            .controller_param_init_config = {
                    .other_speed_feedback_ptr = &pitch_spd,
                    //.current_feedforward_ptr = &arm_gravity_feedforward,
            },
            .controller_setting_init_config = {
                    .outer_loop_type = OPEN_LOOP,
                    .close_loop_type = OPEN_LOOP,
                    .angle_feedback_source = MOTOR_FEED,
                    .speed_feedback_source = OTHER_FEED,
                    //.feedforward_flag = CURRENT_FEEDFORWARD,
            },
            .motor_type = DM6006,
    };



    PID_Init_Config_s pitch_spd_config= {
            .Kp = 3.0f,
            .Ki = 0.0f,
            .Kd = 0.1f,
            .MaxOut = 10,
            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
            .IntegralLimit = 100,
    };
    PID_Init_Config_s pitch_angle_config= {
            .Kp = 15.0f,
            .Ki = 0,
            .Kd = 0,
            .MaxOut = 10,
            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
            .IntegralLimit = 100,
    };

    PIDInit(&pitch_spd_loop,&pitch_spd_config);
    PIDInit(&pitch_angle_loop,&pitch_angle_config);

    Motor_Init_Config_s roll_motor_config = {
            .controller_param_init_config = {
                    .other_speed_feedback_ptr = &roll_spd,
                    .speed_PID = {
                            .Kp = 0.4f,
                            .Ki = 0.0f,
                            .Kd = 0.02f,
                            .MaxOut = 10,
                            .Improve = static_cast<PID_Improvement_e>(PID_Integral_Limit | PID_Derivative_On_Measurement),
                            .IntegralLimit = 10.0F,
                    },
                    .angle_PID = {
                            .Kp = 25.0f,
                            .Ki = 0.0f,
                            .Kd = 0.0f,
                            .MaxOut = 10,
                            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
                            .IntegralLimit = 10.0F,
                    },
            },
            .controller_setting_init_config = {
                    .outer_loop_type = ANGLE_LOOP,
                    .close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
                    .angle_feedback_source = MOTOR_FEED,
                    .speed_feedback_source = OTHER_FEED,
                    //.feedforward_flag = CURRENT_FEEDFORWARD,
            },
            .motor_type = DM4310,
    };



    Motor_Init_Config_s diff_motor_config = {
            .controller_param_init_config = {
                    .speed_PID = {
                            .Kp = 0.6f,
                            .Ki = 0.1f,
                            .Kd = 0.02f,
                            .MaxOut = 10,
                            .Improve = static_cast<PID_Improvement_e>( PID_Integral_Limit | PID_Derivative_On_Measurement),
                            .IntegralLimit = 10.0F,
                            .Output_LPF_RC = 0.02F,
                    },
            },
            .controller_setting_init_config = {
                    .outer_loop_type = SPEED_LOOP,
                    .close_loop_type = SPEED_LOOP,
                    .angle_feedback_source = MOTOR_FEED,
                    .speed_feedback_source = OTHER_FEED,
                    //.feedforward_flag = CURRENT_FEEDFORWARD,
            },
            .motor_type = DM4310,
    };



    const float spd_filter_num = 0.05f;
    first_order_filter_init(&pitch_spd_filter,5e-3,&spd_filter_num);
    const float spd_filter_num_yaw = 0.05f;
    first_order_filter_init(&yaw_spd_filter,5e-3,&spd_filter_num_yaw);
    const float spd_filter_num_roll = 0.05f;
    first_order_filter_init(&roll_spd_filter,5e-3,&spd_filter_num_roll);

    const float spd_filter_num_diff = 0.05f;
    first_order_filter_init(&diff_r_spd_filter,5e-3,&spd_filter_num_diff);
    first_order_filter_init(&diff_l_spd_filter,5e-3,&spd_filter_num_diff);

    const float Qset_filter_num = 0.1f;
    first_order_filter_init(&Qset_filter[0],5e-3,&Qset_filter_num);
    first_order_filter_init(&Qset_filter[1],5e-3,&Qset_filter_num);
    first_order_filter_init(&Qset_filter[2],5e-3,&Qset_filter_num);

    PID_Init_Config_s diff_pitch_config= {
            .Kp = 20.0f,//20.0F,//15.0f,
            .Ki = 0,
            .Kd = 0,
            .MaxOut = 10,
            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
            .IntegralLimit = 100,
    };
    PIDInit(&diff_pitch_loop,&diff_pitch_config);

    PID_Init_Config_s diff_roll_config= {
            .Kp = 14.0f,
            .Ki = 0,
            .Kd = 0,
            .MaxOut = 10,
            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
            .IntegralLimit = 100,
    };
    PIDInit(&diff_roll_loop,&diff_roll_config);


    //需要按id顺序初始化 否则有电机不能正确运行

    //决定放弃yaw轴
//    yaw_motor_config.can_init_config.can_handle = &hcan2;
//    yaw_motor_config.can_init_config.tx_id = 1;
//    yaw_motor_config.can_init_config.rx_id = 2;
//    yaw_motor = DMMotorInit(&yaw_motor_config);

    pitch_motor_config.can_init_config.can_handle = &hcan2;
    pitch_motor_config.can_init_config.tx_id = 3;
    pitch_motor_config.can_init_config.rx_id = 4;
    pitch_motor_l = DMMotorInit(&pitch_motor_config);

    roll_motor_config.can_init_config.can_handle = &hcan2;
    roll_motor_config.can_init_config.tx_id = 5;
    roll_motor_config.can_init_config.rx_id = 6;
    roll_motor = DMMotorInit(&roll_motor_config);

    diff_motor_config.can_init_config.can_handle = &hcan2;
    diff_motor_config.can_init_config.tx_id = 7;
    diff_motor_config.can_init_config.rx_id = 8;
    diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_r_spd;
    diff_r_motor = DMMotorInit(&diff_motor_config);

    diff_motor_config.can_init_config.can_handle = &hcan2;
    diff_motor_config.can_init_config.tx_id = 9;
    diff_motor_config.can_init_config.rx_id = 10;
    diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_l_spd;
    diff_l_motor = DMMotorInit(&diff_motor_config);

    pitch_motor_config.can_init_config.tx_id = 11; //按顺序初始化！！！
    pitch_motor_config.can_init_config.rx_id = 12;
    pitch_motor_r = DMMotorInit(&pitch_motor_config);

    gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
    // YAW
    gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
    gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));

    //上电校准两个4310零点
//    DMMotorCaliEncoder(diff_r_motor);
//    DMMotorCaliEncoder(diff_l_motor);
////水平方向3.02 矿仓 0
//    DMMotorCaliEncoder(pitch_motor_l);

//    DMMotorCaliEncoder(roll_motor);

    q_set[1] = PITCH_MIN * DEGREE_2_RAD; //上电给pitch轴赋值

}

//#define PITCH_OFFSET 3.02F //pitch轴水平时编码器值
//static void DMMotroEnable()
//{
//    if(gimbal_cmd_recv.MotorEnableFlag)
//    {
//        DMMotorSetMode(DM_CMD_MOTOR_MODE,yaw_motor);
//        DMMotorSetMode(DM_CMD_MOTOR_MODE,pitch_motor);
//        DMMotorSetMode(DM_CMD_MOTOR_MODE,roll_motor);
//        DMMotorSetMode(DM_CMD_MOTOR_MODE,diff_r_motor );
//        DMMotorSetMode(DM_CMD_MOTOR_MODE,diff_l_motor );
//    }
//}

/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */

float debug_diff_kp = 0.3f,debug_diff_ki=0,debug_diff_kd = 0.01f;
float diff_motor_r_offset = 0;  float diff_motor_l_offset = 0;
void GimbalTask()
{
    diff_r_motor->motor_controller.speed_PID.Kp = debug_diff_kp;
    diff_r_motor->motor_controller.speed_PID.Ki = debug_diff_ki;
    diff_r_motor->motor_controller.speed_PID.Kd = debug_diff_kd;

    diff_l_motor->motor_controller.speed_PID.Kp = debug_diff_kp;
    diff_l_motor->motor_controller.speed_PID.Ki = debug_diff_ki;
    diff_l_motor->motor_controller.speed_PID.Kd = debug_diff_kd;
    // 获取云台控制数据
    // 后续增加未收到数据的处理
    SubGetMessage(gimbal_sub, &gimbal_cmd_recv);

    /* 控制参数计算 ------------------------------------------------------------------------*/
    //大臂重力补偿力矩
    arm_gravity_feedforward = - GRAVITY_COMP * arm_cos_f32(- pitch_motor_l->measure.position);
    //电机速度滤波
    first_order_filter_cali(&pitch_spd_filter,pitch_motor_l->measure.velocity);
    pitch_spd = pitch_spd_filter.out;
    first_order_filter_cali(&yaw_spd_filter,yaw_motor->measure.velocity);
    yaw_spd = yaw_spd_filter.out;
    first_order_filter_cali(&roll_spd_filter,roll_motor->measure.velocity);
    roll_spd = roll_spd_filter.out;
    //手腕关节
    first_order_filter_cali(&diff_r_spd_filter,diff_r_motor->measure.velocity);
    diff_r_spd = diff_r_spd_filter.out;
    first_order_filter_cali(&diff_l_spd_filter,diff_l_motor->measure.velocity);
    diff_l_spd = diff_l_spd_filter.out;

    float diff_r_angle = diff_r_motor->measure.position - diff_motor_r_offset;
    float diff_l_angle = diff_l_motor->measure.position - diff_motor_l_offset;

    float diff_pitch_angle = (diff_r_angle + (-diff_l_angle))/2;
    float diff_roll_angle = (diff_r_angle - (-diff_l_angle))/2 * 18/52;

    /* 控制参数计算 ------------------------------------------------------------------------*/

    //正运动学
    arm_q[0][0] = 0;//yaw_motor->measure.position;
    arm_q[1][0] = -(pitch_motor_l->measure.position);
    arm_q[2][0] = roll_motor->measure.position;
    arm_q[3][0] = diff_pitch_angle;
    arm_q[4][0] = diff_roll_angle;
    fk_T = engineer_arm.fkine(arm_q);

    fk_p = robotics::t2p(fk_T);
    fk_rpy = robotics::t2rpy(fk_T);
    //逆运动学
//    ik_q = engineer_arm.ikine_analytic(fk_T);
//
//    ik_q_cmd = ik_q.block<5,1>(0,0);

//    ik_q3 = robotics::spherical_wrist_ikine(robotics::t2r(fk_T),arm_q[0][0],arm_q[1][0]);

    if(gimbal_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE)
    {
        static uint8_t last_zero_flag = 0;

        DMMotorStop(pitch_motor_l);DMMotorStop(pitch_motor_r);
        DMMotorStop(yaw_motor);DMMotorStop(roll_motor);
        DMMotorStop(diff_r_motor);DMMotorStop(diff_l_motor);

        if(gimbal_cmd_recv.set_zero_flag != last_zero_flag)
        {
            diff_motor_r_offset = diff_r_motor->measure.position;
            diff_motor_l_offset = diff_l_motor->measure.position;

            PIDClear(&diff_pitch_loop);
            PIDClear(&diff_roll_loop);
            PIDClear(&diff_r_motor->motor_controller.speed_PID);
            PIDClear(&diff_l_motor->motor_controller.speed_PID);
        }
        last_zero_flag = gimbal_cmd_recv.set_zero_flag;

        //不使能模式下  目标值为当前值  防止使能甩大臂
        q_set[1] = pitch_motor_l->measure.position;
    }
    else
    {
        DMMotorEnable(pitch_motor_l);DMMotorEnable(pitch_motor_r);
        DMMotorEnable(yaw_motor);DMMotorEnable(roll_motor);
        DMMotorEnable(diff_r_motor);DMMotorEnable(diff_l_motor);
    }

    if(gimbal_cmd_recv.gimbal_mode == GIMBAL_MANUAL_MODE) //各关节分开控制
    {
        q_set[0] = 0;
        q_set[1] += gimbal_cmd_recv.pitch * DEGREE_2_RAD;
        q_set[2] += gimbal_cmd_recv.roll * DEGREE_2_RAD;
        q_set[3] += gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD;
        q_set[4] += gimbal_cmd_recv.diff_roll * DEGREE_2_RAD;
    }
    //地矿和银矿模式直接计算角度  不逆解了
    if(gimbal_cmd_recv.gimbal_mode == GIMBAL_FLOOR_MODE)
    {
        q_set[0] = 0;
        q_set[1] += gimbal_cmd_recv.pitch * DEGREE_2_RAD;
        q_set[2] = 0;
        q_set[3] = -arm_q[1][0]-PI/2;
        q_set[4] = 0;
    }
    if(gimbal_cmd_recv.gimbal_mode == GIMBAL_SILVER_MODE)
    {
        q_set[0] = 0;
        q_set[1] += gimbal_cmd_recv.pitch * DEGREE_2_RAD;
        q_set[2] = 0;
        q_set[3] = -arm_q[1][0];
        q_set[4] = 0;
    }
    if(gimbal_cmd_recv.gimbal_mode == GIMBAL_STORAGE_MODE)
    {
        q_set[0] = 0;
        q_set[1] += gimbal_cmd_recv.pitch * DEGREE_2_RAD;
        q_set[2] = 0;
        q_set[3] = -arm_q[1][0] + 7*PI/6;
        q_set[4] = 0;
    }
    //if(gimbal_cmd_recv.gimbal_mode != GIMBAL_MANUAL_MODE && gimbal_cmd_recv.gimbal_mode != GIMBAL_ZERO_FORCE)
    if(gimbal_cmd_recv.gimbal_mode == GIMBAL_IKINE_MODE)
    {

        if(last_gimbal_mode == GIMBAL_MANUAL_MODE)
        {
            //切换至逆解模式时，目标值设置为当前值
            cmd_R = robotics::t2r( fk_T);
        }
        for (int i = 0; i < 4; ++i) {
            cmd_quat[i][0] = gimbal_cmd_recv.quat[i];
        }

//        if(gimbal_cmd_recv.gimbal_mode == GIMBAL_SILVER_MODE || gimbal_cmd_recv.gimbal_mode == GIMBAL_FLOOR_MODE || gimbal_cmd_recv.gimbal_mode == GIMBAL_STORAGE_MODE)
//        {
//            float trans_rpy_data[3] = {gimbal_cmd_recv.rpy[0]*DEGREE_2_RAD,gimbal_cmd_recv.rpy[1]*DEGREE_2_RAD,gimbal_cmd_recv.rpy[2]*DEGREE_2_RAD};
//            Matrixf<3,1> trans_rpy(trans_rpy_data);
//            cmd_R = robotics::rpy2r(trans_rpy);
//        }
        if(gimbal_cmd_recv.gimbal_mode == GIMBAL_IKINE_MODE)
        {
            float trans_data[3] = {0,-PI,0};
            Matrixf<3,1> trans(trans_data);
            cmd_R = robotics::quat2r(cmd_quat) * robotics::rpy2r(trans); //转到基座坐标系下的姿态
        }

        ik_q3 = robotics::spherical_wrist_ikine(cmd_R,arm_q[0][0],arm_q[1][0]);

            //后三关节的误差 选用误差小的一组解
            float err[2][3] = {0};
            Matrixf<3,1> arm_q3 = arm_q.block<3,1>(2,0);
            for (int i = 0; i < 2; ++i) {
                Matrixf<3,1> ik = ik_q3.block<3,1>(0,i);
                for (int j = 0; j < 3; ++j) {
                    err[i][j] = loop_float_constrain(ik[j][0] - arm_q3[j][0],-PI,PI);
                }
            }
            float err1 = abs(err[1][0]) + abs(err[1][1]) + abs(err[1][2]);
            float err0 = abs(err[0][0]) + abs(err[0][1]) + abs(err[0][2]);
            //选用 总 误差小的一组解
//            if (err1 >= err0)
//                ik_q3_cmd = ik_q3.block<3,1>(0,0);
//            else
//                ik_q3_cmd = ik_q3.block<3,1>(0,1);

//            // 选用 roll 误差小的一组解
            float roll_err1 = ik_q3[0][0] - arm_q3[0][0];
            roll_err1 = loop_float_constrain(roll_err1,-PI,PI);

            float roll_err2 = ik_q3[1][0] - arm_q3[0][0];
            roll_err2 = loop_float_constrain(roll_err2,-PI,PI);


            if (abs(roll_err1) <= abs(roll_err2))
                ik_q3_cmd = ik_q3.block<3,1>(0,0);
            else
                ik_q3_cmd = ik_q3.block<3,1>(0,1);
            //选用 第一组解
        //ik_q3_cmd = ik_q3.block<3,1>(0,0);

            float q_err[3] = {ik_q3_cmd[0][0] - arm_q3[0][0],
                              ik_q3_cmd[1][0] - arm_q3[1][0],
                              ik_q3_cmd[2][0] - arm_q3[2][0]};

            ik_q3_cmd[0][0] += loop_float_constrain(ik_q3_cmd[0][0] - arm_q3[0][0],-PI,PI);
            ik_q3_cmd[1][0] += loop_float_constrain(ik_q3_cmd[1][0] - arm_q3[1][0],-PI,PI);
            ik_q3_cmd[2][0] += loop_float_constrain(ik_q3_cmd[2][0] - arm_q3[2][0],-PI,PI);

//            ik_q3_cmd[0][0] = float_constrain(ik_q3_cmd[0][0],-ROLL* DEGREE_2_RAD,ROLL* DEGREE_2_RAD);
            //ik_q3_cmd[1][0] = float_constrain(ik_q3_cmd[1][0],-DIFF_PITCH* DEGREE_2_RAD,DIFF_PITCH* DEGREE_2_RAD);
//            ik_q3_cmd[2][0] = float_constrain(ik_q3_cmd[2][0],-DIFF_ROLL* DEGREE_2_RAD,DIFF_ROLL* DEGREE_2_RAD);

            for (int i = 0; i < 3; ++i) {
                first_order_filter_cali(&Qset_filter[i],ik_q3_cmd[i][0]);
                q_set[i+2] = Qset_filter[i].out;
            }
            //大臂pitch用遥控器数据
        q_set[1] += gimbal_cmd_recv.pitch * DEGREE_2_RAD;


    }
    //保存上次模式
    last_gimbal_mode = gimbal_cmd_recv.gimbal_mode;

    q_set[1] = float_constrain(q_set[1],PITCH_MIN* DEGREE_2_RAD,PITCH_MAX* DEGREE_2_RAD);

    q_set[2] = float_constrain(q_set[2],-ROLL* DEGREE_2_RAD,ROLL* DEGREE_2_RAD);

    q_set[3] = float_constrain(q_set[3],-DIFF_PITCH* DEGREE_2_RAD,DIFF_PITCH* DEGREE_2_RAD);
    q_set[4] = float_constrain(q_set[4],-DIFF_ROLL* DEGREE_2_RAD,DIFF_ROLL* DEGREE_2_RAD);


    float diff_pitch_angle_out = PIDCalculate(&diff_pitch_loop, diff_pitch_angle, q_set[3]);
    float diff_roll_angle_out = PIDCalculate(&diff_roll_loop, diff_roll_angle, q_set[4]);

    float r_speed_set = diff_pitch_angle_out + diff_roll_angle_out;
    float l_speed_set = diff_pitch_angle_out - diff_roll_angle_out;

    //pitch轴双环PID pitch yaw先遥控器控制
    float pitch_angle_out = PIDCalculate(&pitch_angle_loop,pitch_motor_l->measure.position,q_set[1]);
    float pitch_spd_out = PIDCalculate(&pitch_spd_loop,pitch_spd,pitch_angle_out) + arm_gravity_feedforward;

    DMMotorSetRef(pitch_motor_l,pitch_spd_out / 2);
    DMMotorSetRef(pitch_motor_r, -pitch_spd_out / 2);
    DMMotorSetRef(yaw_motor,q_set[0]);

    DMMotorSetRef(roll_motor,q_set[2]);

    DMMotorSetRef(diff_r_motor,r_speed_set);
    DMMotorSetRef(diff_l_motor,-l_speed_set);




    // 在合适的地方添加pitch重力补偿前馈力矩
    // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
    // ...

    // 设置反馈数据,主要是imu和yaw的ecd
    gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
    gimbal_feedback_data.diff_pitch_feedback = diff_pitch_angle * RAD_2_DEGREE;
    gimbal_feedback_data.pitch_feedback = - pitch_motor_l->measure.position * RAD_2_DEGREE;

    // 推送消息
    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
}