adrc 参数加入配置

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

Applications/app/nv_storage.c

@@ -119,6 +119,14 @@ static void nv_default_foc_params(void) {
 	foc_params.pid_conf[PID_Lock_id].kp = (0.01f);
 	foc_params.pid_conf[PID_Lock_id].ki = (0.20f);
 	foc_params.pid_conf[PID_Lock_id].kd = 0;
+#ifdef CONFIG_SPEED_LADRC
+	foc_params.f_adrc_vel_lim_Wo = CONFIG_LADRC_Wo;
+	foc_params.f_adrc_vel_lim_Wcv = CONFIG_LADRC_Wcv;
+	foc_params.f_adrc_vel_lim_B0 = CONFIG_LADRC_B0;
+	foc_params.f_adrc_vel_Wo = CONFIG_LADRC_Wo;
+	foc_params.f_adrc_vel_Wcv = CONFIG_LADRC_Wcv;
+	foc_params.f_adrc_vel_B0 = CONFIG_LADRC_B0;
+#endif
 }
 
 static void nv_default_gear_config(void) {

Applications/app/nv_storage.h

@@ -27,7 +27,16 @@ typedef struct {
 	pid_conf_t pid_conf[PID_Max_id];
 	float f_minThroVol;
 	float f_maxThroVol;
-	u8    res[2048 - 157];
+
+	float f_adrc_vel_lim_Wo;
+	float f_adrc_vel_lim_Wcv;
+	float f_adrc_vel_lim_B0;
+
+	float f_adrc_vel_Wo;
+	float f_adrc_vel_Wcv;
+	float f_adrc_vel_B0;
+
+	u8    res[2048 - 157 - 24];
 	u16   crc16;
 }foc_params_t;
 

Applications/foc/commands.c

@@ -117,7 +117,7 @@ static void process_ext_command(foc_cmd_body_t *command) {
 	
 }
 
-static u8 ignore_with_speed[] = {Foc_Set_Gear_Limit, Foc_Conf_Pid, Foc_Set_Throttle_throld, Foc_Set_Config, Foc_Set_eBrake_Throld, Foc_Set_Limiter_Config, Foc_End_Write_TRQ_Table, Foc_SN_Write};
+static u8 ignore_with_speed[] = {Foc_Set_Adrc_Params, Foc_Set_Gear_Limit, Foc_Conf_Pid, Foc_Set_Throttle_throld, Foc_Set_Config, Foc_Set_eBrake_Throld, Foc_Set_Limiter_Config, Foc_End_Write_TRQ_Table, Foc_SN_Write};
 
 static bool _can_process_with_speed(u8 cmd) {
 	int size = ARRAY_SIZE(ignore_with_speed);
@@ -326,7 +326,32 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			}
 			break;
 		}
