修改电机外特性查表方式

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

Applications/bsp/gd32/board_mc105_v3.h

@@ -415,7 +415,7 @@
 #endif
 
 #define DEBUG_PORT_UART2
-//#define CONFIG_MOTOR_TORQUE_CONF "foc/motor/A1_motor_config.c"
+#define CONFIG_MOTOR_TORQUE_CONF "foc/motor/A1_motor_config.c"
 
 //#define CONFIG_DQ_STEP_RESPONSE
 

Applications/foc/motor/motor_param.c

@@ -137,17 +137,17 @@ static void motor_rpm_lookup(s16 rpm, vol_rpm_torque_map *map, rpm_torque_map **
 	}
 }
 
-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) {
+static void motor_current_lookup(s16 torque, rpm_torque_map *map,  torque2dq_t**currl, torque2dq_t **currh) {
+	torque2dq_t *table = map->dqmap;
+	if (torque <= table[0].torque) {
 		*currl = table;
         *currh = table + 1;
-	}else if (torque >= table[CONFIG_MAX_D_COUNT - 1].curr) {
+	}else if (torque >= table[CONFIG_MAX_D_COUNT - 1].torque) {
 		*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) {
+		if (torque <= table[i].torque) {
 			*currl = table + i - 1;
 			*currh = table + i;
 			break;
@@ -155,29 +155,36 @@ static void motor_current_lookup(s16 torque, rpm_torque_map *map, curr_pair_t **
 	}
 }
 
-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;
+static void motor_torque_lookup(s16 vel, s16 torque, rpm_torque_map *mapl, rpm_torque_map *maph, dq_t *dq_out) {
+	torque2dq_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);
+	float dl = line_intp(torque, currl->torque, currh->torque, currl->d, currh->d);
+    _DEBUG("rpm %d -> curr %d : %d, id=%f\n", mapl->rpm, currl->torque, currh->torque, 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 dh = line_intp(torque, currl->torque, currh->torque, currl->d, currh->d);
+    _DEBUG("rpm %d -> curr %d : %d, id=%f\n", maph->rpm, currl->torque, currh->torque, dh);
 	float d = line_intp(vel, mapl->rpm, maph->rpm, dl, dh);
     _DEBUG("id = %f\n", d);
-	curr->curr = torque;
-	curr->id = d;
 
+	float ql = line_intp(torque, currl->torque, currh->torque, currl->q, currh->q);
+    _DEBUG("rpm %d -> curr %d : %d, iq=%f\n", mapl->rpm, currl->torque, currh->torque, ql);
+	motor_current_lookup(torque, maph, &currl, &currh);
+	float qh = line_intp(torque, currl->torque, currh->torque, currl->q, currh->q);
+    _DEBUG("rpm %d -> curr %d : %d, id=%f\n", maph->rpm, currl->torque, currh->torque, qh);
+	float q = line_intp(vel, mapl->rpm, maph->rpm, ql, qh);
+    _DEBUG("id = %f\n", d);
+	dq_out->d = d;
+	dq_out->q = q;
 }
 
 #define SCALE(X) (X/10)
 
-void motor_mpta_fw_lookup2(float rpm, float torque, dq_t *dq_out) {
+void motor_mpta_fw_lookup2(float rpm, float vdc, float torque, dq_t *dq_out) {
 	vol_rpm_torque_map *vmapl, *vmaph;
 	rpm_torque_map *rmapl, *rmaph;
-	curr_pair_t currl, currh;
+	dq_t dql, dqh;
 	s16 vel = ABS(((s16)rpm));
-	s16 bus_vol = get_vbus_int();
+	s16 bus_vol = vdc;
     torque = torque * 10;
 	/* 通过当前电压查找 对应的转速扭矩 */
 	motor_vol_lookup(bus_vol, vol_rpm_curr_map, &vmapl, &vmaph);
@@ -186,32 +193,47 @@ void motor_mpta_fw_lookup2(float rpm, float torque, dq_t *dq_out) {
 	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_torque_lookup(vel, ABS(torque), rmapl, rmaph, &dql);
 
 	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);
+	motor_torque_lookup(vel, ABS(torque), rmapl, rmaph, &dqh);
 
-	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 = sqrtsub2_f(torque, 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);
-	}
+	float d = line_intp(bus_vol, vmapl->vol, vmaph->vol, dql.d, dqh.d);
+	float q = line_intp(bus_vol, vmapl->vol, vmaph->vol, dql.q, dqh.q);
 
+	dq_out->d = SCALE(d);
+	dq_out->q = SCALE(q) * SIGN(torque);
 }
 
 
+/* 通过电机外特性曲线获取当前转速和母线电压下的最大扭矩 */
+s16 motor_torque_ext_char_curve(s16 rpm, s16 vdc) {
+	vol_rpm_torque_map *vmapl, *vmaph;
+	rpm_torque_map *rmapl, *rmaph;
+	s16 vel = ABS(rpm);
+	/* 通过当前电压查找 对应的转速扭矩 */
+	motor_vol_lookup(vdc, 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);
+	s16 max_t1 = rmapl->dqmap[CONFIG_MAX_D_COUNT-1].torque;
+	s16 max_t2 = rmaph->dqmap[CONFIG_MAX_D_COUNT-1].torque;
+	s16 low_vel_max_t = line_intp(vel, rmapl->rpm, rmaph->rpm, max_t1, max_t2);
+
+	/* 获取高电压对应当前转速的最大扭矩 */
+	motor_rpm_lookup(vel, vmaph, &rmapl, &rmaph);
+    _DEBUG("vol %d -> rpm %d : %d\n", vmaph->vol, rmapl->rpm, rmaph->rpm);
+	max_t1 = rmapl->dqmap[CONFIG_MAX_D_COUNT-1].torque;
+	max_t2 = rmaph->dqmap[CONFIG_MAX_D_COUNT-1].torque;
+	s16 high_vel_max_t = line_intp(vel, rmaph->rpm, rmaph->rpm, max_t1, max_t2);
+
+	/* 对两个电压的扭矩插值，获取最终的最大扭矩 */
+	return line_intp(vdc, vmapl->vol, vmaph->vol, low_vel_max_t, high_vel_max_t);
+}
+
 //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

@@ -33,7 +33,7 @@ typedef struct {
 /* 转速对应的d轴电流 */
 typedef struct {
 	s16 rpm;
-	curr_pair_t curr[CONFIG_MAX_D_COUNT];
+	torque2dq_t dqmap[CONFIG_MAX_D_COUNT];
 }rpm_torque_map;
 
 /* 电压、转速对应的d轴电流 */