重写过温保护和软欠压限流

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

Applications/app/app.c

@@ -49,28 +49,74 @@ void app_start(void){
 	shark_task_run();
 }
 
+#if 0
+static void test_motor_temp(void) {
+	static s16 temp = 100;
+	static bool add = true;
+	sys_debug("mot: %d->%f\n", temp, motor_temp_high_limit_test(temp));
+	if (temp > 90 && temp != 300) {
+		temp += add?1:-1;
+		if (temp == 133) {
+			add = false;
+		}
+	}else {
+		temp = 300;
+	}
+}
+
+static void test_mos_temp(void) {
+	static s16 temp = 88;
+	static bool add = true;
+	sys_debug("mos: %d->%f\n", temp, mos_temp_high_limit_test(temp));
+	if (temp > 85 && temp != -40) {
+		temp += add?1:-1;
+		if (temp == 123) {
+			add = false;
+		}
+	}else {
+		temp = -40;
+	}
+
+}
+
+static void test_vbus_vol(void) {
+	static s16 vol = 65;
+	static bool add = false;
+	sys_debug("vbus: %d->%d\n", vol, vbus_voltage_low_limit_test(vol));
+	vol += add?1:-1;
+	if (vol < 40) {
+		add = true;
+	}else if (vol >= 65) {
+		add = false;
+	}
+}
+
+#endif
+
 static void app_print_log(void) {
 	//sys_debug("Slow: %d - %d, err:%d, %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time, g_meas_MCTask.exec_max_error_time, g_meas_MCTask.exec_time_error);
-	//sys_debug("Fast: %d - %d, err: %d-%d-%d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_hi_err, g_meas_foc.intval_low_err, g_meas_foc.exec_max_error_time);
-	//sys_debug("FOC time err %d %d %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error, g_meas_foc.exec_max_error_time, g_meas_foc.exec_time_error);
-	//sys_debug("acc vol %d, bid %d\n", get_acc_vol(), gpio_board_id());
+	sys_debug("Fast: %d - %d, err: %d-%d-%d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_hi_err, g_meas_foc.intval_low_err, g_meas_foc.exec_max_error_time);
+	sys_debug("FOC time err %d %d %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error, g_meas_foc.exec_max_error_time, g_meas_foc.exec_time_error);
+	sys_debug("acc vol %d, bid %d\n", get_acc_vol(), gpio_board_id());
 	//sys_debug("throttle %f\n", get_throttle_float());
 	sys_debug("ADC Vref %f, %f\n", get_adc_vref(), adc_5vref_compesion());
-	sys_debug("dead time 0x%x\n", get_deadtime());
+	//sys_debug("dead time 0x%x\n", get_deadtime());
 	//thro_torque_log();
 	//sys_debug("_>%f, %f, %f\n", ladrc_observer_get()->ld, ladrc_observer_get()->lq, ladrc_observer_get()->poles);
-	encoder_log();
+	//encoder_log();
 	//motor_debug();
-	sample_log();
+	//sample_log();
 	//throttle_log();
-	sys_debug("Trq: %f-%f-%f\n", motor.controller.ramp_input_torque.target, motor.controller.ramp_input_torque.interpolation, motor.controller.ramp_input_torque.step);
+	//sys_debug("Trq: %f-%f-%f\n", motor.controller.ramp_input_torque.target, motor.controller.ramp_input_torque.interpolation, motor.controller.ramp_input_torque.step);
 	sys_debug("FOC is %s, %d,%d\n", mot_contrl_is_start(mot_contrl())?"start":"stop", motor.foc_start_cnt, motor.foc_stop_cnt);
 	//F_debug();
 	//eCtrl_debug_log();
 	//sys_debug("enc err %d, %d\n", foc_observer_enc_errcount(), foc_observer_sensorless_stable());
 	//mc_err_code_log();
 	//sys_debug("=====\n");
-
+	//test_mos_temp();
+	//test_mos_temp();
+	//test_vbus_vol();
 }
 
 static u32 app_report_task(void *p) {

Applications/foc/core/controller.c

@@ -496,17 +496,25 @@ void mot_contrl_slow_task(mot_contrl_t *ctrl) {
 
 
 u8 mot_contrl_protect(mot_contrl_t *ctrl) {
+	float torque_lim;
 	u8 changed = FOC_LIM_NO_CHANGE;
-	float dc_lim = (float)vbus_under_vol_limit();
-	u16 mot_trq_lim = motor_temp_high_limit(type_phase);
-	s16 mos_trq_lim = mos_temp_high_limit();
-	float torque_lim = (float)min(mos_trq_lim, mot_trq_lim);
-
-	/*如果电机高温扭矩限制后，还是比当前的扭矩大，需要通过限制母线电流来实现降温*/
-	if ((torque_lim != HW_LIMIT_NONE) && (torque_lim >= mc_get_max_torque_now())) {
-		float mot_idc_lim = motor_temp_high_limit(type_idc);
-		dc_lim = min(dc_lim, mot_idc_lim);
+	float dc_lim = (float)vbus_voltage_low_limit();
+	float mot_lim_r = motor_temp_high_limit();
+	float mos_lim_r = mos_temp_high_limit();
+	float mos_torque = HW_LIMIT_NONE;
+	float mot_torque = HW_LIMIT_NONE;
+	float mot_idc = HW_LIMIT_NONE;
+	if (mot_lim_r < 1.0f) {
+		mot_torque = mot_lim_r * mc_gear_conf()->max_torque;
+		mot_idc    = mot_lim_r * mc_gear_conf()->max_idc;
+	}
+	if (mos_lim_r < 1.0f) {
+		mos_torque = mos_lim_r * mc_gear_conf()->max_torque;
 	}
+
+	dc_lim = min(dc_lim, mot_idc);
+	torque_lim = min(mos_torque, mot_torque);
+
 	if (ctrl->protlim.dc_curr != dc_lim || ctrl->protlim.torque != torque_lim) {
 		if ((dc_lim > ctrl->protlim.dc_curr) || (torque_lim > ctrl->protlim.torque)) {
 			changed = FOC_LIM_CHANGE_H;

Applications/foc/limit.c

@@ -6,250 +6,235 @@
 #include "foc/mc_error.h"
 #include "libs/logger.h"
 
-static limter_t motor_temp_lim[3];
-static limter_t mos_temp_lim[3];
-static limter_t vol_under_lim[1];
+static temp_lim_t motor_tl;
+static temp_lim_t mosfet_t;
+static vol_limt_t vbus;
 static bool _inited = false;
 static bool _can_recovery = true;
-static s16 mot_temp, mos_temp;
 
 static void limiter_init(void) {
-	
-	for (int i = 0; i < CONFIG_TEMP_PROT_NUM; i++) {
-		motor_temp_lim[i].enter_pointer = mc_conf()->p_mot[i].enter_pointer;
-		motor_temp_lim[i].exit_pointer = mc_conf()->p_mot[i].exit_pointer;
-		motor_temp_lim[i].limit_value = mc_conf()->p_mot[i].limit_value;
-		sys_debug("%d-%d-%d\n", motor_temp_lim[i].enter_pointer, motor_temp_lim[i].exit_pointer, motor_temp_lim[i].limit_value);
-		mos_temp_lim[i].enter_pointer = mc_conf()->p_mos[i].enter_pointer;
-		mos_temp_lim[i].exit_pointer = mc_conf()->p_mos[i].exit_pointer;
-		mos_temp_lim[i].limit_value = mc_conf()->p_mos[i].limit_value;
-		sys_debug("%d-%d-%d\n", mos_temp_lim[i].enter_pointer, mos_temp_lim[i].exit_pointer, mos_temp_lim[i].limit_value);
-	}
-	vol_under_lim[0].enter_pointer = mc_conf()->p_vol.enter_pointer;
-	vol_under_lim[0].exit_pointer = mc_conf()->p_vol.exit_pointer;
-	vol_under_lim[0].limit_value = mc_conf()->p_vol.limit_value;
-	//sys_debug("%d-%d-%d\n", vol_under_lim[0].enter_pointer, vol_under_lim[0].exit_pointer, vol_under_lim[0].limit_value);
-	mot_temp = sample_motor_temp();
-	mos_temp = sample_mos_temp();
+	if (_inited) {
+		return;
+	}
+	motor_tl.high = mc_conf()->p_mot[0].enter_pointer;
+	motor_tl.high_lim = mc_conf()->p_mot[0].limit_value;
+	motor_tl.mid = mc_conf()->p_mot[1].enter_pointer;
+	motor_tl.mid_lim = mc_conf()->p_mot[1].limit_value;
+	motor_tl.start = mc_conf()->p_mot[2].enter_pointer;
+	motor_tl.start_lim = mc_conf()->p_mot[2].limit_value;
+	motor_tl.curr_lim = 1.0f;
+	motor_tl.temp = sample_motor_temp();
+	sys_debug("mot: %d-%d, %d-%d, %d-%d\n", motor_tl.high, motor_tl.high_lim, motor_tl.mid, motor_tl.mid_lim, motor_tl.start, motor_tl.start_lim);
+	mosfet_t.high = mc_conf()->p_mos[0].enter_pointer;
+	mosfet_t.high_lim = mc_conf()->p_mos[0].limit_value;
+	mosfet_t.mid = mc_conf()->p_mos[1].enter_pointer;
+	mosfet_t.mid_lim = mc_conf()->p_mos[1].limit_value;
+	mosfet_t.start = mc_conf()->p_mos[2].enter_pointer;
+	mosfet_t.start_lim = mc_conf()->p_mos[2].limit_value;
+	mosfet_t.curr_lim = 1.0f;
+	mosfet_t.temp = sample_mos_temp();
+	sys_debug("mos: %d-%d, %d-%d, %d-%d\n", mosfet_t.high, mosfet_t.high_lim, mosfet_t.mid, mosfet_t.mid_lim, mosfet_t.start, mosfet_t.start_lim);
+	vbus.crit_low = mc_conf()->c.min_dc_vol;
+	vbus.lower = mc_conf()->p_vol.enter_pointer;
+	vbus.lower_lim = mc_conf()->p_vol.limit_value;
+	sys_debug("vbus: %d, %d-%d\n", vbus.crit_low, vbus.lower, vbus.lower_lim);
+	_inited = true;
 }
 
-static u16 _temp_limiter(s16 temp, limter_t *lim) {
-	if (!lim->is_limit) {
-		if (temp < lim->enter_pointer) {
-			lim->ticks = 0;
-			return HW_LIMIT_NONE;
-		}
-		if (lim->ticks == 0) {
-			lim->ticks = get_tick_ms();
-		}else if (get_delta_ms(lim->ticks) >= 500){
-			lim->is_limit = true;
-			lim->ticks = 0;
-			return lim->limit_value;
-		}
-		return HW_LIMIT_NONE;
-	}else {
-		if (temp >= lim->exit_pointer) {
-			lim->ticks = 0;
-			return lim->limit_value;
+static bool temp_limit_check(temp_lim_t *limit, s16 temp) {
+	if (!limit->is_limit) {
+		if (temp < limit->start) {
+			limit->lim_ticks = 0;
+		}else {
+			if (++limit->lim_ticks >= 1) {
+				limit->is_limit = true;
+				limit->lim_ticks = 0;
+			}
 		}
-		if (lim->ticks == 0) {
-			lim->ticks = get_tick_ms();
-		}else if (get_delta_ms(lim->ticks) >= 500) {
-			lim->is_limit = false;
-			lim->ticks = 0;
-			return HW_LIMIT_NONE;
+	}
+	if (limit->is_limit) {
+		if (temp < limit->start) {
+			if (++limit->lim_ticks >= 1) {
+				limit->is_limit = false;
+				limit->lim_ticks = 0;
+			}
+		}else {
+			limit->lim_ticks = 0;
 		}
-		return lim->limit_value;
 	}
+
+	return limit->is_limit;
 }
 
-static u16 _vol_limiter(s16 vol, limter_t *lim) {
-	if (!lim->is_limit) {
-		if (vol > lim->enter_pointer) {
-			lim->ticks = 0;
-			return HW_LIMIT_NONE;
-		}
-		if (lim->ticks == 0) {
-			lim->ticks = get_tick_ms();
-		}else if (get_delta_ms(lim->ticks) >= 5){
-			lim->is_limit = true;
-			lim->ticks = 0;
-			return lim->limit_value;
+#define TEMP_SENSOR_ERR_CNT 20
+
+float get_temp_limit_value(temp_lim_t *limit, s16 temp, s16 err_temp) {
+	if ((temp == err_temp) || ABS(temp - limit->temp) >= 20) {
+		limit->temp = temp;
+		if (limit->temp_sensor_err < TEMP_SENSOR_ERR_CNT) {
+			limit->temp_sensor_err++;
+			return limit->curr_lim;
+		}else {
+			return min(limit->curr_lim, 0.5f); //温度传感器异常，返回上次的限流
 		}
-		return HW_LIMIT_NONE;
 	}else {
-		if (vol <= lim->exit_pointer) {
-			lim->ticks = 0;
-			return lim->limit_value;
-		}
-		if (lim->ticks == 0) {
-			lim->ticks = get_tick_ms();
-		}else if (get_delta_ms(lim->ticks) >= 100) {
-			lim->is_limit = false;
-			lim->ticks = 0;
-			return HW_LIMIT_NONE;
+		limit->temp_sensor_err = 0;
+	}
+	limit->temp = temp;
+	if (!temp_limit_check(limit, temp)) {
+		limit->curr_lim = 1.0f;
+	}else {
+		if (temp < limit->start) {
+			return limit->curr_lim; //keep prev limit value
+		}else if (temp > limit->high) {
+			limit->curr_lim = 0;
+		}else {
+			if (temp >= limit->start && temp <= limit->mid) {
+				limit->curr_lim = f_map(temp, limit->start, limit->mid, limit->start_lim, limit->mid_lim)/100.0f;
+			}else {
+				limit->curr_lim = f_map(temp, limit->mid, limit->high, limit->mid_lim, limit->high_lim)/100.0f;
+			}
 		}
-		return lim->limit_value;
 	}
+
+	return limit->curr_lim;
 }
 
-static u16 _motor_limit(limit_type type) {
-	static int temp_sensor_err = 0;
-	static u16 lim_value  = HW_LIMIT_NONE;
+/* this maybe limit power or torque, based on the current power */
+float motor_temp_high_limit(void) {
+	limiter_init();
 	s16 temp = get_motor_temp_raw();
-	if ((temp == 300) || ABS(temp - mot_temp) >= 20) {
-		if (temp_sensor_err < 20) {
-			temp_sensor_err++;
-		}else {
-			if (mc_set_critical_error(FOC_CRIT_MOT_TEMP_Sensor)) {
-				mc_crit_err_add(FOC_CRIT_MOT_TEMP_Sensor, temp, mot_temp);
-			}
+	float value = get_temp_limit_value(&motor_tl, temp, 300);
+	if (motor_tl.temp_sensor_err == TEMP_SENSOR_ERR_CNT) {
+		if (mc_set_critical_error(FOC_CRIT_MOT_TEMP_Sensor)) {
+			mc_crit_err_add(FOC_CRIT_MOT_TEMP_Sensor, temp, motor_tl.temp);
 		}
-		return lim_value; //温度传感器异常，返回上次的限流
-	}else {
-		mot_temp = temp;
-		temp_sensor_err = 0;
-	}
-	for(int i = 0; i < ARRAY_SIZE(motor_temp_lim); i++) {
-		limter_t *lim = motor_temp_lim + i;
-		lim_value = _temp_limiter(temp, lim);
-		if (lim_value != HW_LIMIT_NONE) {
-			if (lim_value == 0) {
-				if (mc_set_critical_error(FOC_CRIT_MOTOR_TEMP_Lim)) {
-					mc_crit_err_add(FOC_CRIT_MOTOR_TEMP_Lim, temp, (s16)mot_contrl_get_speed(&motor.controller));
-				}
-			}else if (_can_recovery){
-				mc_clr_critical_error(FOC_CRIT_MOTOR_TEMP_Lim);
-			}
-			gear_t *gear = mc_gear_conf();
-
-			float prv_lim_value;
-			float next_lim_tmp;
-			if (i < (ARRAY_SIZE(motor_temp_lim)-1)) {
-				prv_lim_value = (float)motor_temp_lim[i + 1].limit_value;
-			}else {
-				prv_lim_value = 100.0f; //最低一级限流
-			}
-			if (i != 0) {
-				next_lim_tmp = (float)motor_temp_lim[i - 1].enter_pointer;
-			}else {
-				next_lim_tmp = (float)lim->enter_pointer + 10.0f; //最大一级限流
-			}
-			float delta_tmp = (next_lim_tmp - (float)lim->enter_pointer);
-			float delta_value = (prv_lim_value - (float)lim->limit_value);
-			float curr_value = prv_lim_value - (float)(temp - lim->enter_pointer)/delta_tmp * delta_value;
-			curr_value = fclamp(curr_value, 0, prv_lim_value);
-			float max_value = (float)gear->max_torque;
-			if (type == type_idc) {
-				max_value = (float)gear->max_idc;
-			}
-			lim_value = (u16)((max_value * curr_value) / 100.0f);
-			mc_set_motor_lim_level(i + 1);
-			return lim_value;
-		}else {
-			mc_set_motor_lim_level(0);
+	}
+	if (value == 0.0f) {
+		if (mc_set_critical_error(FOC_CRIT_MOTOR_TEMP_Lim)) {
+			mc_crit_err_add(FOC_CRIT_MOTOR_TEMP_Lim, temp, (s16)mot_contrl_get_speed(mot_contrl()));
 		}
+	}else if (_can_recovery){
+		mc_clr_critical_error(FOC_CRIT_MOTOR_TEMP_Lim);
 	}
-	
-	return HW_LIMIT_NONE;
+
+	return value;
 }
 
-static u16 _mos_limit(void) {
-	static int temp_sensor_err = 0;
-	static u16 lim_value  = HW_LIMIT_NONE;
-	s16 temp = get_mos_temp_raw();
-	if ((temp == -40) || ABS(temp - mos_temp) >= 20) {
-		if (temp_sensor_err < 20) {
-			temp_sensor_err++;
-		}else {
-			if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Sensor)) {
-				mc_crit_err_add(FOC_CRIT_MOS_TEMP_Sensor, temp, mos_temp);
-			}
+float motor_temp_high_limit_test(s16 temp) {
+	limiter_init();
+	float value = get_temp_limit_value(&motor_tl, temp, 300);
+	if (motor_tl.temp_sensor_err == TEMP_SENSOR_ERR_CNT) {
+		if (mc_set_critical_error(FOC_CRIT_MOT_TEMP_Sensor)) {
+			mc_crit_err_add(FOC_CRIT_MOT_TEMP_Sensor, temp, motor_tl.temp);
 		}
-		return lim_value; //温度传感器异常，返回上次的限流
-	}else {
-		mos_temp = temp;
-		temp_sensor_err = 0;
-	}
-	for(int i = 0; i < ARRAY_SIZE(mos_temp_lim); i++) {
-		limter_t *lim = mos_temp_lim + i;
-		lim_value = _temp_limiter(temp, lim);
-		if (lim_value != HW_LIMIT_NONE) {
-			if (lim_value == 0) {
-				if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Lim)) {
-					mc_crit_err_add(FOC_CRIT_MOS_TEMP_Lim, temp, (s16)mot_contrl_get_speed(&motor.controller));
-				}
-			}else if (_can_recovery){
-				mc_clr_critical_error(FOC_CRIT_MOS_TEMP_Lim);
-			}
-			gear_t *gear = mc_gear_conf();
-
-			float prv_lim_value;
-			float next_lim_tmp;
-			if (i < (ARRAY_SIZE(mos_temp_lim)-1)) {
-				prv_lim_value = (float)mos_temp_lim[i + 1].limit_value;
-			}else {
-				prv_lim_value = 100.0f; //最低一级限流
-			}
-			if (i != 0) {
-				next_lim_tmp = (float)mos_temp_lim[i - 1].enter_pointer;
-			}else {
-				next_lim_tmp = (float)lim->enter_pointer + 10.0f; //最大一级限流
-			}
-			float delta_tmp = (next_lim_tmp - (float)lim->enter_pointer);
-			float delta_value = (prv_lim_value - (float)lim->limit_value);
-			float curr_value = prv_lim_value - (float)(temp - lim->enter_pointer)/delta_tmp * delta_value;
-			curr_value = fclamp(curr_value, 0, prv_lim_value);
-			lim_value = (u16)(((float)gear->max_torque * curr_value) / 100.0f);
-			mc_set_mos_lim_level(i + 1);
-			return lim_value;
-		}else {
-			mc_set_mos_lim_level(0);
+	}
+	if (value == 0.0f) {
+		if (mc_set_critical_error(FOC_CRIT_MOTOR_TEMP_Lim)) {
+			mc_crit_err_add(FOC_CRIT_MOTOR_TEMP_Lim, temp, (s16)mot_contrl_get_speed(mot_contrl()));
 		}
+	}else if (_can_recovery){
+		mc_clr_critical_error(FOC_CRIT_MOTOR_TEMP_Lim);
 	}
-	
-	return HW_LIMIT_NONE;
+
+	return value;
 }
 
-/* this maybe limit power or torque, based on the current power */
-u16 motor_temp_high_limit(limit_type type) {
-	if (!_inited) {
-		_inited = true;
-		limiter_init();
+
+/* limit the max torque(max phase current) */
+float mos_temp_high_limit(void) {
+	limiter_init();
+	s16 temp = get_mos_temp_raw();
+	float value = get_temp_limit_value(&mosfet_t, temp, -40);
+	if (mosfet_t.temp_sensor_err == TEMP_SENSOR_ERR_CNT) {
+		if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Sensor)) {
+			mc_crit_err_add(FOC_CRIT_MOS_TEMP_Sensor, temp, mosfet_t.temp);
+		}
+	}
+	if (value == 0.0f) {
+		if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Lim)) {
+			mc_crit_err_add(FOC_CRIT_MOS_TEMP_Lim, temp, (s16)mot_contrl_get_speed(mot_contrl()));
+		}
+	}else if (_can_recovery){
+		mc_clr_critical_error(FOC_CRIT_MOS_TEMP_Lim);
 	}
-	return _motor_limit(type);
+
+	return value;
 }
 
-/* limit the max torque(max phase current) */
-u16 mos_temp_high_limit(void) {
-	if (!_inited) {
-		_inited = true;
-		limiter_init();
+float mos_temp_high_limit_test(s16 temp) {
+	limiter_init();
+	float value = get_temp_limit_value(&mosfet_t, temp, -40);
+	if (mosfet_t.temp_sensor_err == TEMP_SENSOR_ERR_CNT) {
+		if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Sensor)) {
+			mc_crit_err_add(FOC_CRIT_MOS_TEMP_Sensor, temp, mosfet_t.temp);
+		}
 	}
-	return _mos_limit();
+	if (value == 0.0f) {
+		if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Lim)) {
+			mc_crit_err_add(FOC_CRIT_MOS_TEMP_Lim, temp, (s16)mot_contrl_get_speed(mot_contrl()));
+		}
+	}else if (_can_recovery){
+		mc_clr_critical_error(FOC_CRIT_MOS_TEMP_Lim);
+	}
+
+	return value;
 }
 
+
 /* limit the DC bus current */
-u16 vbus_under_vol_limit(void) {
-	if (!_inited) {
-		_inited = true;
-		limiter_init();
-	}
+u16 vbus_voltage_low_limit(void) {
+	limiter_init();
 	s16 vol = (s16)sample_vbus_raw();
-	for(int i = 0; i < ARRAY_SIZE(vol_under_lim); i++) {
-		limter_t *lim = vol_under_lim + i;
-		u16 lim_value = _vol_limiter(vol, lim);
-		if (lim_value != HW_LIMIT_NONE) {
-			if (mc_set_critical_error(FOC_CRIT_UN_Vol_Err)) {
-				if (mot_contrl_get_speed(&motor.controller) > CONFIG_ZERO_SPEED_RPM) {
-					mc_crit_err_add(FOC_CRIT_UN_Vol_Err, vol, (s16)mot_contrl_get_speed(&motor.controller));
-				}
+	if (!vbus.is_limit && (vol <= vbus.lower)) {
+		vbus.is_limit = true;
+		if (mc_set_critical_error(FOC_CRIT_UN_Vol_Err)) {
+			if (mot_contrl_get_speed(mot_contrl()) > CONFIG_ZERO_SPEED_RPM) {
+				mc_crit_err_add(FOC_CRIT_UN_Vol_Err, vol, (s16)mot_contrl_get_speed(mot_contrl()));
 			}
-			return lim_value;
+		}
+	}else if (vbus.is_limit && (vol > vbus.lower)) {
+		vbus.is_limit = false;
+		if (_can_recovery) {
+			mc_clr_critical_error(FOC_CRIT_UN_Vol_Err);
 		}
 	}
-	if (_can_recovery) {
-		mc_clr_critical_error(FOC_CRIT_UN_Vol_Err);
+	if (vol > vbus.lower) {
+		return HW_LIMIT_NONE;
+	}else if (vol < vbus.crit_low) {
+		return 0.0f;
 	}
-	return HW_LIMIT_NONE;
+	return (u16)f_map(vol, vbus.crit_low, vbus.lower, 0.0f, vbus.lower_lim);
+}
+
+u16 vbus_voltage_low_limit_test(s16 vol) {
+	limiter_init();
+	if (!vbus.is_limit && (vol <= vbus.lower)) {
+		vbus.is_limit = true;
+		if (mc_set_critical_error(FOC_CRIT_UN_Vol_Err)) {
+			if (mot_contrl_get_speed(mot_contrl()) > CONFIG_ZERO_SPEED_RPM) {
+				mc_crit_err_add(FOC_CRIT_UN_Vol_Err, vol, (s16)mot_contrl_get_speed(mot_contrl()));
+			}
+		}
+	}else if (vbus.is_limit && (vol > vbus.lower)) {
+		vbus.is_limit = false;
+		if (_can_recovery) {
+			mc_clr_critical_error(FOC_CRIT_UN_Vol_Err);
+		}
+	}
+	if (vol > vbus.lower) {
+		return HW_LIMIT_NONE;
+	}else if (vol < vbus.crit_low) {
+		return 0.0f;
+	}
+	return (u16)f_map(vol, vbus.crit_low, vbus.lower, 0.0f, vbus.lower_lim);
+}
+
+
+bool motor_temp_limited(void) {
+	return motor_tl.is_limit;
+}
+
+bool mos_temp_limited(void) {
+	return mosfet_t.is_limit;
 }

Applications/foc/limit.h

@@ -3,21 +3,37 @@
 #include "os/os_types.h"
 
 #define HW_LIMIT_NONE ((u16)0x7FFF)
+
+typedef struct {
+	bool is_limit;
+	u32  lim_ticks;
+	s16  high; //exsit_low -> lower -> higher
+	u16  high_lim;
+	s16  mid;
+	u16  mid_lim;
+	s16  start;
+	u16  start_lim;
+	float curr_lim;
+	u16   temp_sensor_err;
+	s16   temp;
+}temp_lim_t;
+
 typedef struct {
-	u16 limit_value;
-	s16 enter_pointer;
-	s16 exit_pointer;
-	u32 ticks;
 	bool is_limit;
-}limter_t;
-typedef enum {
-	type_phase,
-	type_idc,
-}limit_type;
+	u32  ticks;
+	s16  crit_low;
+	s16  lower;
+	u16  lower_lim;
+}vol_limt_t;
 
-u16 motor_temp_high_limit(limit_type type);
-u16 mos_temp_high_limit(void);
-u16 vbus_under_vol_limit(void);
+float motor_temp_high_limit(void);
+float mos_temp_high_limit(void);
+u16   vbus_voltage_low_limit(void);
+bool motor_temp_limited(void);
+bool mos_temp_limited(void);
+float motor_temp_high_limit_test(s16 temp);
+float mos_temp_high_limit_test(s16 temp);
+u16 vbus_voltage_low_limit_test(s16 vol);
 
 #endif /* _Limit_H__ */