更新foc，lineramp_t

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

Applications/app/app.c

@@ -128,7 +128,7 @@ static void plot_smo_angle(void) {
 	float smo_angle = foc_observer_sensorless_angle();
 	float delta = smo_angle - motor.controller.foc.in.mot_angle;
 	float s, c;
-	arm_sin_cos_f32(delta, &s, &c);
+	arm_sin_cos(delta, &s, &c);
 	delta = fast_atan2(s, c)/PI*180.0f;
 	can_plot3(motor.controller.foc.in.mot_angle, smo_angle, delta);
 #else
@@ -145,7 +145,7 @@ static u32 _app_plot_task(void * args) {
 	}else if (plot_type == 4) {
 		can_plot2((s16)motor.controller.foc.out.curr_dq.d, (s16)motor.controller.foc.out.curr_dq.q);
 	}else if (plot_type == 5) {
-		can_plot2((s16)motor.controller.foc.ramp_curr_dq.d , (s16)motor.controller.foc.ramp_curr_dq.d);
+		can_plot2((s16)motor.controller.foc.in.target_id.interpolation , (s16)motor.controller.foc.in.target_iq.interpolation);
 	}else if (plot_type == 6) {
 		//do it in other place
 	}else if (plot_type == 7) {

Applications/foc/core/controller.c

@@ -20,8 +20,6 @@ void mot_contrl_init(mot_contrl_t *ctrl) {
 	ctrl->foc.half_period = FOC_PWM_Half_Period;
 	ctrl->force_angle = INVALID_ANGLE;
 	ctrl->adv_angle = INVALID_ANGLE;
-	ctrl->foc.d_ramp_time = CURRENT_LOOP_RAMP_COUNT;
-	ctrl->foc.q_ramp_time = CURRENT_LOOP_RAMP_COUNT;
 	ctrl->hwlim.dc_curr = CONFIG_HW_MAX_DC_CURRENT;
 	ctrl->hwlim.mot_vel = CONFIG_HW_MAX_MOTOR_RPM;
 	ctrl->hwlim.phase_curr = CONFIG_HW_MAX_PHASE_CURR;
@@ -296,8 +294,7 @@ static void mot_contrl_dq_assign(mot_contrl_t *ctrl) {
 		motor_mpta_fw_lookup(ctrl->foc.in.mot_velocity, ctrl->target_torque, &ctrl->target_idq);
 	}
 	u32 mask = cpu_enter_critical();
-	ctrl->foc.in.target_curr_dq.d = ctrl->target_idq.d;
-	ctrl->foc.in.target_curr_dq.q = ctrl->target_idq.q;
+	foc_set_target_idq(&ctrl->foc, &ctrl->target_idq);
 	cpu_exit_critical(mask);
 }
 

Applications/foc/core/controller.h

@@ -92,15 +92,6 @@ typedef struct {
 	float dc_vol_max;
 }user_limit;
 
-typedef struct {
-	float target;
-	float interpolation;
-	float step;
-	float time;
-	float time_dec;
-	bool  b_force_step;
-}lineramp_t;
-
 typedef struct {
 	lineramp_t vel;
 	lineramp_t torque;
@@ -203,19 +194,6 @@ void mot_contrl_calc_current(mot_contrl_t *ctrl);
 void mot_contrl_get_pid(mot_contrl_t *ctrl, u8 id, float *kp, float *ki, float *kd);
 void mot_contrl_set_pid(mot_contrl_t *ctrl, u8 id, float kp, float ki, float kd);
 
-static __INLINE void line_ramp_set_target(lineramp_t *line, float target) {
-	if (line->target != target) {
-		line->target = target;
-		if (!line->b_force_step) {
-			float delta = (line->target - line->interpolation);
-			if (delta >= 0) {
-				line->step = delta / line->time;
-			}else {
-				line->step = -delta / line->time_dec;
-			}
-		}
-	}
-}
 
 static __INLINE bool mot_contrl_start(mot_contrl_t *ctrl, u8 mode) {
 	mot_contrl_enable(ctrl, true);
@@ -301,64 +279,5 @@ static __INLINE u32 mot_contrl_get_torque_acc_time(mot_contrl_t *ctrl) {
 	return ctrl->input_torque.time;
 }
 
-static __INLINE void line_ramp_init(lineramp_t *line, float time) {
-	line->target = 0;
-	line->interpolation = 0;
-	line->step = 0;
-	line->b_force_step = false;
-	line->time_dec = line->time = time;
-}
-
-static __INLINE void line_ramp_reset(lineramp_t *line, float target) {
-	line->target = target;
-	line->interpolation = target;
-}
-
-
-static __INLINE void line_ramp_set_time(lineramp_t *line, float time) {
-	line->time_dec = line->time = time;
-	line->b_force_step = false;
-}
-
-static __INLINE void line_ramp_set_dectime(lineramp_t *line, float time) {
-	line->time_dec = time;
-	line->b_force_step = false;
-}
-
-static __INLINE void line_ramp_set_acctime(lineramp_t *line, float time) {
-	line->time = time;
-	line->b_force_step = false;
-}
-
-static __INLINE void line_ramp_set_step(lineramp_t *line, float step) {
-	line->step = 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);
-		if (delta >= 0) {
-			line->step = delta / line->time;
-		}else {
-			line->step = -delta / line->time_dec;
-		}
-	}
-}
-
-static __INLINE float line_ramp_step(lineramp_t *line) {
-	step_towards(&line->interpolation, line->target, line->step);
-	return line->interpolation;
-}
-
 #endif /* _CONTROLLER_H__ */
 

Applications/foc/core/foc.c

@@ -22,8 +22,10 @@ void foc_init(foc_t *foc) {
 	foc->qaxis.ki = mc_conf()->c.pid[PID_IQ_ID].ki;
 	foc->qaxis.kd = mc_conf()->c.pid[PID_IQ_ID].kd;
 	foc->qaxis.DT = foc->ts;
-	foc->ramp_curr_dq.d = 0;
-	foc->ramp_curr_dq.q = 0;
+	line_ramp_init(&foc->in.target_id, CURRENT_LOOP_RAMP_COUNT);
+	line_ramp_set_minstep(&foc->in.target_id, 5.0f);
+	line_ramp_init(&foc->in.target_iq, CURRENT_LOOP_RAMP_COUNT);
+	line_ramp_set_minstep(&foc->in.target_iq, 5.0f);
 	foc->mot_velocity_filterd = 0;
 	foc->mot_vel_radusPers = 0;
 	memset(&foc->in, 0, sizeof(foc_in_t));
@@ -64,20 +66,13 @@ void foc_update(foc_t *foc) {
 	arm_sin_cos(foc->in.mot_angle, &foc->sin, &foc->cos);
 	park(foc, &foc->in.curr_ab, &foc->out.curr_dq);
 	if (!foc->in.b_openloop) {
-		float d_step = (foc->in.target_curr_dq.d - foc->ramp_curr_dq.d) / foc->d_ramp_time;
-		float q_step = (foc->in.target_curr_dq.q - foc->ramp_curr_dq.q) / foc->q_ramp_time;
-		if (d_step < 0) {
-			d_step = -d_step;
-		}
-		if (q_step < 0) {
-			q_step = -q_step;
-		}
+
 		/* limiter Vd output for PI controller */
 		float max_vd = max_Vdc * SQRT3_BY_2;
 		foc->daxis.max = max_vd;
 		foc->daxis.min = -max_vd;
-		step_towards(&foc->ramp_curr_dq.d, foc->in.target_curr_dq.d, d_step);
-		float err = foc->ramp_curr_dq.d - foc->out.curr_dq.d;
+		
+		float err = line_ramp_step(&foc->in.target_id) - foc->out.curr_dq.d;
 		float id_ff = id_feedforward(foc);
 		foc->in.target_vol_dq.d = PI_Controller_Current(&foc->daxis, err, id_ff);
 
@@ -85,8 +80,7 @@ void foc_update(foc_t *foc) {
 		float max_vq = sqrtf(SQ(max_vd) - SQ(foc->in.target_vol_dq.d));
 		foc->qaxis.max = max_vq;
 		foc->qaxis.min = -max_vq;
-		step_towards(&foc->ramp_curr_dq.q, foc->in.target_curr_dq.q, q_step);
-		err = foc->ramp_curr_dq.q - foc->out.curr_dq.q;
+		err = line_ramp_step(&foc->in.target_iq) - foc->out.curr_dq.q;
 		float iq_ff = iq_feedforward(foc);
 		foc->in.target_vol_dq.q = PI_Controller_Current(&foc->qaxis, err, iq_ff);
 	}else {

Applications/foc/core/foc.h

@@ -2,6 +2,7 @@
 #define _FOC_H__
 #include "math/fix_math.h"
 #include "foc/core/PI_Controller.h"
+#include "foc/lineramp.h"
 
 typedef struct {
 	float a;
@@ -19,7 +20,8 @@ typedef struct {
 	float   vol_abc[3]; //相对地电压
 	float   mot_velocity;   //from hall or encoder
 	float 	mot_angle; //from hall or encoder
-	dq_t    target_curr_dq;
+	lineramp_t target_id;
+	lineramp_t target_iq;
 	dq_t    target_vol_dq;
 	float 	dc_vol;
 	bool    b_openloop;
@@ -41,11 +43,8 @@ typedef struct {
 typedef struct {
 	foc_in_t in;
 	foc_out_t out;
-	dq_t    ramp_curr_dq;
 	float   mot_velocity_filterd; //电机滤波后的转速
 	float   mot_vel_radusPers; //电机的电角速度
-	float   d_ramp_time;
-	float   q_ramp_time;
 	float   ts;
 	float 	sin;
 	float 	cos;
@@ -56,8 +55,14 @@ typedef struct {
 
 void foc_init(foc_t *foc);
 void foc_update(foc_t *foc);
+
 void foc_abc_2_dq(float a, float b, float c, float *d, float *q);
 
+static __INLINE void foc_set_target_idq(foc_t *foc, dq_t *idq) {
+	line_ramp_set_target(&foc->in.target_id, idq->d);
+	line_ramp_set_target(&foc->in.target_iq, idq->q);
+}
+
 static __INLINE void clark(float A, float B, float C, albt_t *alpha_beta){
 	alpha_beta->a = A;
 	alpha_beta->b = ONE_BY_SQRT3 * (B - C);

Applications/foc/lineramp.h

@@ -0,0 +1,82 @@
+#ifndef _LINE_RAMP_H__
+#define _LINE_RAMP_H__
+
+typedef struct {
+	float target;
+	float interpolation;
+	float step;
+	float time;
+	float time_dec;
+	float min_step;
+}lineramp_t;
+
+static __INLINE void line_ramp_set_target(lineramp_t *line, float target) {
+	if (line->target != target) {
+		line->target = target;
+		float delta = (line->target - line->interpolation);
+		if (delta >= 0) {
+			line->step = delta / line->time;
+		}else {
+			line->step = -delta / line->time_dec;
+		}
+		if ((line->min_step >= 0) && (line->step < line->min_step)) {
+			line->step = line->min_step;
+		}
+	}
+}
+
+static __INLINE void line_ramp_init(lineramp_t *line, float time) {
+	line->target = 0;
+	line->interpolation = 0;
+	line->step = 0;
+	line->min_step = 0;
+	line->time_dec = line->time = time;
+}
+
+static __INLINE void line_ramp_reset(lineramp_t *line, float target) {
+	line->target = target;
+	line->interpolation = target;
+}
+
+
+static __INLINE void line_ramp_set_time(lineramp_t *line, float time) {
+	line->time_dec = line->time = time;
+}
+
+static __INLINE void line_ramp_set_dectime(lineramp_t *line, float time) {
+	line->time_dec = time;
+}
+
+static __INLINE void line_ramp_set_acctime(lineramp_t *line, float time) {
+	line->time = time;
+}
+
+static __INLINE void line_ramp_set_minstep(lineramp_t *line, float step) {
+	line->min_step = step;
+}
+
+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) {
+	float delta = (line->target - line->interpolation);
+	if (delta >= 0) {
+		line->step = delta / line->time;
+	}else {
+		line->step = -delta / line->time_dec;
+	}
+}
+
+static __INLINE float line_ramp_step(lineramp_t *line) {
+	step_towards(&line->interpolation, line->target, line->step);
+	return line->interpolation;
+}
+
+
+#endif /* _LINE_RAMP_H__ */
+

Applications/foc/motor/motor.c

@@ -1101,8 +1101,8 @@ static void mc_save_err_runtime(void) {
 	mc_error.vbus_x10 = (s16)(sample_vbus_raw() * 10.0f); 
 	mc_error.ibus_x10 = (s16)(sample_ibus_raw() * 10.0f);
 	mc_error.vacc_x10 = (s16) (sample_acc_vol_raw() * 10.0f);
-	mc_error.id_ref_x10 = (s16)(motor.controller.foc.ramp_curr_dq.d * 10.0f);
-	mc_error.iq_ref_x10 = (s16)(motor.controller.foc.ramp_curr_dq.q * 10.0f);
+	mc_error.id_ref_x10 = (s16)(motor.controller.foc.in.target_id.interpolation * 10.0f);
+	mc_error.iq_ref_x10 = (s16)(motor.controller.foc.in.target_iq.interpolation * 10.0f);
 	mc_error.id_x10 = (s16)(motor.controller.foc.out.curr_dq.d * 10.0f);
 	mc_error.iq_x10 = (s16)(motor.controller.foc.out.curr_dq.q * 10.0f);
 	mc_error.vd_x10 = (s16)(motor.controller.foc.out.vol_dq.d * 10.0f);