f_mapxxx 函数修改，统一一个函数处理

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

Applications/foc/core/thro_torque.c

@@ -48,11 +48,7 @@ float thro_torque_gear_map(s16 rpm, u8 gear) {
 			float min_rpm = (i * 1000);
 			float trq2 = gear_rpm_torque(_current_gear->torque[i], _current_gear->max_torque);
 			float max_rpm = (i + 1) * 1000;
-			if (trq1 > trq2) {
-				return f_map_inv((float)rpm, min_rpm, max_rpm, trq2, trq1);
-			}else {
-				return f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
-			}
+			return f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
 		}
 	}
 	return gear_rpm_torque(_current_gear->torque[CONFIG_GEAR_SPEED_TRQ_NUM-1], _current_gear->max_torque);

Applications/foc/core/torque_unused.c

@@ -47,11 +47,7 @@ float torque_max_from_gear_rpm(void) {
 			float min_rpm = map[i-1].rpm;
 			float trq2 = map[i].torque;
 			float max_rpm = map[i].rpm;
-			if (trq1 > trq2) {
-				return f_map_inv((float)rpm, min_rpm, max_rpm, trq2, trq1);
-			}else {
-				return f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
-			}
+			return f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
 		}
 	}
 	return (float)map[map_size-1].torque;	

Applications/foc/motor/motor.c

@@ -274,28 +274,19 @@ float mc_gear_max_torque(s16 vel, u8 gear_n) {
 		return 0;
 	}
 
-	if (vel > gear->max_speed) {
-		vel = gear->max_speed;
-	}
 	vel = ABS(vel);
 	if (vel <= 1000) {
 		return gear_rpm_2_torque(gear->torque[0], gear->max_torque);
 	}
-
-	for (int i = 1; i < CONFIG_GEAR_SPEED_TRQ_NUM; i++) {
-		if (vel <= 1000 * (i + 1)) { //线性插值
-			float trq1 = gear_rpm_2_torque(gear->torque[i-1], gear->max_torque);
-			float min_rpm = (i * 1000);
-			float trq2 = gear_rpm_2_torque(gear->torque[i], gear->max_torque);
-			float max_rpm = (i + 1) * 1000;
-			if (trq1 > trq2) {
-				return f_map_inv((float)vel, min_rpm, max_rpm, trq2, trq1);
-			}else {
-				return f_map((float)vel, min_rpm, max_rpm, trq1, trq2);
-			}
-		}
+	int vel_idx = vel / 1000;
+	if (vel >= CONFIG_GEAR_SPEED_TRQ_NUM -1 ) {
+		return gear_rpm_2_torque(gear->torque[CONFIG_GEAR_SPEED_TRQ_NUM-1], gear->max_torque);
 	}
-	return gear_rpm_2_torque(gear->torque[CONFIG_GEAR_SPEED_TRQ_NUM-1], gear->max_torque);
+	float torque_1 = gear_rpm_2_torque(gear->torque[vel_idx-1], gear->max_torque);
+	float min_rpm = vel_idx * 1000;
+	float torque_2 = gear_rpm_2_torque(gear->torque[vel_idx], gear->max_torque);
+	float max_rpm = min_rpm + 1000;
+	return f_map((float)vel, min_rpm, max_rpm, torque_1, torque_2);
 }
 
 

Applications/foc/motor/motor_param.c

@@ -35,11 +35,7 @@ s16 motor_max_torque_for_rpm(s16 rpm) {
 			float min_rpm = mot_map[i-1].rpm;
 			float trq2 = mot_map[i].torque;
 			float max_rpm = mot_map[i].rpm;
-			if (trq1 > trq2) {
-				return (s16)f_map_inv((float)rpm, min_rpm, max_rpm, trq2, trq1);
-			}else {
-				return (s16)f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
-			}
+			return (s16)f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
 		}
 	}
 	return mot_map[map_size-1].torque;

Applications/math/fast_math.h

@@ -53,14 +53,14 @@ static void fast_norm_angle(float *angle) {
 		*angle += 360.0f;
 	}
 }
-/* 递增map */
-static __INLINE float f_map(float x, float in_min, float in_max, float out_min, float out_max) {
-	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
-}
 
-/* 递减map */
-static __INLINE float f_map_inv(float x, float in_min, float in_max, float out_min, float out_max) {
-	return out_max - (x - in_min) * (out_max - out_min) / (in_max - in_min);
+
+static __INLINE float f_map(float x, float in_min, float in_max, float out1, float out2) {
+	if (out1 > out2) { /* 递增map */
+		return out1 - (x - in_min) * (out1 - out2) / (in_max - in_min);
+	}else { /* 递减map */
+		return (x - in_min) * (out2 - out1) / (in_max - in_min) + out1;
+	}
 }