加入新的motor_mpta_fw_lookup2，查表电压，转速，转矩和id的关系

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

Applications/foc/motor/motor_param.c

@@ -97,6 +97,121 @@ int motor_map_torque_max_count(void) {
 
 #ifdef MOT_HAVE_MAPS
 
+vol_rpm_torque_map vol_rpm_curr_map[CONFIG_MAX_VOL_COUNT];
+
+static void motor_vol_lookup(s16 vol, vol_rpm_torque_map *table, vol_rpm_torque_map **out_mapl, vol_rpm_torque_map **out_maph) {
+	if (vol <= table[0].vol) {
+		*out_mapl = table;
+        *out_maph = table + 1;
+		return;
+	}else if (vol >= table[CONFIG_MAX_VOL_COUNT-1].vol){
+		*out_mapl = table + CONFIG_MAX_VOL_COUNT - 2;
+        *out_maph = table + CONFIG_MAX_VOL_COUNT - 1;
+		return;
+	}
+	for (int i = 1; i < CONFIG_MAX_VOL_COUNT; i++) {
+		if ((vol <= table[i].vol)) {
+			*out_mapl = table + i - 1;
+			*out_maph = table + i;
+			break;
+		}
+	}
+}
+
+static void motor_rpm_lookup(s16 rpm, vol_rpm_torque_map *map, rpm_torque_map **out_map1, rpm_torque_map **out_maph) {
+	rpm_torque_map *table = map->rpm_torque;
+	if (rpm <= table[0].rpm) {
+		*out_map1 = table;
+        *out_maph = table + 1;
+		return;
+	}else if (rpm >= table[CONFIG_MAX_VEL_COUNT-1].rpm) {
+		*out_map1 = *out_maph = table + CONFIG_MAX_VEL_COUNT - 1;
+		return;
+	}
+	for (int i = 1; i < CONFIG_MAX_VEL_COUNT; i++) {
+		if (rpm <= table[i].rpm) {
+			*out_map1 = table + i - 1;
+			*out_maph = table + i;
+			break;
+		}
+	}
+}
+
+static void motor_current_lookup(s16 torque, rpm_torque_map *map, curr_pair_t **currl, curr_pair_t **currh) {
+	curr_pair_t *table = map->curr;
+	if (torque <= table[0].curr) {
+		*currl = table;
+        *currh = table + 1;
+	}else if (torque >= table[CONFIG_MAX_D_COUNT - 1].curr) {
+		*currl = table + CONFIG_MAX_D_COUNT - 2;
+        *currh = table + CONFIG_MAX_D_COUNT - 1;
+	}
+	for (int i = 1; i < CONFIG_MAX_D_COUNT; i++) {
+		if (torque <= table[i].curr) {
+			*currl = table + i - 1;
+			*currh = table + i;
+			break;
+		}
+	}
+}
+
+static void motor_torque_lookup(s16 vel, s16 torque, rpm_torque_map *mapl, rpm_torque_map *maph, curr_pair_t *curr) {
+	curr_pair_t *currl, *currh;
+	motor_current_lookup(torque, mapl, &currl, &currh);
+	float dl = line_intp(torque, currl->curr, currh->curr, currl->id, currh->id);
+    _DEBUG("rpm %d -> curr %d : %d, id=%f\n", mapl->rpm, currl->curr, currh->curr, dl);
+	motor_current_lookup(torque, maph, &currl, &currh);
+	float dh = line_intp(torque, currl->curr, currh->curr, currl->id, currh->id);
+    _DEBUG("rpm %d -> curr %d : %d, id=%f\n", maph->rpm, currl->curr, currh->curr, dh);
+	float d = line_intp(vel, mapl->rpm, maph->rpm, dl, dh);
+    _DEBUG("id = %f\n", d);
+	curr->curr = torque;
+	curr->id = d;
+
+}
+
+#define SCALE(X) (X/10)
+
+void motor_mpta_fw_lookup2(float rpm, float torque, dq_t *dq_out) {
+	vol_rpm_torque_map *vmapl, *vmaph;
+	rpm_torque_map *rmapl, *rmaph;
+	curr_pair_t currl, currh;
+	s16 vel = ABS(((s16)rpm));
+	s16 bus_vol = get_vbus_int();
+    torque = torque * 10;
+	/* 通过当前电压查找 对应的转速扭矩 */
+	motor_vol_lookup(bus_vol, vol_rpm_curr_map, &vmapl, &vmaph);
+	_DEBUG("vol %d : %d\n", vmapl->vol, vmaph->vol);
+	/* 通过当前转速查找对应的扭矩 */
+	motor_rpm_lookup(vel, vmapl, &rmapl, &rmaph);
+    _DEBUG("vol %d -> rpm %d : %d\n", vmapl->vol, rmapl->rpm, rmaph->rpm);
+	/* 通过当前扭矩需求插值计算dq电流 */
+	motor_torque_lookup(vel, ABS(torque), rmapl, rmaph, &currl);
+
+	motor_rpm_lookup(vel, vmaph, &rmapl, &rmaph);
+    _DEBUG("vol %d -> rpm %d : %d\n", vmaph->vol, rmapl->rpm, rmaph->rpm);
+
+	motor_torque_lookup(vel, ABS(torque), rmapl, rmaph, &currh);
+
+	float d = line_intp(bus_vol, vmapl->vol, vmaph->vol, currl.id, currh.id);
+	float q;
+	if (d >= torque) {
+		d = ABS(torque) * SIGN(torque);
+		q = 0;
+	}else {
+		q = sqrtf(torque * torque - d * d);
+	}
+	if (torque != 0) {
+		dq_out->d = SCALE(d);
+		dq_out->q = torque > 0 ? SCALE(q):SCALE(-q);
+	}else {
+		step_towards(&dq_out->d, SCALE(d), 0.5f);
+		step_towards(&dq_out->q, SCALE(q), 0.5f);
+	}
+
+}
+
+
 //x -> rpm
 //z -> torque
 static void intp_line2(float frac_x, s16 z, torque_map_t **map, float *d, float *q) {

Applications/foc/motor/motor_param.h

@@ -4,6 +4,9 @@
 #include "os/os_types.h"
 #include "foc/core/controller.h"
 
+#define CONFIG_MAX_D_COUNT 20
+#define CONFIG_MAX_VEL_COUNT 20
+#define CONFIG_MAX_VOL_COUNT 5
 /* 电机外特性map, 每个转速点对应的最大扭矩 */
 typedef struct {
 	s16 rpm;
@@ -21,6 +24,24 @@ typedef struct {
 	float lq;
 }iq_lq_map_t;
 typedef torque2dq_t torque_map_t;
+
+typedef struct {
+	s16 curr;
+	s16 id;
+}curr_pair_t;
+
+/* 转速对应的d轴电流 */
+typedef struct {
+	s16 rpm;
+	curr_pair_t curr[CONFIG_MAX_D_COUNT];
+}rpm_torque_map;
+
+/* 电压、转速对应的d轴电流 */
+typedef struct {
+	s16 vol;
+	rpm_torque_map rpm_torque[CONFIG_MAX_VEL_COUNT];
+}vol_rpm_torque_map;
+
 s16 motor_max_torque_for_rpm(s16 rpm);
 void motor_mpta_fw_lookup(float rpm, float torque, dq_t *dq_out);
 float motor_get_lq_from_iq(s16 iq);

Applications/math/fast_math.h

@@ -96,6 +96,10 @@ static __INLINE void step_towards_s16(s16 *value, s16 goal, s16 step) {
     }
 }
 
+static __INLINE s16 line_intp(s16 x, s16 x_l, s16 x_h, s16 y_l, s16 y_h) {
+	float r = (float)(x - x_l) /(float)(x_h - x_l);
+	return (s16)(r * (y_h - y_l)) + y_l;
+}
 
 /**
  * Fast atan2