Applications/app/app.c

@@ -21,10 +21,6 @@
 #include "foc/mc_config.h"
 #include "foc/motor/throttle.h"
 
-#ifdef CONFIG_DQ_STEP_RESPONSE
-extern float target_d;
-extern float target_q;
-#endif
 static u32 app_low_task(void *args);
 static u32 app_report_task(void *args);
 static u32 app_plot_task(void *args);
@@ -33,7 +29,7 @@ extern void PMSM_FOC_LogDebug(void);
 extern void mc_err_code_log(void);
 extern void encoder_log(void);
 extern void sample_log(void);
-extern void thro_torque_log(void);
+extern void throttle_log(void);
 extern bool can_is_connect_pc(void);
 extern measure_time_t g_meas_hall;
 extern measure_time_t g_meas_foc;
@@ -54,9 +50,9 @@ void app_start(void){
 }
 
 static void app_print_log(void) {
-	//sys_debug("Slow: %d - %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time);
+	//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\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error);
+	//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());
@@ -65,8 +61,10 @@ static void app_print_log(void) {
 	//sys_debug("_>%f, %f, %f\n", ladrc_observer_get()->ld, ladrc_observer_get()->lq, ladrc_observer_get()->poles);
 	encoder_log();
 	//motor_debug();
-	sample_log();
-	//PMSM_FOC_LogDebug();
+	//sample_log();
+	//throttle_log();
+	sys_debug("Trq: %f-%f-%f\n", motor.controller.input_torque.target, motor.controller.input_torque.interpolation, motor.controller.input_torque.step);
+	sys_debug("contr %d\n", motor.controller.b_start);
 	//F_debug();
 	//eCtrl_debug_log();
 	//sys_debug("enc err %d, %d\n", foc_observer_enc_errcount(), foc_observer_sensorless_stable());
@@ -84,11 +82,14 @@ static u32 app_report_task(void *p) {
 		return 200;
 	}
 	can_report_power(0x45);
-	can_report_dq_current(0x45);
 	can_report_foc_status(0x45);
-	can_report_phase_voltage(0x45);
-	can_report_mpta_values(0x45);
-	can_report_phase_current(0x45);
+	can_report_dq_voltage(0X45);
+	if (motor.controller.b_mtpa_calibrate) {
+		can_report_mpta_values(0x45);
+	}else {
+		can_report_dq_current(0x45);
+	}
+
 	if (mot_params_rs_ested()) {
 		can_report_mot_params_ested(mot_params_get_est_rs(), R_TYPE);
 	}

Applications/bsp/at32/board_at_mc100_v1.h

@@ -29,7 +29,7 @@
 
 #define CONFIG_CURR_LP_CEOF (CONFIG_CURR_LP_WC*2*3.14F/(float)FOC_PWM_FS)
 
-#define CONFIG_96V_MODE_VOL (60.0F)
+#define CONFIG_HIGH_VOL_MODE_MIN_VOL (60.0F)
 
 #define CONFIG_SMO_OBSERVER 1
 
@@ -343,7 +343,7 @@
 #define CONFIG_MOT_TYPE MOTOR_BLUESHARK_A1
 
 #if (CONFIG_MOT_TYPE==MOTOR_BLUESHARK_A1)
-#define CONFIG_FORCE_96V_MODE 1
+#define CONFIG_FORCE_HIGH_VOL_MODE 1
 #endif
 
 //#define CONFIG_DQ_STEP_RESPONSE

Applications/bsp/at32/board_at_mc105_v3.h

@@ -31,17 +31,17 @@
 
 #define CONFIG_CURR_LP_CEOF (CONFIG_CURR_LP_WC*2*3.14F/(float)FOC_PWM_FS)
 
-#define CONFIG_96V_MODE_VOL (60.0F)
+#define CONFIG_HIGH_VOL_MODE_MIN_VOL (60.0F)
 
 #define CONFIG_LADRC_OBSERVER
-//#define CONFIG_FORCE_96V_MODE
+//#define CONFIG_FORCE_HIGH_VOL_MODE
 #ifdef CONFIG_SENSORLESS_TOW_SAMPLES
 #define CONFIG_SENSORLESS_TS (FOC_CTRL_US/2.0f)
 #else
 #define CONFIG_SENSORLESS_TS FOC_CTRL_US
 #endif
 
-//#define CONFIG_FORCE_96V_MODE 1
+//#define CONFIG_FORCE_HIGH_VOL_MODE 1
 
 #define SCHED_TIMER TMR5
 #define SCHED_TIMER_RCU CRM_TMR5_PERIPH_CLOCK

Applications/bsp/gd32/board_mc100_v1.h

@@ -20,7 +20,7 @@
 #define CONFIG_STALL_MAX_CURRENT 100.0f //最大堵转相电流电流
 #define CONFIG_STALL_MAX_TIME    3000   //ms, 超过最大堵转电流持续时间，判断堵转
 
-#define CONFIG_96V_MODE_VOL (60.0F)
+#define CONFIG_HIGH_VOL_MODE_MIN_VOL (60.0F)
 
 #define CONFIG_SMO_OBSERVER 1
 //#define CONFIG_SPEED_LADRC  1

Applications/bsp/gd32/board_mc105_v3.h

@@ -15,18 +15,19 @@
 #define CONFIG_MAX_TORQUE       CONFIG_HW_MAX_PHASE_CURR
 #define CONFIG_MAX_LOCK_TORQUE  100
 #define CONFIG_MAX_ACTIVE_EMF   5000.0F
-//#define CONFIG_BEEP 
 #define CONFIG_STALL_MAX_CURRENT 100.0f //最大堵转相电流电流
 #define CONFIG_STALL_MAX_TIME    3000   //ms, 超过最大堵转电流持续时间，判断堵转
 
-#define CONFIG_96V_MODE_VOL (60.0F)
-
 #define CONFIG_DAUL_THROTTLE 1  //双转把
-//#define CONFIG_SENSORLESS_TOW_SAMPLES
 //#define CONFIG_SMO_OBSERVER 
 #define CONFIG_LADRC_OBSERVER
 //#define CONFIG_SPEED_LADRC  
-#define CONFIG_FORCE_96V_MODE
+#define CONFIG_FORCE_HIGH_VOL_MODE
+
+#ifndef CONFIG_FORCE_HIGH_VOL_MODE
+#define CONFIG_HIGH_VOL_MODE_MIN_VOL (60.0F)
+#endif
+
 #ifdef CONFIG_SENSORLESS_TOW_SAMPLES
 #define CONFIG_SENSORLESS_TS (FOC_CTRL_US/2.0f)
 #else

Applications/bsp/gd32/board_yuanqu.h

@@ -30,7 +30,7 @@
 
 #define CONFIG_CURR_LP_PARAM (CONFIG_CURR_LP_CUT_FREQ*2*3.14F/(float)FOC_PWM_FS)
 
-#define CONFIG_96V_MODE_VOL (55.0F)
+#define CONFIG_HIGH_VOL_MODE_MIN_VOL (55.0F)
 
 #define SCHED_TIMER TIMER5
 #define SCHED_TIMER_RCU RCU_TIMER5

Applications/bsp/gd32/bsp.h

@@ -35,7 +35,11 @@
 #elif defined (MC105_HW_V3)
 #include "bsp/gd32/board_mc105_v3.h"
 #define CONFIG_BOARD_MCXXX
+#ifdef CONFIG_SERVO_MOTOR
+#define CONFIG_BOARD_NAME "MC105_SERVO"
+#else
 #define CONFIG_BOARD_NAME "MC105"
+#endif
 #define CONFIG_MC105_HW_VERSION 3
 #endif
 #endif /* __BSP_GD32_H__ */

Applications/bsp/gd32/can.c

@@ -132,7 +132,8 @@ static void shark_can0_config(void)
 
 static int shark_send_can0_data(can_trasnmit_message_struct *P_message){
 	can_tx_poll();
-	if (circle_put_data(&g_tx_circle, (u8 *)P_message, sizeof(can_trasnmit_message_struct))){
+	int len = sizeof(can_trasnmit_message_struct);
+	if (circle_put_data(&g_tx_circle, (u8 *)P_message, len) == len){
 		return CAN_SEND_OK;
 	}
 	return CAN_SEND_ERROR;
@@ -218,8 +219,8 @@ void shark_can0_deinit(void){
 }
 
 void shark_can0_init(void){
-	circle_buffer_init(&g_tx_circle, _g_tx_buffer, sizeof(_g_tx_buffer));
-	circle_buffer_init(&g_rx_circle, _g_rx_buffer, sizeof(_g_rx_buffer));
+	circle_buffer_init(&g_tx_circle, _g_tx_buffer, sizeof(_g_tx_buffer) - 1);
+	circle_buffer_init(&g_rx_circle, _g_rx_buffer, sizeof(_g_rx_buffer) - 1);
 
 	shark_task_create(_can_poll_task, NULL);
 	shark_can0_txrx_pin_config();

Applications/bsp/n32/board_n32_mc105_v3.h

@@ -24,11 +24,11 @@
 
 #define CONFIG_CURR_LP_CEOF (CONFIG_CURR_LP_WC*2*3.14F/(float)FOC_PWM_FS)
 
-#define CONFIG_96V_MODE_VOL (60.0F)
+#define CONFIG_HIGH_VOL_MODE_MIN_VOL (60.0F)
 
 #define CONFIG_SMO_OBSERVER 1
 //#define CONFIG_SPEED_LADRC  1
-//#define CONFIG_FORCE_96V_MODE 1
+//#define CONFIG_FORCE_HIGH_VOL_MODE 1
 
 #define SCHED_TIMER TIM5
 #define SCHED_TIMER_RCU RCU_TIMER5

Applications/config.h

@@ -16,7 +16,11 @@
 #define  CAN_NODE_ADDR_CCU  		0x43
 #define  CAN_NODE_ADDR_PC  			0x45
 #define  CAN_NODE_ADDR_CCU_AUX      0x46
+#ifdef CONFIG_SERVO_MOTOR
+#define  CAN_NODE_ADDR_MCU  		0x4E
+#else
 #define  CAN_NODE_ADDR_MCU  		0x4D
+#endif
 #define  CAN_NODE_ADDR_DCU  		0x50	/* MCU of the display panel */
 #define  CAN_NODE_ADDR_BLE  		0x51	/*radar*/
 #define  CAN_NODE_ADDR_RCU  		0x52	/*radar*/

Applications/foc/commands.c

@@ -405,6 +405,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			if (command->len >= 2) {
 				bool use = decode_u8(command->data)==0?false:true;
 				u8 r = decode_u8((u8 *)command->data + 1);
+				sys_debug("set thro %d, r: %d\n", use, r);
 				mc_set_throttle_r(use, r);
 			}
 			break;
@@ -442,10 +443,11 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		case Foc_Start_DQ_Calibrate:
 		{
 			u8 start = decode_u8((u8 *)command->data);
-			if (start == Foc_Start) {
+			if (start == 1) {
 				sys_debug("start mpta cali\n");
-				mc_set_foc_mode(CTRL_MODE_CURRENT);
-				mot_contrl_mtpa_calibrate(&motor.controller, true);
+				if (mc_set_foc_mode(CTRL_MODE_CURRENT)) {
+					mot_contrl_mtpa_calibrate(&motor.controller, true);
+				}
 			}else {
 				mot_contrl_mtpa_calibrate(&motor.controller, false);
 				mc_set_foc_mode(CTRL_MODE_TRQ);
@@ -683,6 +685,15 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			}
 			break;
 		}
+		case Foc_Set_Force_Torque:
+		{
+			if (command->len == 2) {
+				s16 tgt_torque = decode_s16((u8 *)command->data);
+				mc_set_force_torque(tgt_torque);
+				sys_debug("torque:%d-%d\n", tgt_torque, motor.s_force_torque);
+			}
+			break;
+		}
 		default:
 		{
 			erroCode = FOC_Unknow_Cmd;

Applications/foc/commands.h

@@ -65,6 +65,7 @@ typedef enum {
 	Foc_Set_LogLevel,
 	Foc_MotPara_Ind,
 	Foc_MotPara_Report,
+	Foc_Set_Force_Torque,
 	Foc_Cmd_Max = 0xDF
 }foc_cmd_t;
 #define CMD_2_CAN_KEY(cmd) ((u16)(((u16)cmd) | (CAN_MY_ADDRESS<<8)))

Applications/foc/core/PI_Controller.h

@@ -9,9 +9,9 @@ typedef struct {
 	float  kd;
 	float  max;
 	float  min;
-	float  Ui;
+	float  ui;
 	float  sat;
-	float  DT;
+	float  ts;
 	bool   is_sat;
 }PI_Controller;
 
@@ -26,7 +26,7 @@ static __INLINE void PI_Controller_max(PI_Controller *pi, float max, float min)
 	pi->min = min;
 }
 static __INLINE void PI_Controller_Reset(PI_Controller *pi, float init) {
-	pi->Ui = (init);
+	pi->ui = (init);
 	pi->sat = 0.0f;
 	pi->is_sat = false;
 }
@@ -34,9 +34,9 @@ static __INLINE void PI_Controller_Reset(PI_Controller *pi, float init) {
 static __INLINE float PI_Controller_Run(PI_Controller *pi, float err) {
 	float kp_err = (err) * pi->kp;
 	float ki_err = (err) * pi->ki;
-	float integral = ki_err * pi->DT;
-	pi->Ui = fclamp(pi->Ui + integral, pi->min, pi->max);
-	float out = pi->Ui + kp_err;
+	float integral = ki_err * pi->ts;
+	pi->ui = fclamp(pi->ui + integral, pi->min, pi->max);
+	float out = pi->ui + kp_err;
 	float sat_out = fclamp(out, pi->min, pi->max);
 	if (out != sat_out) {
 		pi->is_sat = true;
@@ -49,16 +49,16 @@ static __INLINE float PI_Controller_Run(PI_Controller *pi, float err) {
 static __INLINE float PI_Controller_RunVel(PI_Controller *pi, float err) {
 	float kp_err = (err) * pi->kp;
 	float ki_err = (kp_err) * pi->ki;
-	float integral = ki_err * pi->DT;
+	float integral = ki_err * pi->ts;
 
-	pi->Ui = pi->Ui + integral;
-	float out = pi->Ui + kp_err;
+	pi->ui = pi->ui + integral;
+	float out = pi->ui + kp_err;
 	if (out > pi->max) {
 		out = pi->max;
-		pi->Ui = out - kp_err;
+		pi->ui = out - kp_err;
 	}else if (out < pi->min) {
 		out = pi->min;
-		pi->Ui = out - kp_err;
+		pi->ui = out - kp_err;
 	}
 	return out;
 }
@@ -68,18 +68,18 @@ static __INLINE float PI_Controller_RunVel(PI_Controller *pi, float err) {
 static __INLINE float PI_Controller_Current(PI_Controller *pi, float err, float ff) {
 	float kp_err = (err) * pi->kp;
 	float ki_err = (kp_err) * pi->ki;
-	float integral = ki_err * pi->DT;
+	float integral = ki_err * pi->ts;
 
-	pi->Ui = pi->Ui + integral;
-	float out = pi->Ui + kp_err + ff;
+	pi->ui = pi->ui + integral;
+	float out = pi->ui + kp_err + ff;
 	if (out > pi->max) {
 		out = pi->max;
 		pi->is_sat = true;
-		pi->Ui = out - (kp_err + ff);
+		pi->ui = out - (kp_err + ff);
 	}else if (out < pi->min) {
 		out = pi->min;
 		pi->is_sat = true;
-		pi->Ui = out - (kp_err + ff);
+		pi->ui = out - (kp_err + ff);
 	}else {
 		pi->is_sat = false;
 	}
@@ -90,18 +90,18 @@ static __INLINE float PI_Controller_Current(PI_Controller *pi, float err, float
 static __INLINE float PI_Controller_RunSerial(PI_Controller *pi, float err) {
 	float kp_err = (err) * pi->kp;
 	float ki_err = (kp_err) * pi->ki;
-	float integral = ki_err * pi->DT;
+	float integral = ki_err * pi->ts;
 
-	pi->Ui = pi->Ui + integral;
-	float out = pi->Ui + kp_err;
+	pi->ui = pi->ui + integral;
+	float out = pi->ui + kp_err;
 	if (out > pi->max) {
 		out = pi->max;
 		pi->is_sat = true;
-		pi->Ui = out - kp_err;
+		pi->ui = out - kp_err;
 	}else if (out < pi->min) {
 		out = pi->min;
 		pi->is_sat = true;
-		pi->Ui = out - kp_err;
+		pi->ui = out - kp_err;
 	}else {
 		pi->is_sat = false;
 	}
@@ -118,11 +118,11 @@ typedef struct {
 	float observer;
 	float kp;
 	float ki;
-	float Ui;
+	float ui;
 	float out;
 	s32   max_wp;
 	bool  ob_wp;
-	float DT;
+	float ts;
 }PLL_t;
 
 static __INLINE void PLL_Reset(PLL_t *pll, float sample) {
@@ -142,8 +142,8 @@ static __INLINE float PLL_run(PLL_t *pll, float sample, float comp) {
 		observer = -comp - pll->observer;
 	}
 	float delta = sample - observer;
-	pll->observer = observer + (pll->out + pll->kp * delta) * pll->DT;
-	pll->out += pll->ki * delta * pll->DT;
+	pll->observer = observer + (pll->out + pll->kp * delta) * pll->ts;
+	pll->out += pll->ki * delta * pll->ts;
 	return pll->out;
 }
 

Applications/foc/core/PMSM_FOC_Core.c

@@ -131,22 +131,22 @@ static void PMSM_FOC_Conf_PID(void) {
 	gFoc_Ctrl.pi_id.kp = mc_conf()->c.pid[PID_ID_ID].kp;
 	gFoc_Ctrl.pi_id.ki = mc_conf()->c.pid[PID_ID_ID].ki;
 	gFoc_Ctrl.pi_id.kd = mc_conf()->c.pid[PID_ID_ID].kd;
-	gFoc_Ctrl.pi_id.DT = (1.0f/(float)CONFIG_IDQ_CTRL_TS);
+	gFoc_Ctrl.pi_id.ts = (1.0f/(float)CONFIG_IDQ_CTRL_TS);
 
 	gFoc_Ctrl.pi_iq.kp = mc_conf()->c.pid[PID_IQ_ID].kp;
 	gFoc_Ctrl.pi_iq.ki = mc_conf()->c.pid[PID_IQ_ID].ki;
 	gFoc_Ctrl.pi_iq.kd = mc_conf()->c.pid[PID_IQ_ID].kd;
-	gFoc_Ctrl.pi_iq.DT = (1.0f/(float)CONFIG_IDQ_CTRL_TS);
+	gFoc_Ctrl.pi_iq.ts = (1.0f/(float)CONFIG_IDQ_CTRL_TS);
 
 	gFoc_Ctrl.pi_power.kp = mc_conf()->c.pid[PID_IDCLim_ID].kp;
 	gFoc_Ctrl.pi_power.ki = mc_conf()->c.pid[PID_IDCLim_ID].ki;
 	gFoc_Ctrl.pi_power.kd = mc_conf()->c.pid[PID_IDCLim_ID].kd;
-	gFoc_Ctrl.pi_power.DT = slow_ctrl_ts;
+	gFoc_Ctrl.pi_power.ts = slow_ctrl_ts;
 
 	gFoc_Ctrl.pi_lock.kp = mc_conf()->c.pid[PID_AutoHold_ID].kp;
 	gFoc_Ctrl.pi_lock.ki = mc_conf()->c.pid[PID_AutoHold_ID].ki;
 	gFoc_Ctrl.pi_lock.kd = mc_conf()->c.pid[PID_AutoHold_ID].kd;
-	gFoc_Ctrl.pi_lock.DT = slow_ctrl_ts;
+	gFoc_Ctrl.pi_lock.ts = slow_ctrl_ts;
 
 #ifdef CONFIG_SPEED_LADRC
 	ladrc_init(&gFoc_Ctrl.vel_lim_adrc, slow_ctrl_ts, nv_get_foc_params()->f_adrc_vel_lim_Wo, nv_get_foc_params()->f_adrc_vel_lim_Wcv, nv_get_foc_params()->f_adrc_vel_lim_B0);
@@ -155,11 +155,11 @@ static void PMSM_FOC_Conf_PID(void) {
 	gFoc_Ctrl.pi_vel_lim.kp = mc_conf()->c.pid[PID_VelLim_ID].kp;
 	gFoc_Ctrl.pi_vel_lim.ki = mc_conf()->c.pid[PID_VelLim_ID].ki;
 	gFoc_Ctrl.pi_vel_lim.kd = mc_conf()->c.pid[PID_VelLim_ID].kd;
-	gFoc_Ctrl.pi_vel_lim.DT = slow_ctrl_ts;
+	gFoc_Ctrl.pi_vel_lim.ts = slow_ctrl_ts;
 	gFoc_Ctrl.pi_vel.kp = mc_conf()->c.pid[PID_Vel_ID].kp;
 	gFoc_Ctrl.pi_vel.ki = mc_conf()->c.pid[PID_Vel_ID].ki;
 	gFoc_Ctrl.pi_vel.kd = mc_conf()->c.pid[PID_Vel_ID].kd;
-	gFoc_Ctrl.pi_vel.DT = slow_ctrl_ts;
+	gFoc_Ctrl.pi_vel.ts = slow_ctrl_ts;
 #endif
 }
 

Applications/foc/core/controller.c

@@ -29,8 +29,9 @@ void mot_contrl_init(mot_contrl_t *ctrl) {
 	ctrl->hwlim.fw_id = mc_conf()->m.max_fw_id;  //电池能支持的最大弱磁电流
 	ctrl->protlim.dc_curr = HW_LIMIT_NONE;
 	ctrl->protlim.torque = HW_LIMIT_NONE;
-	ctrl->torque_acc_time = 500;
-	ctrl->torque_dec_time = 500;//will be set after start
+	ctrl->torque_acc_time = 500; //will be set after start
+	ctrl->torque_dec_time = 500; //will be set after start
+	ctrl->ebrk_ramp_time = 500;  //will be set after start
 	foc_init(&ctrl->foc);
 }
 bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
@@ -47,6 +48,7 @@ bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
 		line_ramp_init(&ctrl->target_vel, CONFIG_CRUISE_RAMP_TIME);
 		line_ramp_init(&ctrl->target_current, CONFIG_CURRENT_RAMP_TIME);
 		line_ramp_init(&ctrl->input_torque, CONFIG_DEFAULT_TORQUE_RAMP_TIME);
+		line_ramp_init(&ctrl->ramp_adv_angle, CONFIG_CURRENT_RAMP_TIME);
 		mot_contrl_pid(ctrl);
 		mot_contrl_ulimit(ctrl);
 		mot_contrl_rtlimit(ctrl);
@@ -112,7 +114,7 @@ u8 mot_contrl_mode(mot_contrl_t *ctrl) {
 	if (preMode != ctrl->mode_running) {
 		if ((preMode != ctrl->mode_running) && (ctrl->mode_running == CTRL_MODE_TRQ)) {
 			line_ramp_set_acctime(&ctrl->input_torque, ctrl->torque_acc_time);
-			line_ramp_set_dectime(&ctrl->input_torque, ctrl->torque_acc_time);
+			line_ramp_set_dectime(&ctrl->input_torque, ctrl->torque_dec_time);
 			line_ramp_update(&ctrl->input_torque);
 			if (preMode == CTRL_MODE_SPD) {
 				ctrl->target_torque_raw = ctrl->target_torque;
@@ -151,7 +153,7 @@ static __INLINE void phase_curr_unbal_check(mot_contrl_t *ctrl) {
 	LowPass_Filter(ctrl->phase_curr_filted[0], foc->in.curr_abc[0], lowpass);
 	LowPass_Filter(ctrl->phase_curr_filted[1], foc->in.curr_abc[1], lowpass);
 	ctrl->phase_curr_filted[2] = -(ctrl->phase_curr_filted[0] + ctrl->phase_curr_filted[1]);
-	if ((ctrl->angle_last == INVALID_ANGLE) || (foc->mot_vel_radusPers < 100)) {
+	if ((ctrl->angle_last == INVALID_ANGLE) || (foc->mot_vel_radusPers < 100) || ctrl->out_current_vec < 50) {
 		ctrl->angle_last = foc->in.mot_angle;
 		a_max = b_max = c_max = 0;
 		_unbalance_cnt = 0;
@@ -275,9 +277,9 @@ static void mot_contrl_dq_assign(mot_contrl_t *ctrl) {
 		float target_current = line_ramp_get_interp(&ctrl->target_current);
 		if (ctrl->b_mtpa_calibrate && (ctrl->adv_angle != INVALID_ANGLE)) {
 			float s, c;
-			normal_sincosf(degree_2_pi(ctrl->adv_angle + 90.0f), &s, &c);
+			float angle_step = line_ramp_step(&ctrl->ramp_adv_angle);
+			arm_sin_cos(angle_step + 90.0f, &s, &c);
 			ctrl->target_idq.d = target_current * c;
-			
 			if (ctrl->target_idq.d > ctrl->hwlim.fw_id) {
 				ctrl->target_idq.d = ctrl->hwlim.fw_id;
 			}else if (ctrl->target_idq.d < -ctrl->hwlim.fw_id) {
@@ -414,25 +416,25 @@ static void mot_contrl_pid(mot_contrl_t *ctrl) {
 	ctrl->pi_power.kp = mc_conf()->c.pid[PID_IDCLim_ID].kp;
 	ctrl->pi_power.ki = mc_conf()->c.pid[PID_IDCLim_ID].ki;
 	ctrl->pi_power.kd = mc_conf()->c.pid[PID_IDCLim_ID].kd;
-	ctrl->pi_power.DT = slow_ctrl_ts;
+	ctrl->pi_power.ts = slow_ctrl_ts;
 
 	PI_Controller_Reset(&ctrl->pi_lock, 0);
 	ctrl->pi_lock.kp = mc_conf()->c.pid[PID_AutoHold_ID].kp;
 	ctrl->pi_lock.ki = mc_conf()->c.pid[PID_AutoHold_ID].ki;
 	ctrl->pi_lock.kd = mc_conf()->c.pid[PID_AutoHold_ID].kd;
-	ctrl->pi_lock.DT = slow_ctrl_ts;
+	ctrl->pi_lock.ts = slow_ctrl_ts;
 
 	PI_Controller_Reset(&ctrl->pi_vel_lim, 0);
 	ctrl->pi_vel_lim.kp = mc_conf()->c.pid[PID_VelLim_ID].kp;
 	ctrl->pi_vel_lim.ki = mc_conf()->c.pid[PID_VelLim_ID].ki;
 	ctrl->pi_vel_lim.kd = mc_conf()->c.pid[PID_VelLim_ID].kd;
-	ctrl->pi_vel_lim.DT = slow_ctrl_ts;
+	ctrl->pi_vel_lim.ts = slow_ctrl_ts;
 
 	PI_Controller_Reset(&ctrl->pi_vel, 0);
 	ctrl->pi_vel.kp = mc_conf()->c.pid[PID_Vel_ID].kp;
 	ctrl->pi_vel.ki = mc_conf()->c.pid[PID_Vel_ID].ki;
 	ctrl->pi_vel.kd = mc_conf()->c.pid[PID_Vel_ID].kd;
-	ctrl->pi_vel.DT = slow_ctrl_ts;
+	ctrl->pi_vel.ts = slow_ctrl_ts;
 }
 
 static void mot_contrl_ulimit(mot_contrl_t *ctrl) {
@@ -443,7 +445,7 @@ static void mot_contrl_ulimit(mot_contrl_t *ctrl) {
 	ctrl->userlim.dc_vol_min = mc_conf()->c.max_dc_vol;
 	ctrl->userlim.dc_vol_max = mc_conf()->c.min_dc_vol;
 	ctrl->userlim.ebrk_dc_curr = 0xFF;
-	ctrl->userlim.ebrk_torque = mc_conf()->c.max_ebrk_torque;
+	ctrl->userlim.ebrk_torque = mc_get_ebrk_torque();
 }
 
 static void mot_contrl_rtlimit(mot_contrl_t *ctrl) {
@@ -648,13 +650,37 @@ bool mot_contrl_set_torque(mot_contrl_t *ctrl, float torque) {
 	if (is_hw_brake_shutting_power(ctrl) && !ctrl->b_ebrk_running){
 		return false;
 	}
-	torque = fclamp(torque, -ctrl->userlim.torque, ctrl->userlim.torque);
+	float torque_min = 0;
+	float torque_max = ctrl->userlim.torque;
+	if (ctrl->mode_running == CTRL_MODE_EBRAKE) {
+		torque_min = -ctrl->userlim.torque;
+		torque_max = 0;
+	}
+	torque = fclamp(torque, torque_min, torque_max);
+	line_ramp_set_target(&ctrl->input_torque, torque);
+	return true;
+}
+
+/* 这个接口只在上位机直接设置扭矩的时候调试，其他情况一律不能使用，扭矩请求可以未负 */
+bool mot_contrl_set_force_torque(mot_contrl_t *ctrl, float torque) {
+	if (is_hw_brake_shutting_power(ctrl) && !ctrl->b_ebrk_running){
+		return false;
+	}
+	float torque_min = -ctrl->userlim.torque;
+	float torque_max = ctrl->userlim.torque;
+	if (ctrl->mode_running == CTRL_MODE_EBRAKE) {
+		torque_min = -ctrl->userlim.torque;
+		torque_max = 0;
+	}
+	torque = fclamp(torque, torque_min, torque_max);
 	line_ramp_set_target(&ctrl->input_torque, torque);
 	return true;
 }
 
+
 void mot_contrl_mtpa_calibrate(mot_contrl_t *ctrl, bool enable) {
 	if (enable) {
+		line_ramp_reset(&ctrl->ramp_adv_angle, 0);
 		ctrl->b_mtpa_calibrate = true;
 		ctrl->adv_angle = 0;
 	}else {

Applications/foc/core/controller.h

@@ -157,6 +157,7 @@ typedef struct{
 	lineramp_t 		torque_lim;
 	lineramp_t 		dc_curr_lim;
 	lineramp_t		input_torque;
+	lineramp_t		ramp_adv_angle;
 	float 			phase_a_max;
 	float 			phase_b_max;
 	float 			phase_c_max;
@@ -196,6 +197,7 @@ void mot_contrl_get_pid(mot_contrl_t *ctrl, u8 id, float *kp, float *ki, float *
 void mot_contrl_set_pid(mot_contrl_t *ctrl, u8 id, float kp, float ki, float kd);
 void mot_contrl_set_torque_limit_rttime(mot_contrl_t *ctrl, u32 time);
 void mot_contrl_set_vel_limit_rttime(mot_contrl_t *ctrl, u32 time);
+bool mot_contrl_set_force_torque(mot_contrl_t *ctrl, float torque);
 
 
 static __INLINE bool mot_contrl_start(mot_contrl_t *ctrl, u8 mode) {
@@ -258,6 +260,7 @@ static __INLINE void mot_contrl_set_angle(mot_contrl_t *ctrl, float angle) {
 
 static __INLINE void mot_contrl_set_adv_angle(mot_contrl_t *ctrl, float angle) {
 	ctrl->adv_angle = (angle);
+	line_ramp_set_target(&ctrl->ramp_adv_angle, angle);
 }
 
 static __INLINE bool mot_contrl_is_auto_holdding(mot_contrl_t *ctrl) {

Applications/foc/core/foc.c

@@ -17,11 +17,11 @@ void foc_init(foc_t *foc) {
 	foc->daxis.kp = mc_conf()->c.pid[PID_ID_ID].kp;
 	foc->daxis.ki = mc_conf()->c.pid[PID_ID_ID].ki;
 	foc->daxis.kd = mc_conf()->c.pid[PID_ID_ID].kd;
-	foc->daxis.DT = foc->ts;
+	foc->daxis.ts = foc->ts;
 	foc->qaxis.kp = mc_conf()->c.pid[PID_IQ_ID].kp;
 	foc->qaxis.ki = mc_conf()->c.pid[PID_IQ_ID].ki;
 	foc->qaxis.kd = mc_conf()->c.pid[PID_IQ_ID].kd;
-	foc->qaxis.DT = foc->ts;
+	foc->qaxis.ts = foc->ts;
 	line_ramp_init(&foc->in.target_id, CONFIG_CURRENT_LOOP_RAMP_COUNT);
 	line_ramp_set_minstep(&foc->in.target_id, CONFIG_CURRENT_LOOP_RAMP_STEP_MIN);
 	line_ramp_init(&foc->in.target_iq, CONFIG_CURRENT_LOOP_RAMP_COUNT);

Applications/foc/core/smo_observer.c

@@ -27,7 +27,7 @@ void smo_observer_init(float pll_bandwith, float lpf_wc, float Ksmo, float Ksta)
 	smo.motor_poles = mc_conf()->m.poles;
 	smo.dir_ccw = true;
 	PLL_Reset(&smo.pll, 0);
-	smo.pll.DT = smo.ts;
+	smo.pll.ts = smo.ts;
 	smo.pll.kp = pll_bandwith * 2;
 	smo.pll.ki = 0.25f * SQ(smo.pll.kp);
 }	

Applications/foc/core/thro_torque.c

@@ -15,7 +15,7 @@ static thro_torque_t _torque;
 void thro_torque_reset(void) {
 	_torque.accl = false;
 	_torque.thro_filted = 0.0f;
-	_torque.thro_ration = _torque.thro_ration_last = 0.0f;
+	_torque.throttle_opening = _torque.throttle_opening_last = 0.0f;
 	_torque.torque_req = _torque.torque_real = _torque.torque_acc_ = 0.0f;
 	_torque.gear = mc_get_internal_gear();
 }
@@ -30,7 +30,7 @@ static __INLINE float gear_rpm_torque(u8 torque, s16 max) {
 }
 
 float thro_torque_gear_map(s16 rpm, u8 gear) {
-	gear_t *_current_gear = mc_get_gear_config_by_gear(gear);
+	gear_t *_current_gear = mc_gear_conf_by_gear(gear);
 	if (_current_gear == NULL) {
 		return 0;
 	}
@@ -48,11 +48,7 @@ float thro_torque_gear_map(s16 rpm, u8 gear) {
 			float min_rpm = (i * 1000);
 			float trq2 = gear_rpm_torque(_current_gear->torque[i], _current_gear->max_torque);
 			float max_rpm = (i + 1) * 1000;
-			if (trq1 > trq2) {
-				return f_map_inv((float)rpm, min_rpm, max_rpm, trq2, trq1);
-			}else {
-				return f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
-			}
+			return f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
 		}
 	}
 	return gear_rpm_torque(_current_gear->torque[CONFIG_GEAR_SPEED_TRQ_NUM-1], _current_gear->max_torque);
@@ -81,8 +77,8 @@ static float _thro_torque_for_accelerate(float ration) {
 	float thro_torque = max_torque * ration;
 
 	float acc_r = 1.0f;
-	if (_torque.thro_ration_last < 1.0f) {
-		acc_r = (ration - _torque.thro_ration_last)/ (1.0f - _torque.thro_ration_last);
+	if (_torque.throttle_opening_last < 1.0f) {
+		acc_r = (ration - _torque.throttle_opening_last)/ (1.0f - _torque.throttle_opening_last);
 	}
 	acc_r = fclamp(acc_r, 0, 1.0f);
 	float acc_torque = _torque.torque_real + acc_r * (max_torque - _torque.torque_real);
@@ -95,7 +91,7 @@ static float _thro_torque_for_accelerate(float ration) {
 	float torque_acc_ = thro_torque - acc_torque;
 	float step = 0.0f;
 	if (torque_acc_ > 0) {
-		float acc_t = mc_get_gear_config()->accl_time;
+		float acc_t = mc_gear_conf()->accl_time;
 		step = torque_acc_ / (acc_t + 0.00001f);
 	}else {
 		torque_acc_ = 0;
@@ -106,14 +102,14 @@ static float _thro_torque_for_accelerate(float ration) {
 
 
 static float thro_torque_for_accelerate(void) {
-	return _thro_torque_for_accelerate(_torque.thro_ration);
+	return _thro_torque_for_accelerate(_torque.throttle_opening);
 }
 
 static float thro_torque_for_decelerate(void) {
-	if (_torque.thro_ration_last == 0.0f) {
+	if (_torque.throttle_opening_last == 0.0f) {
 		return 0;
 	}
-	float dec_r = _torque.thro_ration / _torque.thro_ration_last;
+	float dec_r = _torque.throttle_opening / _torque.throttle_opening_last;
 	dec_r = fclamp(dec_r, 0.0f, 1.0f);
 	return dec_r * _torque.torque_real;
 }
@@ -148,9 +144,9 @@ static void thro_torque_request(void) {
 			*  加速时间缓慢变小可以防止突然大扭矩加速
 			*/
 			u16 now_ramp_time = eCtrl_get_torque_acc_time();
-			u16 next_ramp_time = mc_get_gear_config()->accl_time;
+			u16 next_ramp_time = mc_gear_conf()->accl_time;
 			if (curr_rpm < CONFIG_ZERO_SPEED_RAMP_RMP) {
-				next_ramp_time = mc_get_gear_config()->zero_accl;
+				next_ramp_time = mc_gear_conf()->zero_accl;
 			}
 			if (next_ramp_time == 0) {
 				next_ramp_time = 100;
@@ -192,9 +188,9 @@ void thro_torque_process(u8 run_mode, float f_throttle) {
 	_torque.gear = mc_get_internal_gear();
 	LowPass_Filter(_torque.spd_filted, PMSM_FOC_GetSpeed(), THRO_RPM_LP_CEOF);
 
-	if (thro_r > _torque.thro_ration) {
+	if (thro_r > _torque.throttle_opening) {
 		if (!_torque.accl) {
-			_torque.thro_ration_last = _torque.thro_ration;
+			_torque.throttle_opening_last = _torque.throttle_opening;
 			_torque.torque_real = PMSM_FOC_Get()->in.s_targetTorque;
 			if (_torque.torque_real < 0) { //电子刹车的时候，扭矩可能为负
 				_torque.torque_real = 0;
@@ -202,9 +198,9 @@ void thro_torque_process(u8 run_mode, float f_throttle) {
 			_torque.torque_acc_ = 0;
 		}
 		_torque.accl = true;
-	}else if (thro_r < _torque.thro_ration) {
+	}else if (thro_r < _torque.throttle_opening) {
 		if (_torque.accl) {
-			_torque.thro_ration_last = _torque.thro_ration;
+			_torque.throttle_opening_last = _torque.throttle_opening;
 			_torque.torque_real = PMSM_FOC_Get()->in.s_targetTorque;
 			if (_torque.torque_real < 0) { //电子刹车的时候，扭矩可能为负
 				_torque.torque_real = 0;
@@ -213,12 +209,12 @@ void thro_torque_process(u8 run_mode, float f_throttle) {
 		_torque.accl = false;
 	}
 
-	_torque.thro_ration = thro_r;
+	_torque.throttle_opening = thro_r;
 	if (run_mode == CTRL_MODE_TRQ) {
 		thro_torque_request();
 	}else if (run_mode == CTRL_MODE_SPD) {
 		if (!mc_is_cruise_enabled()) {
-			float speed_Ref = PMSM_FOC_GetSpeedLimit() * _torque.thro_ration;
+			float speed_Ref = PMSM_FOC_GetSpeedLimit() * _torque.throttle_opening;
 			PMSM_FOC_Set_TgtSpeed(speed_Ref);
 		}
 	}else if (run_mode == CTRL_MODE_EBRAKE) {
@@ -253,5 +249,5 @@ float get_user_request_torque(void) {
 
 void thro_torque_log(void) {
 	sys_debug("accl %d, real %f, req %f\n", _torque.accl, _torque.torque_real, _torque.torque_req);
-	sys_debug("ration %f - %f - %f - %d\n", _torque.thro_ration, _torque.thro_ration_last, thro_torque_for_accelerate(), _torque.gear);
+	sys_debug("ration %f - %f - %f - %d\n", _torque.throttle_opening, _torque.throttle_opening_last, thro_torque_for_accelerate(), _torque.gear);
 }

Applications/foc/core/thro_torque_unused.h

@@ -9,8 +9,8 @@ typedef struct {
 	float torque_acc_;
 	float thro_filted;
 	float spd_filted;
-	float thro_ration;
-	float thro_ration_last;
+	float throttle_opening;
+	float throttle_opening_last;
 	u8    gear;
 }thro_torque_t;
 

Applications/foc/core/torque_unused.c

@@ -47,11 +47,7 @@ float torque_max_from_gear_rpm(void) {
 			float min_rpm = map[i-1].rpm;
 			float trq2 = map[i].torque;
 			float max_rpm = map[i].rpm;
-			if (trq1 > trq2) {
-				return f_map_inv((float)rpm, min_rpm, max_rpm, trq2, trq1);
-			}else {
-				return f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
-			}
+			return f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
 		}
 	}
 	return (float)map[map_size-1].torque;	
@@ -112,12 +108,12 @@ static float get_throttle_torque(float trq_ration) {
 }
 
 float get_thro_request_torque(void) {
-	return get_throttle_torque(torque_ctrl.thro_ration);
+	return get_throttle_torque(torque_ctrl.throttle_opening);
 }
 
-void request_torque(float thro_ration) {
+void request_torque(float throttle_opening) {
 	float curr_rpm = PMSM_FOC_GetSpeed();
-	float ref_torque = get_throttle_torque(thro_ration);
+	float ref_torque = get_throttle_torque(throttle_opening);
 #ifdef CONFIG_CRUISE_ENABLE_ACCL
 	if (mc_is_cruise_enabled() && mc_throttle_released()) {
 		return;
@@ -146,9 +142,9 @@ void request_torque(float thro_ration) {
 	}
 }
 
-void request_speed(float thro_ration) {
+void request_speed(float throttle_opening) {
 	if (!mc_is_cruise_enabled()) {
-		float speed_Ref = PMSM_FOC_GetSpeedLimit() * thro_ration;
+		float speed_Ref = PMSM_FOC_GetSpeedLimit() * throttle_opening;
 		PMSM_FOC_Set_TgtSpeed(speed_Ref);
 	}
 }
@@ -161,11 +157,11 @@ void throttle_process(u8 run_mode, float f_throttle) {
 	}else {
 		LowPass_Filter(torque_ctrl.throttle_value, f_throttle, THRO_REF_LP_CEOF);
 	}
-	torque_ctrl.thro_ration = throttle_ration(f_throttle);
+	torque_ctrl.throttle_opening = throttle_ration(f_throttle);
 	if (run_mode == CTRL_MODE_TRQ) {
-		request_torque(torque_ctrl.thro_ration);
+		request_torque(torque_ctrl.throttle_opening);
 	}else if (run_mode == CTRL_MODE_SPD) {
-		request_speed(torque_ctrl.thro_ration);
+		request_speed(torque_ctrl.throttle_opening);
 	}else if (run_mode == CTRL_MODE_EBRAKE) {
 		eCtrl_reset_Torque(0);
 		if (eCtrl_get_FinalCurrent() < 0.0001f && PMSM_FOC_GetSpeed() < CONFIG_MIN_RPM_EXIT_EBRAKE) {

Applications/foc/core/torque_unused.h

@@ -6,7 +6,7 @@ typedef struct {
 	bool  accl;
 	float torque_req;
 	float throttle_value;
-	float thro_ration; //油门开度百分比
+	float throttle_opening; //油门开度百分比
 	float spd_filted;
 	u32   count;
 }torque_manager_t;
@@ -14,7 +14,7 @@ void torque_init(void);
 void torque_reset(void);
 void trq2dq_lookup(int rpm, float torque, DQ_t *dq_out);
 void throttle_process(u8 run_mode, float f_throttle);
-void request_torque(float thro_ration);
+void request_torque(float throttle_opening);
 void trq2dq_lookup_init(void);
 float get_thro_request_torque(void);
 

Applications/foc/foc_config.h

@@ -61,7 +61,7 @@
 #define CONFIG_Volvec_Delay_Comp /* 电压矢量角度补偿 */
 #define CONFIG_Volvec_Delay_Comp_Start_Vel 500 // rpm
 
-#define CONFIG_ENABLE_IAB_REC 1   // for phase current debug
+#define CONFIG_ENABLE_IAB_REC 0   // for phase current debug
 
 #ifdef CONFIG_SPEED_LADRC
 	#define CONFIG_LADRC_Wo  200.0F

Applications/foc/limit.c

@@ -121,7 +121,7 @@ static u16 _motor_limit(void) {
 			}else if (_can_recovery){
 				mc_clr_critical_error(FOC_CRIT_MOTOR_TEMP_Lim);
 			}
-			gear_t *gear = mc_get_gear_config();
+			gear_t *gear = mc_gear_conf();
 
 			float prv_lim_value;
 			float next_lim_tmp;
@@ -178,7 +178,7 @@ static u16 _mos_limit(void) {
 			}else if (_can_recovery){
 				mc_clr_critical_error(FOC_CRIT_MOS_TEMP_Lim);
 			}
-			gear_t *gear = mc_get_gear_config();
+			gear_t *gear = mc_gear_conf();
 
 			float prv_lim_value;
 			float next_lim_tmp;

Applications/foc/lineramp.h

@@ -19,7 +19,7 @@ static __INLINE void line_ramp_set_target(lineramp_t *line, float target) {
 		}else {
 			line->step = -delta / line->time_dec;
 		}
-		if ((line->min_step >= 0) && (line->step < line->min_step)) {
+		if ((line->min_step > 0) && (line->step < line->min_step)) {
 			line->step = line->min_step;
 		}
 	}

Applications/foc/motor/encoder.c

@@ -27,7 +27,7 @@ static __INLINE void encoder_pll_update_gain(void) {
 }
 
 static void _init_pll(void) {
-	g_encoder.est_pll.DT = FOC_CTRL_US;
+	g_encoder.est_pll.ts = FOC_CTRL_US;
 	g_encoder.est_pll.max_wp = g_encoder.cpr;
 	g_encoder.pll_bandwidth = 0;
 	g_encoder.pll_bandwidth_shadow = mc_conf()->m.nor_pll_band;
@@ -117,7 +117,7 @@ static __INLINE bool encoder_run_pll(float cnt) {
 	g_encoder.est_angle_counts = g_encoder.est_pll.observer;
     bool snap_to_zero_vel = false;
 	g_encoder.est_vel_cnt_filter = LowPass_Filter(g_encoder.est_vel_cnt_filter, g_encoder.est_vel_counts, 0.1f);
-    if (ABS(g_encoder.est_pll.out) < 0.5f * g_encoder.est_pll.DT * g_encoder.est_pll.ki) {
+    if (ABS(g_encoder.est_pll.out) < 0.5f * g_encoder.est_pll.ts * g_encoder.est_pll.ki) {
         g_encoder.est_vel_cnt_filter = g_encoder.est_vel_counts = g_encoder.est_pll.out = 0.0f;  // align delta-sigma on zero to prevent jitter
         snap_to_zero_vel = true;
     }

Applications/foc/motor/motor.c

@@ -35,12 +35,13 @@ static shark_timer_t _led_off_timer = TIMER_INIT(_led_off_timer, _led_off_timer_
 m_contrl_t motor = {
 	.s_direction = POSITIVE,
 	.n_gear = 0,
-	.b_is96Mode = false,
+	.b_high_vol_mode = false,
 	.mode = CTRL_MODE_OPEN,
 	.mos_lim = 0,
 	.motor_lim = 0,
 	.b_ind_start = false,
 	.s_target_speed = MAX_S16,
+	.s_force_torque = MAX_S16,
 	.u_set.idc_lim = IDC_USER_LIMIT_NONE,
 	.u_set.rpm_lim = RPM_USER_LIMIT_NONE,
 	.u_set.ebrk_lvl = 0,
@@ -175,13 +176,13 @@ static u32 _self_check_task(void *p) {
 }
 
 static bool mc_detect_vbus_mode(void) {
-#ifdef CONFIG_FORCE_96V_MODE
-	motor.b_is96Mode = true;
+#ifdef CONFIG_FORCE_HIGH_VOL_MODE
+	motor.b_high_vol_mode = true;
 	return false;
 #else
-	bool is_96mode = motor.b_is96Mode;
-	motor.b_is96Mode = get_vbus_int() >= CONFIG_96V_MODE_VOL;
-	return (is_96mode != motor.b_is96Mode);
+	bool is_96mode = motor.b_high_vol_mode;
+	motor.b_high_vol_mode = get_vbus_int() >= CONFIG_HIGH_VOL_MODE_MIN_VOL;
+	return (is_96mode != motor.b_high_vol_mode);
 #endif
 }
 
@@ -211,8 +212,8 @@ static void _led_off_timer_handler(shark_timer_t *t) {
 }
 
 
-static void mc_gear_vmode_changed(void) {
-	gear_t *gears = mc_get_gear_config();
+static void mc_gear_mode_set(void) {
+	gear_t *gears = mc_gear_conf();
 	if (gears != &sensorless_gear) {
 		sensorless_gear.max_torque = gears->max_torque;
 	}else { //slowly changed
@@ -222,7 +223,6 @@ static void mc_gear_vmode_changed(void) {
 	mot_contrl_set_vel_limit(&motor.controller, (float)min(gears->max_speed, motor.u_set.rpm_lim));
 	mot_contrl_set_dccurr_limit(&motor.controller, (float)min(gears->max_idc, motor.u_set.idc_lim));
 	mot_contrl_set_torque_limit(&motor.controller, (float)gears->max_torque);
-	sys_debug("change %d, %d, %d\n", gears->max_idc, gears->max_speed, gears->max_torque);
 }
 
 void mc_init(void) {
@@ -248,12 +248,12 @@ m_contrl_t * mc_params(void) {
 	return &motor;
 }
 
-gear_t *mc_get_gear_config_by_gear(u8 n_gear) {
+gear_t *mc_gear_conf_by_gear(u8 n_gear) {
 	gear_t *gears;
 	if (!foc_observer_is_encoder()) { //无感模式，受限运行
 		return &sensorless_gear;
 	}
-	if (motor.b_is96Mode) {
+	if (motor.b_high_vol_mode) {
 		gears = &mc_conf()->g_n[0];
 	}else {
 		gears = &mc_conf()->g_l[0];
@@ -261,42 +261,33 @@ gear_t *mc_get_gear_config_by_gear(u8 n_gear) {
 	return &gears[n_gear];
 }
 
-gear_t *mc_get_gear_config(void) {
-	return mc_get_gear_config_by_gear(motor.n_gear);
+gear_t *mc_gear_conf(void) {
+	return mc_gear_conf_by_gear(motor.n_gear);
 }
 
 static __INLINE float gear_rpm_2_torque(u8 torque, s16 max) {
 	return (float)torque/100.0f * max;
 }
 
-float mc_get_max_torque_with_gear_vel(s16 vel, u8 gear) {
-	gear_t *_current_gear = mc_get_gear_config_by_gear(gear);
-	if (_current_gear == NULL) {
+float mc_gear_max_torque(s16 vel, u8 gear_n) {
+	gear_t *gear = mc_gear_conf_by_gear(gear_n);
+	if (gear == NULL) {
 		return 0;
 	}
 
-	if (vel > _current_gear->max_speed) {
-		vel = _current_gear->max_speed;
-	}
 	vel = ABS(vel);
 	if (vel <= 1000) {
-		return gear_rpm_2_torque(_current_gear->torque[0], _current_gear->max_torque);
+		return gear_rpm_2_torque(gear->torque[0], gear->max_torque);
 	}
-
-	for (int i = 1; i < CONFIG_GEAR_SPEED_TRQ_NUM; i++) {
-		if (vel <= 1000 * (i + 1)) { //线性插值
-			float trq1 = gear_rpm_2_torque(_current_gear->torque[i-1], _current_gear->max_torque);
-			float min_rpm = (i * 1000);
-			float trq2 = gear_rpm_2_torque(_current_gear->torque[i], _current_gear->max_torque);
-			float max_rpm = (i + 1) * 1000;
-			if (trq1 > trq2) {
-				return f_map_inv((float)vel, min_rpm, max_rpm, trq2, trq1);
-			}else {
-				return f_map((float)vel, min_rpm, max_rpm, trq1, trq2);
-			}
-		}
+	int vel_idx = vel / 1000;
+	if (vel_idx >= CONFIG_GEAR_SPEED_TRQ_NUM -1 ) {
+		return gear_rpm_2_torque(gear->torque[CONFIG_GEAR_SPEED_TRQ_NUM-1], gear->max_torque);
 	}
-	return gear_rpm_2_torque(_current_gear->torque[CONFIG_GEAR_SPEED_TRQ_NUM-1], _current_gear->max_torque);
+	float torque_1 = gear_rpm_2_torque(gear->torque[vel_idx-1], gear->max_torque);
+	float min_rpm = vel_idx * 1000;
+	float torque_2 = gear_rpm_2_torque(gear->torque[vel_idx], gear->max_torque);
+	float max_rpm = min_rpm + 1000;
+	return f_map((float)vel, min_rpm, max_rpm, torque_1, torque_2);
 }
 
 
@@ -348,6 +339,7 @@ bool mc_start(u8 mode) {
 #ifdef CONFIG_DQ_STEP_RESPONSE
 	mode = CTRL_MODE_CURRENT;
 #endif
+	motor.s_force_torque = MAX_S16;
 	mc_detect_vbus_mode();
 	etcs_enable(&motor.controller.etcs, motor.u_set.b_tcs);
 	if (motor.b_lock_motor) {
@@ -372,16 +364,20 @@ bool mc_start(u8 mode) {
 		mot_contrl_set_error(&motor.controller, FOC_Throttle_Err);
 		return false;
 	}
-
+	
 	pwm_up_enable(false);
 
-	_mc_internal_init(mode, true);
+	u32 mask = cpu_enter_critical();
 
+	_mc_internal_init(mode, true);
 	throttle_torque_reset();
-	mc_gear_vmode_changed();
-	mot_contrl_set_torque_ramp_time(&motor.controller, mc_get_gear_config()->zero_accl, mc_conf()->c.thro_dec_time);
 	motor_encoder_start(true);
 	mot_contrl_start(&motor.controller, mode);
+	mot_contrl_set_torque_ramp_time(&motor.controller, mc_gear_conf()->zero_accl, mc_conf()->c.thro_dec_time);
+	mc_set_ebrk_level(motor.u_set.ebrk_lvl);
+	mc_gear_mode_set();
+	cpu_exit_critical(mask);
+
 	pwm_turn_on_low_side();
 	delay_ms(10);
 	phase_current_offset_calibrate();
@@ -401,7 +397,6 @@ bool mc_start(u8 mode) {
 	if (mc_detect_hwbrake()) {
 		mot_contrl_set_hw_brake(&motor.controller, true);
 	}
-	gpio_beep(200);
 
 	return true;
 }
@@ -459,7 +454,7 @@ bool mc_set_gear(u8 gear) {
 	if (motor.n_gear != gear) {
 		u32 mask = cpu_enter_critical();
 		motor.n_gear = gear;
-		mc_gear_vmode_changed();
+		mc_gear_mode_set();
 		cpu_exit_critical(mask);
 	}
 	return true;
@@ -505,6 +500,18 @@ bool mc_set_target_vel(s16 vel) {
 	return true;
 }
 
+bool mc_set_force_torque(s16 torque) {
+	if (motor.mode != CTRL_MODE_TRQ) {
+		return false;
+	}
+	if (torque == MAX_S16) {
+		motor.s_force_torque = MAX_S16;
+	}else {
+		motor.s_force_torque = fclamp(torque, -mc_gear_conf()->max_torque, mc_gear_conf()->max_torque);
+	}
+	return true;
+}
+
 bool mc_is_cruise_enabled(void) {
 	return motor.b_cruise;
 }
@@ -528,7 +535,7 @@ void mc_set_idc_limit(s16 limit) {
 		return;
 	}
 	motor.u_set.idc_lim = limit;
-	mc_gear_vmode_changed();
+	mc_gear_mode_set();
 }
 
 void mc_set_rpm_limit(s16 limit) {
@@ -536,7 +543,7 @@ void mc_set_rpm_limit(s16 limit) {
 		return;
 	}
 	motor.u_set.rpm_lim = limit;
-	mc_gear_vmode_changed();
+	mc_gear_mode_set();
 }
 
 
@@ -592,7 +599,7 @@ bool mc_set_foc_mode(u8 mode) {
 	bool ret = false;
 	if (mot_contrl_request_mode(&motor.controller, mode)) {
 		motor.mode = mode;
-		if (mode == CTRL_MODE_OPEN || mode == CTRL_MODE_CURRENT) {
+		if ((mode == CTRL_MODE_OPEN || mode == CTRL_MODE_CURRENT) && !mot_contrl_is_start(&motor.controller)) {
 			mot_contrl_start(&motor.controller, motor.mode);
 			pwm_enable_channel();
 		}
@@ -636,7 +643,7 @@ bool mc_start_epm(bool epm) {
 		mot_contrl_request_mode(&motor.controller, CTRL_MODE_TRQ);
 		mot_contrl_set_torque_limit_rttime(&motor.controller, CONFIG_LIMIT_RAMP_TIME);
 		mot_contrl_set_vel_rttime(&motor.controller, CONFIG_CRUISE_RAMP_TIME);
-		mc_gear_vmode_changed();
+		mc_gear_mode_set();
 	}
 	cpu_exit_critical(mask);
 	
@@ -670,7 +677,7 @@ bool mc_start_epm_move(epm_dir_t dir, bool is_command) {
 		
 		if (!mot_contrl_is_start(&motor.controller)) {
 			mot_contrl_start(&motor.controller, motor.mode);
-			mc_gear_vmode_changed();
+			mc_gear_mode_set();
 			pwm_enable_channel();
 		}else if (mot_contrl_is_auto_holdding(&motor.controller)) {
 			mc_auto_hold(false);
@@ -993,6 +1000,9 @@ u32 mc_get_critical_error(void) {
 }
 
 bool mc_throttle_released(void) {
+	if (motor.s_force_torque != MAX_S16) {
+		return motor.s_force_torque == 0;
+	}
 	if (motor.b_ignor_throttle) {
 		return motor.u_throttle_ration == 0;
 	}
@@ -1352,7 +1362,7 @@ static void mc_process_epm_move(void) {
 		target_vel = -mc_conf()->c.max_epm_back_rpm;
 		target_trq = mc_conf()->c.max_epm_back_torque;
 	}else if (!motor.b_epm_cmd_move) {
-		target_vel = throttle_vol_to_open_ration(throttle_get_signal()) * 2.0f * (float)target_vel;
+		target_vel = throttle_vol_to_opening(throttle_get_signal()) * 2.0f * (float)target_vel;
 	}
 	motor.f_epm_trq = target_trq;
 	motor.f_epm_vel = target_vel;
@@ -1471,7 +1481,7 @@ static void mc_motor_runstop(void) {
 	if (!mot_contrl_is_start(&motor.controller) && mc_can_restart_foc()) {
 		mask = cpu_enter_critical();
 		mot_contrl_start(&motor.controller, motor.mode);
-		mc_gear_vmode_changed();
+		mc_gear_mode_set();
 		throttle_torque_reset();
 		pwm_enable_channel();
 		g_meas_foc.first = true;
@@ -1518,7 +1528,7 @@ static void mc_process_throttle_torque(float vol) {
 	}
 }
 /*FOC 的部分处理，比如速度环，状态机，转把采集等*/
-measure_time_t g_meas_MCTask;
+measure_time_t g_meas_MCTask = {.exec_max_time = 100, .intval_max_time = 1000,  .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
 #define mc_TaskStart time_measure_start(&g_meas_MCTask)
 #define mc_TaskEnd time_measure_end(&g_meas_MCTask)
 void Sched_MC_mTask(void) {
@@ -1536,7 +1546,6 @@ void Sched_MC_mTask(void) {
 	mc_process_curise();
 #endif
 	u8 runMode = mot_contrl_mode(&motor.controller);
-
 	/*保护功能*/
 	u8 limted = mot_contrl_protect(&motor.controller);
 	/* 母线电流，实际采集的相电流矢量大小的计算 */
@@ -1560,7 +1569,7 @@ void Sched_MC_mTask(void) {
 	}
 	bool sensor_less = !foc_observer_is_encoder();
 	if (mc_detect_vbus_mode() || (limted == FOC_LIM_CHANGE_L) || (_sensorless_run != sensor_less)) {
-		mc_gear_vmode_changed();
+		mc_gear_mode_set();
 		if (sensor_less && foc_observer_sensorless_stable()) {//unstable 记录在ADC中断处理中
 			if (motor_encoder_may_error() == ENCODER_PWM_ERR) {
 				mc_set_critical_error(FOC_CRIT_Encoder_Err);
@@ -1579,7 +1588,7 @@ void Sched_MC_mTask(void) {
 	/* 如果取消高温，欠压等限流需要释放转把后才生效，确保不会突然加速 */
 	if (motor.b_limit_pending && mc_throttle_released()) {
 		motor.b_limit_pending = false;
-		mc_gear_vmode_changed();
+		mc_gear_mode_set();
 	}
 	/* 堵转处理 */
 	if (mc_run_stall_process(runMode) || (motor.mode == CTRL_MODE_CURRENT)) {
@@ -1611,12 +1620,16 @@ void Sched_MC_mTask(void) {
 					mot_contrl_set_target_vel(&motor.controller, motor.s_target_speed);
 				}
 			}else {
-				float thro = throttle_get_signal();
-				if (motor.b_ignor_throttle) {
-					float r = (float)motor.u_throttle_ration/100.0f;
-					thro = throttle_open_ration_to_vol(r);
+				if (motor.s_force_torque != MAX_S16) {
+					mot_contrl_set_force_torque(&motor.controller, motor.s_force_torque);
+				}else {
+					float thro = throttle_get_signal();
+					if (motor.b_ignor_throttle) {
+						float r = (float)motor.u_throttle_ration/100.0f;
+						thro = throttle_opening_to_vol(r);
+					}
+					mc_process_throttle_torque(thro);
 				}
-				mc_process_throttle_torque(thro);
 			}
 			mot_contrl_slow_task(&motor.controller);
 		}

Applications/foc/motor/motor.h

@@ -52,7 +52,7 @@ typedef struct {
 	u32    n_brake_errors;
 	u32    n_CritiCalErrMask;
 	u8     mode;
-	bool   b_is96Mode;
+	bool   b_high_vol_mode;
 	u8     n_gear;
 	bool   b_limit_pending;
 	gear_t *gear_cfg;
@@ -62,6 +62,7 @@ typedef struct {
 	u8     motor_lim;
 	s16    s_vbus_hw_min;
 	s16    s_target_speed;
+	s16    s_force_torque;
 	mot_contrl_t controller;
 	user_rt_set u_set; //用户运行时设置
 	fan_t  fan[2];
@@ -101,8 +102,8 @@ bool mc_is_cruise_enabled(void);
 bool mc_set_cruise_speed(bool rpm_abs, float target_rpm);
 void mc_set_idc_limit(s16 limit);
 void mc_set_rpm_limit(s16 limit);
-gear_t *mc_get_gear_config(void);
-gear_t *mc_get_gear_config_by_gear(u8 n_gear);
+gear_t *mc_gear_conf(void);
+gear_t *mc_gear_conf_by_gear(u8 n_gear);
 void mc_set_motor_lim_level(u8 l);
 void mc_set_mos_lim_level(u8 l);
 void motor_debug(void);
@@ -117,7 +118,8 @@ bool mc_ind_motor_start(bool start);
 void mc_enable_brkshutpower(u8 shut);
 void mc_enable_tcs(bool enable);
 bool mc_set_target_vel(s16 vel);
-float mc_get_max_torque_with_gear_vel(s16 vel, u8 gear);
+float mc_gear_max_torque(s16 vel, u8 gear);
+bool mc_set_force_torque(s16 torque);
 
 static __INLINE mot_contrl_t *mot_contrl(void) {
 	return &motor.controller;

Applications/foc/motor/motor_param.c

@@ -35,11 +35,7 @@ s16 motor_max_torque_for_rpm(s16 rpm) {
 			float min_rpm = mot_map[i-1].rpm;
 			float trq2 = mot_map[i].torque;
 			float max_rpm = mot_map[i].rpm;
-			if (trq1 > trq2) {
-				return (s16)f_map_inv((float)rpm, min_rpm, max_rpm, trq2, trq1);
-			}else {
-				return (s16)f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
-			}
+			return (s16)f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
 		}
 	}
 	return mot_map[map_size-1].torque;
@@ -227,11 +223,11 @@ void motor_mpta_fw_lookup(float rpm, float torque, dq_t *dq_out) {
 		if (ABS(rpm) < 1000) {
 			d = 0;
 		}else if (ABS(rpm) < 3000) {
-			d = -5;
-		}else if (ABS(rpm) < 5000) {
 			d = -10;
+		}else if (ABS(rpm) < 5000) {
+			d = -30;
 		}else {
-			d = -20;
+			d = -50;
 		}
 	}
 #else

Applications/foc/motor/throttle.c

@@ -20,8 +20,7 @@ static throttle_torque_t _throttle;
 
 void throttle_torque_reset(void) {
 	_throttle.accl = false;
-	_throttle.thro_filted = 0.0f;
-	_throttle.thro_ration = _throttle.thro_ration_last = 0.0f;
+	_throttle.throttle_opening = _throttle.throttle_opening_last = 0.0f;
 	_throttle.torque_req = _throttle.torque_real = _throttle.torque_acc_ = 0.0f;
 	_throttle.gear = mc_get_internal_gear();
 }
@@ -127,7 +126,7 @@ void throttle_force_detect(void) {
 }
 
 /* 获取转把电压对应的油门开度 */
-float throttle_vol_to_open_ration(int thro_val) {
+float throttle_vol_to_opening(float thro_val) {
 	if (thro_val <= throttle_start_vol()) {
 		return 0;
 	}
@@ -139,16 +138,17 @@ float throttle_vol_to_open_ration(int thro_val) {
 /* 获取油门开度 */
 float throttle_get_open_ration(void) {
 	float thro_val = throttle_get_signal();
-	return throttle_vol_to_open_ration(thro_val);
+	return throttle_vol_to_opening(thro_val);
 }
 
 /* 获取油门开度对应的转把电压 */
-float throttle_open_ration_to_vol(float r) {
+float throttle_opening_to_vol(float r) {
 	if (r == 0) {
 		return 0;
+	}else if (r > 1.0f) {
+		r = 1.0f;
 	}
-	float vol = throttle_start_vol() + r * throttle_vol_range();
-	return fclamp(vol, 0, throttle_end_vol());
+	return (throttle_start_vol() + r * throttle_vol_range());
 }
 
 
@@ -205,12 +205,12 @@ void throttle_detect(bool ready) {
 }
 
 static float _throttle_torque_for_accelerate(float ration) {
-	float max_torque = mc_get_max_torque_with_gear_vel((s16)_throttle.vel_filted, _throttle.gear);
+	float max_torque = mc_gear_max_torque((s16)_throttle.vel_filted, _throttle.gear);
 	float thro_torque = max_torque * ration;
 
 	float acc_r = 1.0f;
-	if (_throttle.thro_ration_last < 1.0f) {
-		acc_r = (ration - _throttle.thro_ration_last)/ (1.0f - _throttle.thro_ration_last);
+	if (_throttle.throttle_opening_last < 1.0f) {
+		acc_r = (ration - _throttle.throttle_opening_last)/ (1.0f - _throttle.throttle_opening_last);
 	}
 	acc_r = fclamp(acc_r, 0, 1.0f);
 	float acc_torque = _throttle.torque_real + acc_r * (max_torque - _throttle.torque_real);
@@ -223,7 +223,7 @@ static float _throttle_torque_for_accelerate(float ration) {
 	float torque_acc_ = thro_torque - acc_torque;
 	float step = 0.0f;
 	if (torque_acc_ > 0) {
-		float acc_t = mc_get_gear_config()->accl_time;
+		float acc_t = mc_gear_conf()->accl_time;
 		step = torque_acc_ / (acc_t + 0.00001f);
 	}else {
 		torque_acc_ = 0;
@@ -234,31 +234,32 @@ static float _throttle_torque_for_accelerate(float ration) {
 
 
 static float throttle_torque_for_accelerate(void) {
-	return _throttle_torque_for_accelerate(_throttle.thro_ration);
+	return _throttle_torque_for_accelerate(_throttle.throttle_opening);
 }
 
 static float throttle_torque_for_decelerate(void) {
-	if (_throttle.thro_ration_last == 0.0f) {
+	if (_throttle.throttle_opening_last == 0.0f) {
 		return 0;
 	}
-	float dec_r = _throttle.thro_ration / _throttle.thro_ration_last;
+	float dec_r = _throttle.throttle_opening / _throttle.throttle_opening_last;
 	dec_r = fclamp(dec_r, 0.0f, 1.0f);
 	return dec_r * _throttle.torque_real;
 }
 
 float throttle_get_open_ration_filted(void) {
-	return _throttle.thro_ration;
+	return _throttle.throttle_opening;
 }
 
 #define THRO_RPM_LP_CEOF 0.01f
 float throttle_get_torque(mot_contrl_t * ctrl, float vol) {
-	float thro_r = throttle_vol_to_open_ration(vol);
+	float throttle_opening = throttle_vol_to_opening(vol);
+	float vel = mot_contrl_get_speed(ctrl);
 	_throttle.gear = mc_get_internal_gear();
-	LowPass_Filter(_throttle.vel_filted, mot_contrl_get_speed(ctrl), THRO_RPM_LP_CEOF);
+	LowPass_Filter(_throttle.vel_filted, vel, THRO_RPM_LP_CEOF);
 
-	if (thro_r > _throttle.thro_ration) {
+	if (throttle_opening > _throttle.throttle_opening) {
 		if (!_throttle.accl) {
-			_throttle.thro_ration_last = _throttle.thro_ration;
+			_throttle.throttle_opening_last = _throttle.throttle_opening;
 			_throttle.torque_real = _throttle.torque_req;
 			if (_throttle.torque_real < 0) { //电子刹车的时候，扭矩可能为负
 				_throttle.torque_real = 0;
@@ -266,9 +267,9 @@ float throttle_get_torque(mot_contrl_t * ctrl, float vol) {
 			_throttle.torque_acc_ = 0;
 		}
 		_throttle.accl = true;
-	}else if (thro_r < _throttle.thro_ration) {
+	}else if (throttle_opening < _throttle.throttle_opening) {
 		if (_throttle.accl) {
-			_throttle.thro_ration_last = _throttle.thro_ration;
+			_throttle.throttle_opening_last = _throttle.throttle_opening;
 			_throttle.torque_real = ctrl->target_torque_raw;
 			/* 如果扭矩给定的ramp没有结束，使用原始扭矩请求作为减扭矩的起始点 */
 			if (_throttle.torque_req - line_ramp_get_interp(&ctrl->input_torque) >= 10.0f ) {
@@ -280,7 +281,7 @@ float throttle_get_torque(mot_contrl_t * ctrl, float vol) {
 		}
 		_throttle.accl = false;
 	}
-	_throttle.thro_ration = thro_r;
+	_throttle.throttle_opening = throttle_opening;
 	if (_throttle.accl) {
 		return throttle_torque_for_accelerate();
 	}else {
@@ -309,9 +310,9 @@ void throttle_set_torque(mot_contrl_t * ctrl, float torque) {
 		*  加速时间缓慢变小可以防止突然大扭矩加速
 		*/
 		u16 now_ramp_time = mot_contrl_get_torque_acc_time(ctrl);
-		u16 next_ramp_time = mc_get_gear_config()->accl_time;
+		u16 next_ramp_time = mc_gear_conf()->accl_time;
 		if (curr_vel < CONFIG_ZERO_SPEED_RAMP_RMP) {
-			next_ramp_time = mc_get_gear_config()->zero_accl;
+			next_ramp_time = mc_gear_conf()->zero_accl;
 		}
 
 		if (now_ramp_time != next_ramp_time) {
@@ -345,3 +346,8 @@ float get_user_request_torque(void) {
 	return throttle_torque_for_decelerate();
 }
 
+
+void throttle_log(void) {
+	sys_debug("r:%f, last %f, real:%f, req %f\n", _throttle.throttle_opening, _throttle.throttle_opening_last, _throttle.torque_real, _throttle.torque_req);
+	sys_debug("thro: %f-%f, %f\n", throttle_start_vol(), throttle_end_vol(), mc_gear_max_torque((s16)_throttle.vel_filted, _throttle.gear));
+}

Applications/foc/motor/throttle.h

@@ -11,10 +11,9 @@ typedef struct {
 	float torque_req;
 	float torque_real;
 	float torque_acc_;
-	float thro_filted;
 	float vel_filted;
-	float thro_ration;
-	float thro_ration_last;
+	float throttle_opening;
+	float throttle_opening_last;
 	u8    gear;
 }throttle_torque_t;
 
@@ -34,8 +33,8 @@ void throttle_set_torque(mot_contrl_t * ctrl, float torque);
 float throttle_get_torque(mot_contrl_t * ctrl, float vol);
 float throttle_get_open_ration_filted(void);
 float get_user_request_torque(void);
-float throttle_open_ration_to_vol(float r);
-float throttle_vol_to_open_ration(int thro_val);
+float throttle_opening_to_vol(float r);
+float throttle_vol_to_opening(float thro_val);
 void throttle_torque_reset(void);
 #endif /* _THROTTLE_H__ */
 

Applications/libs/time_measure.c

@@ -40,7 +40,9 @@ void time_measure_end(measure_time_t *m) {
 	m->exec_time = time_delta_us(m->exec_count, NULL);
 	if (m->exec_time > m->exec_max_time) {
 		m->exec_time_error ++;
-		m->exec_max_error_time = m->exec_time;
+		if (m->exec_time > m->exec_max_error_time) {
+			m->exec_max_error_time = m->exec_time;
+		}
 	}
 }
 

Applications/math/fast_math.h

@@ -53,14 +53,14 @@ static void fast_norm_angle(float *angle) {
 		*angle += 360.0f;
 	}
 }
-/* 递增map */
-static __INLINE float f_map(float x, float in_min, float in_max, float out_min, float out_max) {
-	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
-}
 
-/* 递减map */
-static __INLINE float f_map_inv(float x, float in_min, float in_max, float out_min, float out_max) {
-	return out_max - (x - in_min) * (out_max - out_min) / (in_max - in_min);
+
+static __INLINE float f_map(float x, float in_min, float in_max, float out1, float out2) {
+	if (out1 > out2) { /* 递增map */
+		return out1 - (x - in_min) * (out1 - out2) / (in_max - in_min);
+	}else { /* 递减map */
+		return (x - in_min) * (out2 - out1) / (in_max - in_min) + out1;
+	}
 }
 
 

Applications/prot/can_foc_msg.c

@@ -60,6 +60,17 @@ void can_report_dq_current(u8 can) {
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }
 
+void can_report_dq_voltage(u8 can) {
+	u8 data[6];
+	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Dq_Vol));
+	float id = foc()->out.vol_dq.d;
+	float iq = foc()->out.vol_dq.q;
+	encode_s16(data + 2, S16Q5(id));
+	encode_s16(data + 4, S16Q5(iq));
+	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
+}
+
+
 void can_response_hall_offset(u8 can, int offset) {
 	u8 data[7];
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Cali_Hall_Offset));
@@ -111,9 +122,6 @@ void can_mcast_foc_status2(void) {
 
 void can_report_mpta_values(u8 can) {
 	u8 data[8];
-	if (!motor.controller.b_mtpa_calibrate) {
-		return;
-	}
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_MTPA_DQ_Angle));
 	encode_s16(data + 2, S16Q5(mot_contrl_get_current_vector(&motor.controller)));
 	encode_s16(data + 4, S16Q5(motor.controller.out_idq_filterd.d));

Applications/prot/can_foc_msg.h

@@ -19,6 +19,7 @@ void can_plot1(s16 v1);
 void can_plot2(s16 v1, s16 v2);
 void can_plot3(s16 v1, s16 v2, s16 v3);
 void can_report_mot_params_ested(float v, u8 type);
+void can_report_dq_voltage(u8 can);
 
 #endif	/*_Can_Foc_Msg_H__ */
 

Project/MC105_V3_SERVO.uvoptx

@@ -77,7 +77,7 @@
         <tvExpOptDlg>0</tvExpOptDlg>
         <IsCurrentTarget>1</IsCurrentTarget>
       </OPTFL>
-      <CpuCode>0</CpuCode>
+      <CpuCode>255</CpuCode>
       <DebugOpt>
         <uSim>0</uSim>
         <uTrg>1</uTrg>