bugs in dji motor driver fixed (de2e2021) · Commits · Wenxi XU / Mambo

drivers/pid/pid_mit.c

+2 −2

Original line number	Diff line number	Diff line
		@@ -25,8 +25,8 @@ LOG_MODULE_REGISTER(pid_mit, CONFIG_MOTOR_LOG_LEVEL);
		.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(node_id, i_max), \
		.output_limit = DT_STRING_UNQUOTED(node_id, out_max), \
		.integral_limit = DT_STRING_UNQUOTED_OR(node_id, i_max, 0), \
		.output_limit = DT_STRING_UNQUOTED_OR(node_id, out_max, 0), \
		}

		#define PID_CONFIG_DEFINE(inst) \

drivers/pid/pid_single.c

+2 −2

Original line number	Diff line number	Diff line
		@@ -20,8 +20,8 @@
		#define PID_SINGLE_DT_DRIVER_CONFIG_GET(node_id) \
		{ \
		.k_p = DT_STRING_UNQUOTED(node_id, k_p), \
		.integral_limit = DT_STRING_UNQUOTED(node_id, i_max), \
		.output_limit = DT_STRING_UNQUOTED(node_id, out_max), \
		.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), \
		.mit = false, \

include/zephyr/drivers/pid.h

+25 −1

Original line number	Diff line number	Diff line
		@@ -85,6 +85,13 @@ STATIC_VOID pid_calc(struct pid_data *data)
		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 (pid_para->integral_limit != 0) {
		if (fabsf(data->err_integral) > pid_para->integral_limit) {
		data->err_integral = data->err_integral > 0
		? pid_para->integral_limit
		: -pid_para->integral_limit;
		}
		}
		}
		if (!float_equal(kd, 0)) {
		data->err_derivate = kd * err / deltaT;
		@@ -92,7 +99,12 @@ STATIC_VOID pid_calc(struct pid_data *data)
		// 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);

		if (pid_para->output_limit != 0 &&
		fabsf(*(data->output)) > pid_para->output_limit) {
		(data->output) = (data->output) > 0 ? pid_para->output_limit
		: -pid_para->output_limit;
		}
		return;
		} else {
		if (data->curr == NULL) {
		return;
		@@ -104,6 +116,13 @@ STATIC_VOID pid_calc(struct pid_data *data)
		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 (pid_para->integral_limit != 0) {
		if (fabsf(data->err_integral) > pid_para->integral_limit) {
		data->err_integral = data->err_integral > 0
		? pid_para->integral_limit
		: -pid_para->integral_limit;
		}
		}
		}
		if (!float_equal(kd, 0)) {
		data->err_derivate =
		@@ -111,6 +130,11 @@ STATIC_VOID pid_calc(struct pid_data *data)
		}

		(data->output) = kp (err + data->err_integral + data->err_derivate);
		if (pid_para->output_limit != 0 &&
		fabsf(*(data->output)) > pid_para->output_limit) {
		(data->output) = (data->output) > 0 ? pid_para->output_limit
		: -pid_para->output_limit;
		}
		return;
		}
		}

Admin message