diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index e9345cc..20137e0 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -240,7 +240,8 @@ void DJIMotorControl() Motor_Controller_s *motor_controller; // 电机控制器 DJI_Motor_Measure_s *measure; // 电机测量值 float pid_measure, pid_ref; // 电机PID测量值和设定值 - + float speed_acceleration; // 加速度 + // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值 for (size_t i = 0; i < idx; ++i) { // 减小访存开销,先保存指针引用 @@ -268,7 +269,14 @@ void DJIMotorControl() if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))) { if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD) + { + // 计算加速度,并作为前馈数据 + speed_acceleration = motor_controller->speed_PID.Measure - motor_controller->speed_PID.Last_Measure; + if(speed_acceleration >= SPEED_FEEDFORWARD_MAX_DIFFERENTIABLE) // 防止微分无穷大 + speed_acceleration = SPEED_FEEDFORWARD_MAX_DIFFERENTIABLE; + motor_controller->speed_feedforward_ptr = &speed_acceleration; pid_ref += *motor_controller->speed_feedforward_ptr; + } if (motor_setting->speed_feedback_source == OTHER_FEED) pid_measure = *motor_controller->other_speed_feedback_ptr; diff --git a/modules/motor/DJImotor/dji_motor.h b/modules/motor/DJImotor/dji_motor.h index bd07c1a..73a91de 100644 --- a/modules/motor/DJImotor/dji_motor.h +++ b/modules/motor/DJImotor/dji_motor.h @@ -26,7 +26,9 @@ /* 滤波系数设置为1的时候即关闭滤波 */ #define SPEED_SMOOTH_COEF 0.85f // 最好大于0.85 #define CURRENT_SMOOTH_COEF 0.9f // 必须大于0.9 -#define ECD_ANGLE_COEF_DJI 0.043945f // (360/8192),将编码器值转化为角度制 +#define ECD_ANGLE_COEF_DJI 0.043945f // (360/8192),将编码器值转化为角度制、 + +#define SPEED_FEEDFORWARD_MAX_DIFFERENTIABLE 3000 /* DJI电机CAN反馈信息*/ typedef struct