加入上位机可以设置正负扭矩的功能，一半用在对拖系统中

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

Applications/app/app.c

@@ -61,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();
-	throttle_log();
+	//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());

Applications/foc/commands.c

@@ -685,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/controller.c

@@ -660,6 +660,23 @@ bool mot_contrl_set_torque(mot_contrl_t *ctrl, float 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) {
 		ctrl->b_mtpa_calibrate = true;

Applications/foc/core/controller.h

@@ -196,6 +196,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) {

Applications/foc/motor/motor.c

@@ -41,6 +41,7 @@ m_contrl_t motor = {
 	.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,
@@ -338,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) {
@@ -498,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;
 }
@@ -986,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;
 	}
@@ -1603,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_opening_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

@@ -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];
@@ -118,6 +119,7 @@ void mc_enable_brkshutpower(u8 shut);
 void mc_enable_tcs(bool enable);
 bool mc_set_target_vel(s16 vel);
 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;