1. MATH_sat 使用clamp 替代

2. dq电流环给定最小ramp用宏定义

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

Applications/foc/core/PI_Controller.h

@@ -2,29 +2,7 @@
 #define _PI_Contrller_H__
 #include "math/fast_math.h"
 #include "bsp/bsp.h"
-#if 0
-typedef struct {
-	s16q5_t  kp;
-	s16q5_t  ki;
-	s16q5_t  max;
-	s16q5_t  min;
-	s16q5_t  Ui;
-	s16q14_t DT;
-}PI_Controller;
 
-static __INLINE void PI_Controller_Reset(PI_Controller *pi, s16q5_t init) {
-	pi->Ui = init;
-}
-
-static __INLINE s16q5_t PI_Controller_run(PI_Controller *pi, s16q5_t err) {
-	s16q5_t kp_err = S16_mul(err,pi->kp, 5);
-	s16q5_t ki_err = S16_mul(err,pi->ki, 5);
-	s16q5_t integral = S16_mul(ki_err, pi->DT, 14);
-	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
-	s32q14_t out = pi->Ui + kp_err;
-	return MATH_sat(out, pi->min, pi->max);
-}
-#else
 typedef struct {
 	float  kp;
 	float  ki;
@@ -57,9 +35,9 @@ static __INLINE float PI_Controller_Run(PI_Controller *pi, float err) {
 	float kp_err = (err) * pi->kp;
 	float ki_err = (err) * pi->ki;
 	float integral = ki_err * pi->DT;
-	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
+	pi->Ui = fclamp(pi->Ui + integral, pi->min, pi->max);
 	float out = pi->Ui + kp_err;
-	float sat_out =  (MATH_sat(out, pi->min, pi->max));
+	float sat_out = fclamp(out, pi->min, pi->max);
 	if (out != sat_out) {
 		pi->is_sat = true;
 	}else {
@@ -169,6 +147,5 @@ static __INLINE float PLL_run(PLL_t *pll, float sample, float comp) {
 	return pll->out;
 }
 
-#endif
 #endif	/*_PI_Contrller_H__*/
 

Applications/foc/core/adrc.c

@@ -21,7 +21,7 @@ float ladrc_run(ladrc_t *adrc, float in, float feedback) {
 	//do error compestion
 	float ref = in;
 	float out = ((ref - adrc->z1) * adrc->wcv - adrc->z2) / adrc->b0;
-	adrc->out = MATH_sat(out, adrc->out_min, adrc->out_max);
+	adrc->out = fclamp(out, adrc->out_min, adrc->out_max);
 
 	return adrc->out;
 }

Applications/foc/core/foc.c

@@ -22,10 +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;
-	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);
+	line_ramp_init(&foc->in.target_id, CONFIG_CURRENT_LOOP_RAMP_COUNT);
+	line_ramp_set_minstep(&foc->in.target_id, CONFIG_CURRENT_LOOP_RAMP_STEP_MIN);
+	line_ramp_init(&foc->in.target_iq, CONFIG_CURRENT_LOOP_RAMP_COUNT);
+	line_ramp_set_minstep(&foc->in.target_iq, CONFIG_CURRENT_LOOP_RAMP_STEP_MIN);
 	foc->mot_velocity_filterd = 0;
 	foc->mot_vel_radusPers = 0;
 	memset(&foc->in, 0, sizeof(foc_in_t));

Applications/foc/foc_config.h

@@ -41,7 +41,8 @@
 #define CONFIG_RAMP_SECOND_TARGET (5.0F * 1.5F)
 
 /* dq轴电流环指令斜波 */
-#define CURRENT_LOOP_RAMP_COUNT 15.0f //15个控制周期 完成ramp给定
+#define CONFIG_CURRENT_LOOP_RAMP_COUNT 15.0f //15个控制周期 完成ramp给定
+#define CONFIG_CURRENT_LOOP_RAMP_STEP_MIN 5.0F//ramp step 不小于step min
 
 #define CONFIG_CRUISE_ENABLE_ACCL 1 //定速巡航可以加速，油门回退，继续定速巡航
 

Applications/math/fast_math.h

@@ -27,9 +27,6 @@
 void fast_sincos(float angle, float *sin, float *cos);
 void arm_sin_cos(float angle, float *s, float *c);
 
-#define MATH_sat(in, minOut, maxOut) (min((maxOut), MAX((in), (minOut))))
-
-
 static __INLINE int32_t sclamp(int32_t v, int32_t minv, int32_t maxv) {
 	if (v < minv) {
 		return minv;