扭矩查表后的D轴电流需要step给定，防止D轴电流突变

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

Applications/foc/core/trq2dq_table.c

@@ -13,7 +13,7 @@
 static trq2dq_t **table_map = NULL;
 //x -> rpm
 //z -> torque
-static void intp_line2(float frac_x, float z, trq2dq_t **map, DQ_t *dq_out) {
+static void intp_line2(float frac_x, float z, trq2dq_t **map, float *d, float *q) {
 	float frac_z1 = 0; //对应x1索引的t_maps
 	float frac_z2 = 0; //对应x2索引的t_maps
 
@@ -30,11 +30,11 @@ static void intp_line2(float frac_x, float z, trq2dq_t **map, DQ_t *dq_out) {
 
 	float c1 = (1.0f - frac_z1) * map[0]->d + frac_z1 * map[1]->d; //第一行插值
 	float c2 = (1.0f - frac_z2) * map[2]->d + frac_z2 * map[3]->d; //第二行插值
-	dq_out->d = c1 * (1.0f - frac_x) + c2 * frac_x;                //两行插值
+	*d = c1 * (1.0f - frac_x) + c2 * frac_x;                //两行插值
 	
 	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;
-	dq_out->q = c1 * (1.0f - frac_x) + c2 * frac_x;
+	*q = c1 * (1.0f - frac_x) + c2 * frac_x;
 }
 
 static void get_torque_range(float z, int index, int max_index, int *left, int *right) {
@@ -60,6 +60,7 @@ static void get_torque_range(float z, int index, int max_index, int *left, int *
 	*right = low_right;
 }
 
+
 void trq2dq_lookup_init(void) {
 	if (table_map == NULL) {
 		trq2dq_table_t *table = nv_get_trq2dq_table();
@@ -71,7 +72,7 @@ void trq2dq_lookup_init(void) {
 
 void trq2dq_lookup(int rpm, float torque, DQ_t *dq_out) {
 	if (table_map == NULL) {
-		dq_out->d = 0;
+		step_towards(&dq_out->d, 0, 1.0f);
 		dq_out->q = torque;
 		return;
 	}
@@ -105,11 +106,13 @@ void trq2dq_lookup(int rpm, float torque, DQ_t *dq_out) {
 	maps[1] = &table_map[low][low_right];
 	maps[2] = &table_map[high][high_left];
 	maps[3] = &table_map[high][high_right];
-	float frac_x = 0;
+	float frac_x = 0, d = 0, q = 0;
 	int x1 = IDX2RPM(low);
 	int x2 = IDX2RPM(high);
 	if (x1 != x2) {
 		frac_x = (float)(rpm - x1)/(x2 - x1);
-	}	
-	intp_line2(frac_x, torque, maps, dq_out);
+	}
+	intp_line2(frac_x, torque, maps, &d, &q);
+	step_towards(&dq_out->d, d, 1.0f);
+	dq_out->q = q;
 }