强制无感，低速后不报错

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

Applications/foc/core/foc_observer.c

@@ -20,7 +20,6 @@ void foc_observer_init(void) {
 	foc_obser.is_sensorless_stable = false;
 	foc_obser.sensorless_stable_cnt = 0;
 	foc_obser.sensorless_unstable_cnt = 0;
-	foc_obser.b_force_sensorless = false;
 #ifdef CONFIG_SMO_OBSERVER
 	smo_observer_init(CONFIG_SMO_PLL_BANDWITH, CONFIG_SMO_LFP_WC, CONFIG_SMO_GAIN_K, CONFIG_SMO_SIGMOID_MAX);
 	foc_obser.is_sensorless_enable = true;
@@ -47,7 +46,12 @@ float foc_observer_update(float uAlp, float uBeta, float iAlp, float iBeta){
 
 bool foc_observer_diagnostic(float enc_angle, float enc_vel) {
 	if (foc_obser.b_force_sensorless) {
-		return true;
+		if (foc_obser.is_sensorless_stable) {
+			if (foc_obser.sensorless_speed <= foc_observer_sensorless_working_speed()/2.0f) {
+				foc_obser.is_sensorless_stable = false;
+			}
+		}
+		return false;
 	}
 	if (enc_vel <= (foc_observer_sensorless_working_speed() - 100.0f)) {
 		if (!foc_obser.is_sensorless_running) {
@@ -137,6 +141,10 @@ bool foc_observer_sensorless_stable(void) {
 	return foc_obser.is_sensorless_stable;
 }
 
+bool foc_observer_is_force_sensorless(void) {
+	return foc_obser.b_force_sensorless;
+}
+
 float foc_observer_sensorless_working_speed(void) {
 #ifdef CONFIG_SMO_OBSERVER
 	return CONFIG_SMO_MIN_SPEED;

Applications/foc/core/foc_observer.h

@@ -35,6 +35,7 @@ u16 foc_observer_enc_errcount(void);
 float foc_observer_sensorless_diff(void);
 float foc_observer_sensorless_ration(void);
 bool foc_observer_sensorless_stable(void);
+bool foc_observer_is_force_sensorless(void);
 
 #endif /*_FOC_OBSERVER_H__*/
 

Applications/foc/motor/mot_params_ind.c

@@ -100,8 +100,9 @@ static void _rs_ind_timer_handler(shark_timer_t *t) {
 		float rs = vd / (PMSM_FOC_Get()->out.s_RealIdq.d + 0.0001f);
 		rs_est_value = LowPass_Filter(rs_est_value, rs, 0.2f);
 		if (rs_meas_time-- <= 0) {
-			mc_ind_motor_start(false);
 			mot_params_ind_stop();
+			delay_ms(1000);
+			mc_ind_motor_start(false);
 			finish = true;
 			b_rs_ested = true;
 			sys_debug("est rs = %f\n", rs_est_value);

Applications/foc/motor/motor.c

@@ -1106,10 +1106,8 @@ void ADC_IRQHandler(void) {
 
 #if (CONFIG_ENABLE_IAB_REC==1)
 	if (b_iab_rec && (iab_w_count < CONFIG_IAB_REC_COUNT)) {
-		float iabc[3];
-		phase_current_get(iabc);
-		ia[iab_w_count] = (s16)iabc[0];
-		ib[iab_w_count] = (s16)iabc[1];
+		ia[iab_w_count] = (s16)PMSM_FOC_Get()->in.s_iABC[0];
+		ib[iab_w_count] = (s16)PMSM_FOC_Get()->in.s_iABC[1];
 		iab_w_count ++;
 	}
 #endif
@@ -1124,19 +1122,23 @@ void ADC_IRQHandler(void) {
 		if (PMSM_FOC_Is_Start()) {
 			pwm_disable_channel();
 			/* 记录错误 */
-			mc_save_err_runtime();
+			if (!foc_observer_is_force_sensorless()) {
+				mc_save_err_runtime();
+			}
 			PMSM_FOC_Stop();
 			g_meas_foc.first = true;
-			if (mc_set_critical_error(FOC_CRIT_Angle_Err)) {
-				mc_crit_err_add_s16(FOC_CRIT_Angle_Err, (s16)motor_encoder_get_speed());
-			}
-			if (motor_encoder_may_error() == ENCODER_PWM_ERR) {
-				if (mc_set_critical_error(FOC_CRIT_Encoder_Err)) {
-					mc_crit_err_add(FOC_CRIT_Encoder_Err, (s16)enc_pwm_err_ms, enc_delta_err2);
+			if (!foc_observer_is_force_sensorless()) {
+				if (mc_set_critical_error(FOC_CRIT_Angle_Err)) {
+					mc_crit_err_add_s16(FOC_CRIT_Angle_Err, (s16)motor_encoder_get_speed());
 				}
-			}else if (motor_encoder_may_error() == ENCODER_AB_ERR) {
-				if (mc_set_critical_error(FOC_CRIT_ENC_AB_Err)) {
-					mc_crit_err_add(FOC_CRIT_ENC_AB_Err, enc_delta_err1, enc_delta_err2);
+				if (motor_encoder_may_error() == ENCODER_PWM_ERR) {
+					if (mc_set_critical_error(FOC_CRIT_Encoder_Err)) {
+						mc_crit_err_add(FOC_CRIT_Encoder_Err, (s16)enc_pwm_err_ms, enc_delta_err2);
+					}
+				}else if (motor_encoder_may_error() == ENCODER_AB_ERR) {
+					if (mc_set_critical_error(FOC_CRIT_ENC_AB_Err)) {
+						mc_crit_err_add(FOC_CRIT_ENC_AB_Err, enc_delta_err1, enc_delta_err2);
+					}
 				}
 			}
 		}

Applications/foc/motor/motor_param.c

@@ -208,7 +208,23 @@ void motor_mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out) {
 }
 #else
 void motor_mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out) {
-	dq_out->d = 0;
+	float d = 0;
+#if 0
+	if (rpm < 50) {
+		d = 0;
+	}else if (rpm < 100) {
+		d = -2;
+	}else if (rpm < 500) {
+		d = -6;
+	}else if (rpm < 1000) {
+		d = -12;
+	}else if (rpm < 1600) {
+		d = -16;
+	}else {
+		d = -20;
+	}
+#endif
+	dq_out->d = d;
 	dq_out->q = torque;
 }