1.堵转处理，单独一个函数

2.report foc status加入启动时间，挡位s

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

Applications/foc/motor/motor.c

@@ -144,7 +144,7 @@ bool mc_start(u8 mode) {
 	if (mc_is_hwbrake()) {
 		PMSM_FOC_Brake(true);
 	}
-	gpio_beep(1000);
+	gpio_beep(200);
 	return true;
 }
 
@@ -486,34 +486,12 @@ static bool mc_can_stop_foc(void) {
 	return false;
 }
 
-//#define ANGLE_TEST
-#ifdef ANGLE_TEST
-static void _debug_angle(void) {
-	if (motor.b_start) {
-		PMSM_FOC_Set_Angle(motor.s_testAngle);
-		if (++motor.s_testAngle >= 360) {
-			motor.s_testAngle = 0;
-		}
-	}
-}
-#endif
-/*FOC 的部分处理，比如速度环，状态机，转把采集等*/
-measure_time_t g_meas_MCTask;
-void Sched_MC_mTask(void) {
-	time_measure_start(&g_meas_MCTask);
-	u8 runMode = PMSM_FOC_CtrlMode();
-#if ANGLE_TEST
-	_debug_angle();
-#endif
-	PMSM_FOC_Calc_iDC();
-	if (motor.b_calibrate || (motor.mode == CTRL_MODE_OPEN)) {
-		return;
-	}
-	if ((runMode == CTRL_MODE_TRQ || runMode == CTRL_MODE_SPD) && !PMSM_FOC_MotorLocking()) {
+static bool mc_run_stall_process(u8 run_mode) {
+	if ((run_mode == CTRL_MODE_TRQ || run_mode == CTRL_MODE_SPD) && !PMSM_FOC_MotorLocking()) {
 		//堵转判断
 		if (motor.b_runStall) {
 			if (!mc_throttle_released()) {
-				return;
+				return true;
 			}
 			motor.b_runStall = false; //转把释放，清除堵转标志
 		}else if ((ABS(PMSM_FOC_GetSpeed()) < 1.0f) && (PMSM_FOC_Get()->out.s_RealIdq.q >= CONFIG_STALL_MAX_CURRENT)){
@@ -523,14 +501,32 @@ void Sched_MC_mTask(void) {
 				if (get_delta_ms(motor.runStall_time) >= CONFIG_STALL_MAX_TIME) {
 					motor.b_runStall = true;
 					motor.runStall_time = 0;
-					torque_speed_target(runMode, 0.0f);
-					return;
+					torque_speed_target(run_mode, 0.0f);
+					return true;
 				}
 			}
 		}else {
 			motor.runStall_time = 0;
 		}
 	}
+	return false;
+}
+
+/*FOC 的部分处理，比如速度环，状态机，转把采集等*/
+measure_time_t g_meas_MCTask;
+void Sched_MC_mTask(void) {
+	time_measure_start(&g_meas_MCTask);
+	u8 runMode = PMSM_FOC_CtrlMode();
+
+	/* 母线电流计算 */
+	PMSM_FOC_Calc_iDC();
+	if (motor.b_calibrate || (motor.mode == CTRL_MODE_OPEN)) {
+		return;
+	}
+	/* 堵转处理 */
+	if (mc_run_stall_process(runMode)) {
+		return;
+	}
 	if ((runMode != CTRL_MODE_OPEN) || (motor.mode != CTRL_MODE_OPEN)) {
 		if (motor.mode != CTRL_MODE_OPEN) {
 			u32 mask;

Applications/prot/can_foc_msg.c

@@ -79,10 +79,11 @@ void can_report_pid_value(u8 can, u8 id) {
 }
 
 void can_report_foc_status(u8 can) {
-	u8 data[8];
+	u8 data[12];
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Status));
 	mc_get_running_status(data+2);
 	u32 errMask = PMSM_FOC_GetCriticalError();
 	encode_u32(data + 3, errMask);
+	encode_u32(data + 7, shark_get_seconds());
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }