加入独立的强制D轴对齐的功能

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

Applications/foc/commands.c

@@ -319,7 +319,11 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		case Foc_Force_Open_Run:
 		{
 			s16 vd = decode_s16((u8 *)command->data);
-			mc_force_run_open(vd, 0);
+			bool align = false;
+			if (command->len > 2) {
+				align = decode_u8((u8 *)command->data + 2)==1?true:false;
+			}
+			mc_force_run_open(vd, 0, align);
 			break;
 		}
 		case Foc_Set_Open_Dq_Vol:

Applications/foc/motor/motor.c

@@ -748,7 +748,7 @@ u16 mc_get_running_status2(void) {
 static float _force_angle = 0.0f;
 static int   _force_wait = 2000;
 /* 开环，强制给定电角度和DQ的电压 */
-void mc_force_run_open(s16 vd, s16 vq) {
+void mc_force_run_open(s16 vd, s16 vq, bool align) {
 	if (motor.b_start || motor.b_force_run) {
 		if (vd == 0 && vq == 0) {
 			PMSM_FOC_SetOpenVdq(0, 0);
@@ -782,8 +782,12 @@ void mc_force_run_open(s16 vd, s16 vq) {
 	pwm_enable_channel();
 	phase_current_calibrate_wait();
 	PMSM_FOC_Set_MotAngle(0);
-	PMSM_FOC_SetOpenVdq((float)vd * 1.0f, 0);
-	_force_wait = 2000;
+	PMSM_FOC_SetOpenVdq((float)vd, 0);
+	if (align) {
+		_force_wait = 2000 + 1;
+	}else {
+		_force_wait = 2000;
+	}
 	motor.b_force_run = true;
 }
 
@@ -1346,7 +1350,7 @@ static void mc_process_epm_move(void) {
 
 static bool mc_process_force_running(void) {
 	if (motor.b_calibrate || (motor.mode == CTRL_MODE_OPEN)) {
-		if (motor.b_force_run) {
+		if (motor.b_force_run && _force_wait <= 2000) {
 			if (_force_wait > 0) {
 				--_force_wait;
 			}else {

Applications/foc/motor/motor.h

@@ -90,7 +90,7 @@ bool mc_set_critical_error(u8 err);
 void mc_clr_critical_error(u8 err);
 u32 mc_get_critical_error(void);
 void mc_set_fan_duty(u8 duty);
-void mc_force_run_open(s16 vd, s16 vq);
+void mc_force_run_open(s16 vd, s16 vq, bool align);
 u16 mc_get_running_status2(void);
 bool mc_enable_cruise(bool enable);
 bool mc_is_cruise_enabled(void);