-
+		case Foc_Set_Adrc_Params:
+		{
+			if (command->len < 24) {
+				erroCode = FOC_Param_Err;
+				break;
+			}
+			nv_get_foc_params()->f_adrc_vel_lim_Wo = decode_float(command->data);
+			nv_get_foc_params()->f_adrc_vel_lim_Wcv = decode_float((u8 *)command->data + 4);
+			nv_get_foc_params()->f_adrc_vel_lim_B0 = decode_float((u8 *)command->data + 8);
+			nv_get_foc_params()->f_adrc_vel_Wo = decode_float((u8 *)command->data + 12);
+			nv_get_foc_params()->f_adrc_vel_Wcv = decode_float((u8 *)command->data + 16);
+			nv_get_foc_params()->f_adrc_vel_B0 = decode_float((u8 *)command->data + 20);
+			nv_save_foc_params();
+			break;
+		}
+		case Foc_Get_Adrc_Params:
+		{
+			encode_float(response+3, nv_get_foc_params()->f_adrc_vel_lim_Wo);
+			encode_float(response+7, nv_get_foc_params()->f_adrc_vel_lim_Wcv);
+			encode_float(response+11, nv_get_foc_params()->f_adrc_vel_lim_B0);
+			encode_float(response+15, nv_get_foc_params()->f_adrc_vel_Wo);
+			encode_float(response+19, nv_get_foc_params()->f_adrc_vel_Wcv);
+			encode_float(response+23, nv_get_foc_params()->f_adrc_vel_B0);
+			len += 24;
+			break;
+		}
 		case Foc_Set_EPM_Mode:
 		{
 			bool mode = decode_u8((u8 *)command->data) == 0?false:true;

Applications/foc/commands.h

@@ -42,6 +42,8 @@ typedef enum {
 	Foc_SN_Read,
 	Foc_Set_DQ_Current, //0x44
 	Foc_Set_Gear_Mode,//0-3, eco, normal, sport, turbo
+	Foc_Set_Adrc_Params,
+	Foc_Get_Adrc_Params,
 	Foc_Hall_Phase_Cali_Result = 160,
 	Foc_Hall_Offset_Cali_Result,
 	Foc_Report_Speed,

Applications/foc/core/PMSM_FOC_Core.c

@@ -131,6 +131,7 @@ static void PMSM_FOC_Reset_PID(void) {
 }
 
 static void PMSM_FOC_Conf_PID(void) {
+	float slow_ctrl_ts = (1.0f/(float)CONFIG_SPD_CTRL_TS);
 	gFoc_Ctrl.pi_id.kp = nv_get_foc_params()->pid_conf[PID_D_id].kp;
 	gFoc_Ctrl.pi_id.ki = nv_get_foc_params()->pid_conf[PID_D_id].ki;
 	gFoc_Ctrl.pi_id.kd = nv_get_foc_params()->pid_conf[PID_D_id].kd;
@@ -144,25 +145,25 @@ static void PMSM_FOC_Conf_PID(void) {
 	gFoc_Ctrl.pi_power.kp = nv_get_foc_params()->pid_conf[PID_Pow_id].kp;
 	gFoc_Ctrl.pi_power.ki = nv_get_foc_params()->pid_conf[PID_Pow_id].ki;
 	gFoc_Ctrl.pi_power.kd = nv_get_foc_params()->pid_conf[PID_Pow_id].kd;
-	gFoc_Ctrl.pi_power.DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
+	gFoc_Ctrl.pi_power.DT = slow_ctrl_ts;
 
 	gFoc_Ctrl.pi_lock.kp = nv_get_foc_params()->pid_conf[PID_Lock_id].kp;
 	gFoc_Ctrl.pi_lock.ki = nv_get_foc_params()->pid_conf[PID_Lock_id].ki;
 	gFoc_Ctrl.pi_lock.kd = nv_get_foc_params()->pid_conf[PID_Lock_id].kd;
-	gFoc_Ctrl.pi_lock.DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
+	gFoc_Ctrl.pi_lock.DT = slow_ctrl_ts;
 
 #ifdef CONFIG_SPEED_LADRC
-	ladrc_init(&gFoc_Ctrl.vel_lim_adrc, 1.0f/(float)CONFIG_SPD_CTRL_TS, CONFIG_LADRC_Wo, CONFIG_LADRC_Wcv, CONFIG_LADRC_B0);
-	ladrc_init(&gFoc_Ctrl.vel_adrc, 1.0f/(float)CONFIG_SPD_CTRL_TS, CONFIG_LADRC_Wo, CONFIG_LADRC_Wcv, CONFIG_LADRC_B0);
+	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);
+	ladrc_init(&gFoc_Ctrl.vel_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);
 #else
 	gFoc_Ctrl.pi_torque.kp = nv_get_foc_params()->pid_conf[PID_TRQ_id].kp;
 	gFoc_Ctrl.pi_torque.ki = nv_get_foc_params()->pid_conf[PID_TRQ_id].ki;
 	gFoc_Ctrl.pi_torque.kd = nv_get_foc_params()->pid_conf[PID_TRQ_id].kd;
-	gFoc_Ctrl.pi_torque.DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
+	gFoc_Ctrl.pi_torque.DT = slow_ctrl_ts;
 	gFoc_Ctrl.pi_speed.kp = nv_get_foc_params()->pid_conf[PID_Spd_id].kp;
 	gFoc_Ctrl.pi_speed.ki = nv_get_foc_params()->pid_conf[PID_Spd_id].ki;
 	gFoc_Ctrl.pi_speed.kd = nv_get_foc_params()->pid_conf[PID_Spd_id].kd;
-	gFoc_Ctrl.pi_speed.DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
+	gFoc_Ctrl.pi_speed.DT = slow_ctrl_ts;
 #endif
 }