#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_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_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(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_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_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.01f,//0.02f, .MaxOut = 10, .Improve = static_cast(PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, .angle_PID = { .Kp = 15.0f,//25.0f, .Ki = 0.0f, .Kd = 0.0f, .MaxOut = 10, .Improve = static_cast(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(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_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 = 30.0f,//20.0F,//15.0f, .Ki = 0, .Kd = 0, .MaxOut = 10, .Improve = static_cast(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 = 50.0f, .Ki = 0, .Kd = 0, .MaxOut = 10, .Improve = static_cast(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.2f,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.total_angle - diff_motor_r_offset; float diff_l_angle = diff_l_motor->measure.total_angle - 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]); static uint8_t last_zero_flag = 0; if(gimbal_cmd_recv.set_zero_flag != last_zero_flag) { diff_motor_r_offset = diff_r_motor->measure.total_angle; diff_motor_l_offset = diff_l_motor->measure.total_angle; PIDClear(&diff_pitch_loop); PIDClear(&diff_roll_loop); PIDClear(&diff_r_motor->motor_controller.speed_PID); PIDClear(&diff_l_motor->motor_controller.speed_PID); //目标值也清零 q_set[3] = 0 ; q_set[4] = 0 ; } last_zero_flag = gimbal_cmd_recv.set_zero_flag; if(gimbal_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE) { DMMotorStop(pitch_motor_l);DMMotorStop(pitch_motor_r); DMMotorStop(yaw_motor);DMMotorStop(roll_motor); DMMotorStop(diff_r_motor);DMMotorStop(diff_l_motor); //不使能模式下 目标值为当前值 防止使能甩大臂 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); //roll轴不做限位处理 //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); }