解决速度环问题

Signed-off-by: huhui <huhui@sharkgulf.com>

Applications/foc/core/controller.c

@@ -120,7 +120,7 @@ u8 mot_contrl_mode(mot_contrl_t *ctrl) {
 				ctrl->target_torque_raw = ctrl->target_torque;
 				PI_Controller_Reset(&ctrl->pi_vel_lim, ctrl->target_torque_raw);
 			}else if (preMode == CTRL_MODE_CURRENT) {
-				ctrl->target_torque_raw = ctrl->target_current.interpolation;
+				ctrl->target_torque_raw = line_ramp_get_interp(&ctrl->target_current);
 				PI_Controller_Reset(&ctrl->pi_vel_lim, ctrl->target_torque_raw);
 			}
 		}else if ((preMode == CTRL_MODE_TRQ) && (ctrl->mode_running == CTRL_MODE_SPD)) {
@@ -241,8 +241,8 @@ bool mot_contrl_update(mot_contrl_t *ctrl) {
 	foc->in.b_openloop = ctrl->mode_running == CTRL_MODE_OPEN;
 	phase_curr_unbal_check(ctrl);
 	if (foc->in.b_openloop) {
-		foc->in.target_vol_dq.d = ctrl->target_vd.interpolation;
-		foc->in.target_vol_dq.q = ctrl->target_vq.interpolation;
+		foc->in.target_vol_dq.d = line_ramp_get_interp(&ctrl->target_vd);
+		foc->in.target_vol_dq.q = line_ramp_get_interp(&ctrl->target_vq);
 	}
 
 	foc_update(foc);
@@ -258,7 +258,7 @@ bool mot_contrl_update(mot_contrl_t *ctrl) {
 
 static __INLINE float mot_contrl_dc_curr_limiter(mot_contrl_t *ctrl, float maxTrq) {
 	ctrl->pi_power.max = maxTrq;
-	float errRef = ctrl->dc_curr_lim.interpolation - ctrl->dc_curr_filted;
+	float errRef = line_ramp_get_interp(&ctrl->dc_curr_lim) - ctrl->dc_curr_filted;
 	return PI_Controller_Run(&ctrl->pi_power, errRef);
 }
 
@@ -266,14 +266,14 @@ static __INLINE float mot_contrl_vel_limiter(mot_contrl_t *ctrl, float maxTrq) {
 	ctrl->pi_vel_lim.max = maxTrq;
 	ctrl->pi_vel_lim.min = 0;
 
-	float err = ctrl->vel_lim.interpolation - ctrl->foc.in.mot_velocity;
+	float err = line_ramp_get_interp(&ctrl->vel_lim) - ctrl->foc.in.mot_velocity;
 	return PI_Controller_RunVel(&ctrl->pi_vel_lim, err);
 }
 
 /* current vector or torque to dq axis current */
 static void mot_contrl_dq_assign(mot_contrl_t *ctrl) {
 	if (ctrl->mode_running == CTRL_MODE_CURRENT) {
-		float target_current = ctrl->target_current.interpolation;
+		float target_current = line_ramp_get_interp(&ctrl->target_current);
 		if (ctrl->b_mtpa_calibrate && (ctrl->adv_angle != INVALID_ANGLE)) {
 			float s, c;
 			normal_sincosf(degree_2_pi(ctrl->adv_angle + 90.0f), &s, &c);
@@ -360,24 +360,27 @@ void mot_contrl_dq_calc(mot_contrl_t *ctrl) {
 		line_ramp_step(&ctrl->target_current);
 	}else if (ctrl->mode_running == CTRL_MODE_EBRAKE) {
 		float maxTrq = line_ramp_step(&ctrl->input_torque);
-		if (ctrl->input_torque.target < 0.0001f && foc->in.mot_velocity < CONFIG_MIN_RPM_EXIT_EBRAKE) {
+		if (line_ramp_get_target(&ctrl->input_torque) < 0.0001f && foc->in.mot_velocity < CONFIG_MIN_RPM_EXIT_EBRAKE) {
 			maxTrq = 0;
 		}
 		crosszero_step_towards(&ctrl->target_torque, maxTrq);
 	}else if (ctrl->mode_running == CTRL_MODE_TRQ) {
 		float refTorque = line_ramp_step(&ctrl->input_torque);
-		refTorque = min(refTorque, ctrl->torque_lim.interpolation) * etcs_out;
+		refTorque = min(refTorque, line_ramp_get_interp(&ctrl->torque_lim)) * etcs_out;
 		float maxTrq = mot_contrl_vel_limiter(ctrl, refTorque);
 		ctrl->target_torque_raw = mot_contrl_dc_curr_limiter(ctrl, maxTrq);
 		crosszero_step_towards(&ctrl->target_torque, ctrl->target_torque_raw);
 	}else if (ctrl->mode_running == CTRL_MODE_SPD){
-		float refSpeed = line_ramp_step(&ctrl->cruise_vel);
-		float maxSpeed = ctrl->target_vel.target;
+		float refSpeed;
+		float maxSpeed;
 		if (ctrl->b_cruiseEna) {
-			maxSpeed = ctrl->cruise_vel.target;
-			refSpeed = ctrl->cruise_vel.interpolation;
+			refSpeed = line_ramp_step(&ctrl->cruise_vel);
+			maxSpeed = line_ramp_get_target(&ctrl->cruise_vel);
+		}else {
+			refSpeed = line_ramp_step(&ctrl->target_vel);
+			maxSpeed = line_ramp_get_target(&ctrl->target_vel);
 		}
-		float max_input = ctrl->torque_lim.interpolation * etcs_out;
+		float max_input = line_ramp_get_interp(&ctrl->torque_lim) * etcs_out;
 		if (maxSpeed >= 0) {
 			ctrl->pi_vel.max = max_input;
 #ifdef CONFIG_SERVO_MOTOR

Applications/foc/core/controller.h

@@ -335,6 +335,15 @@ static __INLINE void line_ramp_set_step(lineramp_t *line, float step) {
 	line->b_force_step = true;
 }
 
+static __INLINE float line_ramp_get_target(lineramp_t *line) {
+	return line->target;
+}
+
+static __INLINE float line_ramp_get_interp(lineramp_t *line) {
+	return line->interpolation;
+}
+
+
 static __INLINE void line_ramp_update(lineramp_t *line) {
 	if (!line->b_force_step) {
 		float delta = (line->target - line->interpolation);

Applications/foc/motor/throttle.c

@@ -271,7 +271,7 @@ float throttle_get_torque(mot_contrl_t * ctrl, float vol) {
 			_throttle.thro_ration_last = _throttle.thro_ration;
 			_throttle.torque_real = ctrl->target_torque_raw;
 			/* 如果扭矩给定的ramp没有结束，使用原始扭矩请求作为减扭矩的起始点 */
-			if (_throttle.torque_req - ctrl->input_torque.interpolation >= 10.0f ) {
+			if (_throttle.torque_req - line_ramp_get_interp(&ctrl->input_torque) >= 10.0f ) {
 				_throttle.torque_real = _throttle.torque_req;
 			}
 			if (_throttle.torque_real < 0) { //电子刹车的时候，扭矩可能为负