Limits in PID is added. (30da38fc) · Commits · Wenxi XU / Mambo

drivers/pid/pid_mit.c

+3 −3

Original line number	Diff line number	Diff line
		@@ -18,15 +18,15 @@

		#define DT_DRV_COMPAT pid_mit

		LOG_MODULE_REGISTER(pid_mit, CONFIG_MOTOR_LOG_LEVEL);

		#define PID_SINGLE_DT_DRIVER_CONFIG_GET(node_id) \
		{ \
		.k_p = DT_STRING_UNQUOTED(node_id, k_p), \
		.k_i = DT_STRING_UNQUOTED(node_id, k_i), \
		.k_d = DT_STRING_UNQUOTED(node_id, k_d), \
		.integral_limit = DT_STRING_UNQUOTED_OR(node_id, i_max, 0), \
		.detri_lpf = DT_STRING_UNQUOTED_OR(node_id, detri_lpf, NAN), \
		.output_limit = DT_STRING_UNQUOTED_OR(node_id, out_max, 0), \
		.output_offset = DT_STRING_UNQUOTED_OR(node_id, offset, 0), \
		}

		#define PID_CONFIG_DEFINE(inst) \
		@@ -36,7 +36,7 @@ LOG_MODULE_REGISTER(pid_mit, CONFIG_MOTOR_LOG_LEVEL);
		#define PID_INST(inst) \
		PID_CONFIG_DEFINE(inst) \
		PID_DEVICE_DT_DEFINE(DT_DRV_INST(inst), NULL, NULL, NULL, &mit_cfg_##inst, POST_KERNEL, \
		CONFIG_MOTOR_INIT_PRIORITY, NULL);
		90, NULL);

		DT_INST_FOREACH_STATUS_OKAY(PID_INST)

drivers/pid/pid_single.c

+5 −3

Original line number	Diff line number	Diff line
		@@ -22,9 +22,11 @@
		.k_p = DT_STRING_UNQUOTED(node_id, k_p), \
		.integral_limit = DT_STRING_UNQUOTED_OR(node_id, i_max, 0), \
		.output_limit = DT_STRING_UNQUOTED_OR(node_id, out_max, 0), \
		.k_i = DT_STRING_UNQUOTED(node_id, k_i), \
		.k_d = DT_STRING_UNQUOTED(node_id, k_d), \
		.detri_lpf = DT_STRING_UNQUOTED_OR(node_id, detri_lpf, NAN), \
		.k_i = DT_STRING_UNQUOTED_OR(node_id, k_i, NAN), \
		.k_d = DT_STRING_UNQUOTED_OR(node_id, k_d, NAN), \
		.mit = false, \
		.output_offset = DT_STRING_UNQUOTED_OR(node_id, offset, 0), \
		}

		#define PID_CONFIG_DEFINE(inst) \
		@@ -34,7 +36,7 @@
		#define PID_INST(inst) \
		PID_CONFIG_DEFINE(inst) \
		PID_DEVICE_DT_DEFINE(DT_DRV_INST(inst), NULL, NULL, NULL, &pid_config_##inst, POST_KERNEL, \
		CONFIG_MOTOR_INIT_PRIORITY, NULL);
		90, NULL);

		DT_INST_FOREACH_STATUS_OKAY(PID_INST)

dts/bindings/pid/pid,single.yaml

+4 −2

Original line number	Diff line number	Diff line
		@@ -14,11 +14,13 @@ properties:
		required: true
		k_i:
		type: string
		required: true
		k_d:
		type: string
		required: true
		i_max:
		type: string
		out_max:
		type: string
		offset:
		type: string
		detri_lpf:
		type: string
		No newline at end of file

dts/bindings/pid/pid.mit.yaml

+4 −2

Original line number	Diff line number	Diff line
		@@ -14,11 +14,13 @@ properties:
		required: true
		k_i:
		type: string
		required: true
		k_d:
		type: string
		required: true
		i_max:
		type: string
		out_max:
		type: string
		offset:
		type: string
		detri_lpf:
		type: string
		No newline at end of file

include/zephyr/drivers/pid.h

+45 −27

