转把错误处理

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

Applications/foc/commands.h

@@ -93,6 +93,7 @@ typedef struct {
 	u16    etcs    :2; //1:disable, 2:enable, 3:ignore
 	u16    epm     :2; //1:disable, 2:enable
 	u16    epm_dir :2; //0:stop, 1:back, 2 forword
+	u16    thro_dec:1; //0:do noting, 1:do throttle auto detect
 	//32-47 bit
 	u16    cruise  :2; //1:disable, 2:enable
 	u16    inc     :2; //1:dec, 2:inc
@@ -103,7 +104,26 @@ typedef struct {
 
 typedef struct {
 	//0-15 bit
-	u16 status;
+	union {
+		u16 status;
+		struct _x{
+			u16 start     :1;
+			u16 gear	  :2;
+			u16 autohold  :1;
+			u16 hw_brk    :1;
+			u16 cruise	  :1;
+			u16 epm_mode  :1;
+			u16 lock_mot  :1;
+			u16 e_brk	  :1;
+			u16 crit_err  :1;
+			u16 fan_run   :1;
+			u16 stall     :1;
+			u16 idc_lim   :1;
+			u16 trq_lim   :1;
+			u16 etcs_run  :1;
+			u16 thro_err  :1;
+		}s;
+	}ustat;
 	//16-31 bit
 	u16 vdc    :9;
 	u16 mot_ll :2; //motor temp limiter level

Applications/foc/foc_config.h

@@ -20,9 +20,9 @@
 #define CONFIG_DEFAULT_FW_ENABLE 1
 /* 转把 */
 #define CONFIG_THROTTLE_MIN_VALUE 0.4F   /* 转把最小电压，低于这个值，不给启动 */
-#define CONFIG_THROTTLE_START_VALUE 1.2f /* 转把启动电压 */
-#define CONFIG_THROTTLE_END_VALUE 3.8f /* 转把停止电压 */
-#define CONFIG_THROTTLE_MAX_VALUE 4.8F /* 转把最大电压，大于这个值，不给启动 */
+#define CONFIG_THROTTLE_START_VALUE 1.0f /* 转把启动电压 */
+#define CONFIG_THROTTLE_END_VALUE 4.0f /* 转把停止电压 */
+#define CONFIG_THROTTLE_MAX_VALUE 4.6F /* 转把最大电压，大于这个值，不给启动 */
 
 #define CONFIG_MIN_CRUISE_RPM 	  1000   /* 能启动定速巡航的最小速度 */
 #define CONFIG_CRUISE_EXIT_RPM    700    /* 自动推出定速巡航的最小速度*/

Applications/foc/motor/motor.c

@@ -696,11 +696,11 @@ void mc_get_running_status(u8 *data) {
 
 u16 mc_get_running_status2(void) {
 	u16 data = 0;
-	data = motor.n_gear;
-	data |= (PMSM_FOC_AutoHoldding()?1:0) << 2;
-	data |= (motor.b_break?1:0) << 3;
-	data |= (motor.b_cruise?1:0) << 4;
-	data |= (motor.b_start?1:0) << 5;
+	data = motor.b_start?1:0;
+	data |= (motor.n_gear & 0x7) << 1;
+	data |= (PMSM_FOC_AutoHoldding()?1:0) << 3;
+	data |= (motor.b_break?1:0) << 4;
+	data |= (motor.b_cruise?1:0) << 5;
 	data |= (mc_is_epm()?1:0) << 6;
 	data |= (motor.b_lock_motor) << 7; //motor locked
 	data |= (eCtrl_is_eBrk_Running()?1:0) << 8; //能量回收运行标志
@@ -710,6 +710,7 @@ u16 mc_get_running_status2(void) {
 	data |= (PMSM_FOC_iDC_is_Limited()?1:0) << 12; //是否欠压限制母线电流
 	data |= (PMSM_FOC_Torque_is_Limited()?1:0) << 13; //是否高温限扭矩
 	data |= (etcs_is_running()?1:0) << 14; //电子tcs是否正在工作
+	data |= (throttle_not_released_err()?1:0) << 15;
 	return data;
 }
 

Applications/foc/motor/throttle.c

@@ -12,6 +12,7 @@ static float _start_v, _end_v;
 static bool _auto_detect_sv = true;
 static int  _auto_detect_sv_cnt = 0;
 static float _auto_detect_sv_totle = 0;
+static int _detect_release_cnt = 0;
 #define CONFIG_SAFE_INV_V   0.06f
 
 void throttle_init(void) {
@@ -95,6 +96,22 @@ bool throttle_is_released(void) {
 #endif
 }
 
+bool throttle_not_released_err(void)
+{
+	return ((err_mask & THRO_NOT_RELEASED) != 0);
+}
+
+void throttle_force_detect(void) {
+	u32 mask = cpu_enter_critical();
+	throttle_init();
+	_auto_detect_sv = true;
+	_auto_detect_sv_cnt = 0;
+	_auto_detect_sv_totle = 0;
+	_detect_release_cnt = 0;
+	err_mask = 0; //clear err mask
+	cpu_exit_critical(mask);
+}
+
 void throttle_detect(bool ready) {
 	float thr_5v = get_thro_5v_float();
 	float thr_sig = get_throttle_float();
@@ -120,9 +137,16 @@ void throttle_detect(bool ready) {
 		}
 	}
 #endif
+	if (!ready && throttle_get_signal() > _start_v) {
+		if (_detect_release_cnt < 500) {
+			_detect_release_cnt ++;
+		}else {
+			err_mask |= THRO_NOT_RELEASED;
+		}
+	}
 	if (ready) {
 		_auto_detect_sv = false;
-	}else if (!throttle_is_all_error() && !ready && _auto_detect_sv) {
+	}else if (!throttle_is_all_error() && !throttle_not_released_err() && !ready && _auto_detect_sv) {
 		float v = throttle_get_signal();
 		if (v < _start_v) {
 			_auto_detect_sv_totle += v;

Applications/foc/motor/throttle.h

@@ -4,10 +4,12 @@
 #define	THRO2_5V_ERR_BIT 0x02
 #define	THRO1_SIG_ERR_BIT 0x04
 #define	THRO2_SIG_ERR_BIT 0x08
+#define THRO_NOT_RELEASED 0x10
 
 void throttle_init(void);
 bool throttle_is_released(void);
 bool throttle_is_all_error(void);
+bool throttle_not_released_err(void);
 u8 throttle_get_errors(void);
 float throttle_get_signal(void);
 void throttle_detect(bool ready);