使用sqrtsub2_f计算两个数据平方差开方

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

Applications/foc/core/controller.c

@@ -289,7 +289,7 @@ static void mot_contrl_dq_assign(mot_contrl_t *ctrl) {
 			}else if (ctrl->target_idq.d < -ctrl->hwlim.fw_id) {
 				ctrl->target_idq.d = -ctrl->hwlim.fw_id;
 			}
-			ctrl->target_idq.q = sqrtf(SQ(target_current) - SQ(ctrl->target_idq.d));
+			ctrl->target_idq.q = sqrtsub2_f(target_current, ctrl->target_idq.d);
 			if (s < 0) {
 				ctrl->target_idq.q = -ctrl->target_idq.q;
 			}

Applications/foc/core/foc.c

@@ -75,7 +75,7 @@ void foc_update(foc_t *foc) {
 		foc->in.target_vol_dq.d = PI_Controller_Current(&foc->daxis, err, id_ff);
 
 		/* limiter Vq output for PI controller */
-		float max_vq = sqrtf(SQ(max_vd) - SQ(foc->in.target_vol_dq.d));
+		float max_vq = sqrtsub2_f(max_vd, foc->in.target_vol_dq.d);
 		foc->qaxis.max = max_vq;
 		foc->qaxis.min = -max_vq;
 		err = line_ramp_step(&foc->in.target_iq) - foc->out.curr_dq.q;
@@ -84,7 +84,7 @@ void foc_update(foc_t *foc) {
 	}else {
 		float max_vd = max_Vdc * SQRT3_BY_2;
 		foc->in.target_vol_dq.d = fclamp(foc->in.target_vol_dq.d, -max_vd, max_vd);
-		float max_vq = sqrtf(SQ(max_vd) - SQ(foc->in.target_vol_dq.d));
+		float max_vq = sqrtsub2_f(max_vd, foc->in.target_vol_dq.d);
 		foc->in.target_vol_dq.q = fclamp(foc->in.target_vol_dq.q, -max_vq, max_vq);
 	}
 

Applications/foc/motor/motor_param.c

@@ -199,7 +199,7 @@ void motor_mpta_fw_lookup2(float rpm, float torque, dq_t *dq_out) {
 		d = ABS(torque) * SIGN(torque);
 		q = 0;
 	}else {
-		q = sqrtf(torque * torque - d * d);
+		q = sqrtsub2_f(torque, d);
 	}
 	if (torque != 0) {
 		dq_out->d = SCALE(d);
@@ -235,7 +235,7 @@ static void intp_line2(float frac_x, s16 z, torque_map_t **map, float *d, float
 #ifdef MOT_USE_PHASE_I
 	if (z != 0) {
 		if (z >= ABS(*d)) {
-			*q = sqrtf(z*z - (*d)*(*d));
+			*q = sqrtsub2_f(z, (*d));
 		}else {
 			c1 = (1.0f - frac_z1) * map[0]->q + frac_z1 * map[1]->q;
 			c2 = (1.0f - frac_z2) * map[2]->q + frac_z2 * map[3]->q;
@@ -330,7 +330,7 @@ void motor_mpta_fw_lookup(float rpm, float torque, dq_t *dq_out) {
 		arm_sin_cos(advanced_angle + 90.0f, &s, &c);
 		d = ABS(torque) * c;
 		d = fclamp(d, -mc_conf()->m.max_fw_id, mc_conf()->m.max_fw_id);
-		q = sqrtf(SQ(torque) - SQ(d));
+		q = sqrtsub2_f(torque, d);
 		if (torque < 0) {
 			q = -q;
 		}

Applications/math/fast_math.h

@@ -20,7 +20,7 @@
 #define SQ(x) ((x)*(x))
 #endif
 #define NORM2_f(x,y)		(sqrtf(SQ(x) + SQ(y)))
-
+#define sqrtsub2_f(x,y)       (sqrtf(SQ(x) - SQ(y)))
 // nan and infinity check for floats
 #define UTILS_IS_INF(x)		((x) == (1.0F / 0.0F) || (x) == (-1.0F / 0.0F))
 #define UTILS_IS_NAN(x)		((x) != (x))