Original line number	Diff line number	Diff line
		@@ -43,6 +43,8 @@ struct pid_config {
		float k_d;
		float integral_limit;
		float output_limit;
		float output_offset;
		float detri_lpf;
		bool mit;
		};

		@@ -74,7 +76,6 @@ STATIC_VOID pid_calc(struct pid_data *data)
		return;
		}
		const struct pid_config *pid_para = dev->config;
		if (!pid_para->mit) {
		if (data->curr == NULL) {
		return;
		}
		@@ -83,8 +84,12 @@ STATIC_VOID pid_calc(struct pid_data *data)
		float kd = pid_para->k_d;
		float err = (data->ref) - (data->curr);
		float deltaT = k_cyc_to_us_near32((data->curr_time) - (data->prev_time));
		if (!float_equal(ki, 0)) {
		data->err_integral += (err * deltaT) / (1000000 * ki);
		if (deltaT == 0) {
		return;
		}
		if (!pid_para->mit) {
		if (!isnanf(ki) && !float_equal(ki, 0)) {
		data->err_integral += (err * deltaT) / ki;
		if (pid_para->integral_limit != 0) {
		if (fabsf(data->err_integral) > pid_para->integral_limit) {
		data->err_integral = data->err_integral > 0
		@@ -93,12 +98,22 @@ STATIC_VOID pid_calc(struct pid_data *data)
		}
		}
		}
		if (!float_equal(kd, 0)) {
		data->err_derivate = kd * err / deltaT;
		if (!isnanf(kd)) {
		if (isnanf(pid_para->detri_lpf)) {
		data->err_derivate =
		kd * ((data->detri_ref) - (data->detri_curr)) / deltaT;
		} else {
		data->err_derivate =
		pid_para->detri_lpf * data->err_derivate +
		(1 - pid_para->detri_lpf) *
		(kd * ((data->detri_ref) - (data->detri_curr)) /
		deltaT);
		}
		}
		// LOG_INF("integral: %d, derivate: %d", to16t(ki * (err * deltaT) / 1000000),
		// to16t(kd * 1000000 * err / deltaT));
		(data->output) = kp (err + data->err_integral + data->err_derivate);
		(data->output) = kp (err + data->err_integral + data->err_derivate) +
		pid_para->output_offset;
		if (pid_para->output_limit != 0 &&
		fabsf(*(data->output)) > pid_para->output_limit) {
		(data->output) = (data->output) > 0 ? pid_para->output_limit
		@@ -106,16 +121,8 @@ STATIC_VOID pid_calc(struct pid_data *data)
		}
		return;
		} else {
		if (data->curr == NULL) {
		return;
		}
		float kp = pid_para->k_p;
		float ki = pid_para->k_i;
		float kd = pid_para->k_d;
		float err = (data->ref) - (data->curr);
		float deltaT = k_cyc_to_us_near32((data->curr_time) - (data->prev_time));
		if (!float_equal(ki, 0)) {
		data->err_integral += (err * deltaT) / (1000000 * ki);
		if (!isnanf(ki) && !float_equal(ki, 0)) {
		data->err_integral += (err * deltaT) / ki;
		if (pid_para->integral_limit != 0) {
		if (fabsf(data->err_integral) > pid_para->integral_limit) {
		data->err_integral = data->err_integral > 0
		@@ -124,12 +131,21 @@ STATIC_VOID pid_calc(struct pid_data *data)
		}
		}
		}
		if (!float_equal(kd, 0)) {
		if (!isnanf(kd)) {
		if (isnanf(pid_para->detri_lpf)) {
		data->err_derivate =
		kd * ((data->detri_ref) - (data->detri_curr)) / deltaT;
		} else {
		data->err_derivate =
		pid_para->detri_lpf * data->err_derivate +
		(1 - pid_para->detri_lpf) *
		(kd * ((data->detri_ref) - (data->detri_curr)) /
		deltaT);
		}
		}

		(data->output) = kp (err + data->err_integral + data->err_derivate);
		(data->output) = kp (err + data->err_integral + data->err_derivate) +
		pid_para->output_offset;
		if (pid_para->output_limit != 0 &&
		fabsf(*(data->output)) > pid_para->output_limit) {
		(data->output) = (data->output) > 0 ? pid_para->output_limit
		@@ -163,6 +179,8 @@ STATIC_VOID pid_reg_time(struct pid_data data, uint32_t curr_cyc, uint32_t *pr
		}
		data->curr_time = curr_cyc;
		data->prev_time = prev_cyc;
		data->err_integral = 0;
		data->err_derivate = 0;
		}

		STATIC_VOID mit_reg_detri_input(struct pid_data data, float detri_curr, float *detri_ref)

Admin message