PID separate data and params (24161c27) · Commits · Wenxi XU / Mambo

drivers/motor/dji/motor_dji.c

+9 −11

Original line number	Diff line number	Diff line
		@@ -36,6 +36,7 @@ const struct device *can_devices[] = {
		.flags = 0, \
		.full = {{false}}, \
		.mask = 0, \
		.thread_sem = &dji_thread_sem, \
		}

		static int frames_id(int tx_id)
		@@ -342,7 +343,7 @@ void can_rx_callback(const struct device can_dev, struct can_frame frame, void
		// Suppose it is 3508/2006
		uint8_t id = (rx_frame.id & 0xF) - 1;
		// If RX_ID does not match, it should be GM6020
		if (ctrl_struct->rx_ids[id] != rx_frame.id && id > 4) {
		if (ctrl_struct->rx_ids[id] != rx_frame.id && id >= 4) {
		id -= 4;
		}
		// It should match, but in case we check again
		@@ -397,7 +398,7 @@ void can_rx_callback(const struct device can_dev, struct can_frame frame, void
		}

		if (full) {
		k_sem_give(&ctrl_struct->thread_sem);
		k_sem_give(ctrl_struct->thread_sem);
		}
		// k_thread_resume(dji_motor_ctrl_thread);
		k_spin_unlock(&motor_data->data_input_lock, key);
		@@ -405,7 +406,6 @@ void can_rx_callback(const struct device can_dev, struct can_frame frame, void
		}

		static const struct can_filter filter20x = {.id = 0x200, .mask = 0x3F0, .flags = 0};
		static const struct can_filter test = {.id = 0x1, .mask = 0x3FF, .flags = 0};

		static void can_tx_callback(const struct device can_dev, int error, void user_data)
		{
		@@ -550,24 +550,22 @@ static void motor_calc(const struct device *dev)

		struct can_frame txframe;

		static struct k_sem dji_thread_sem;

		static void can_send_entry(void arg1, void arg2, void *arg3)
		{
		ARG_UNUSED(arg1);
		ARG_UNUSED(arg2);
		ARG_UNUSED(arg3);
		k_sem_init(&(ctrl_structs[0].thread_sem), 0, 2);
		k_sem_init(&dji_thread_sem, 0, 2);
		struct device *can_dev = NULL;
		for (int i = 0; i < CAN_COUNT; i++) {
		k_sem_init(&tx_queue_sem[i], 3, 3); // 初始化信号量

		can_dev = (struct device *)ctrl_structs[i].can_dev;
		can_start(can_dev);
		if (i != 0) {
		ctrl_structs[i].thread_sem = ctrl_structs[0].thread_sem;
		}
		int err = can_add_rx_filter(can_dev, can_rx_callback, &ctrl_structs[i], &test);
		err = can_add_rx_filter(can_dev, can_rx_callback, &ctrl_structs[i], &test);
		err = can_add_rx_filter(can_dev, can_rx_callback, &ctrl_structs[i], &filter20x);

		int err = can_add_rx_filter(can_dev, can_rx_callback, &ctrl_structs[i], &filter20x);

		if (err < 0) {
		LOG_ERR("Error adding CAN filter (err %d)", err);
		@@ -619,7 +617,7 @@ static void can_send_entry(void arg1, void arg2, void *arg3)
		}
		}
		}
		k_sem_take(&(ctrl_structs[0].thread_sem), K_MSEC(1));
		k_sem_take(&dji_thread_sem, K_MSEC(1));

		int curr_time = k_cycle_get_32();
		for (int i = 0; i < 2; i++) {

drivers/motor/dji/motor_dji.h

+3 −1

Original line number	Diff line number	Diff line
		@@ -30,6 +30,8 @@ typedef uint16_t allmotor_id_t;
		find the rpm in motor_cans->target_rpm[canbus_id] */
		typedef uint16_t motor_id_t;

		static struct k_sem dji_thread_sem;

		struct motor_controller {
		const struct device *can_dev;

		@@ -46,7 +48,7 @@ struct motor_controller {
		uint8_t flags;
		uint8_t mask[5];
		struct device *motor_devs[8];
		struct k_sem thread_sem;
		struct k_sem *thread_sem;
		};

		struct dji_motor_data {

drivers/pid/pid_mit.c

+0 −13

Original line number	Diff line number	Diff line
		@@ -32,19 +32,6 @@ static bool float_equal(float a, float b)
		return fabsf(a - b) < 0.0001f;
		}

		struct pid_mit_data {
		float *ref;
		float *detri_ref;
		float *curr;
		float *detri_curr;
		float err_integral;
		float err_derivate;
		float ratio;
		int32_t *curr_time;
		int32_t *prev_time;
		float *output;
		};

		struct pid_mit_config {
		const struct pid_single_config common;
		};

drivers/pid/pid_single.c

+4 −77

Original line number	Diff line number	Diff line
		@@ -17,88 +17,16 @@

		#define DT_DRV_COMPAT pid_single

		#include "zephyr/logging/log.h"
		LOG_MODULE_REGISTER(pid, 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), \
		.mit = false, \
		}

		#include <limits.h>
		#include <math.h>
		#include <stdint.h>

		static bool float_equal(float a, float b)
		{
		return fabsf(a - b) < 0.0001f;
		}

		static void single_pid_calc(const struct device *pid_dev)
		{
		const struct pid_single_config *pid_para = pid_dev->config;
		struct pid_single_data *pid_data = pid_dev->data;
		if (pid_data->curr == NULL) {
		return;
		}
		float kp = pid_para->k_p;
		float ki = pid_para->k_i;
		float kd = pid_para->k_d;
		float err = (pid_data->ref) - (pid_data->curr);
		float deltaT = k_cyc_to_us_near32((pid_data->curr_time) - (pid_data->prev_time));
		if (!float_equal(ki, 0)) {
		pid_data->err_integral += (err * deltaT) / (1000000 * ki);
		}
		if (!float_equal(kd, 0)) {
		pid_data->err_derivate = kd * err / deltaT;
		}
		// LOG_INF("integral: %d, derivate: %d", to16t(ki * (err * deltaT) / 1000000),
		// to16t(kd * 1000000 * err / deltaT));
		(pid_data->output) = kp (err + pid_data->err_integral + pid_data->err_derivate);
		return;
		}

		static void single_pid_reg_input(const struct device pid_dev, float curr, float *ref)
		{
		struct pid_single_data *pid_data = pid_dev->data;
		pid_data->curr = curr;
		pid_data->ref = ref;
		return;
		}

		static void single_pid_reg_time(const struct device pid_dev, uint32_t curr_time,
		uint32_t *prev_time)
		{
		struct pid_single_data *pid_data = pid_dev->data;
		pid_data->curr_time = curr_time;
		pid_data->prev_time = prev_time;
		return;
		}

		static void single_pid_reg_output(const struct device pid_dev, float output)
		{
		struct pid_single_data *pid_data = pid_dev->data;
		pid_data->output = output;
		return;
		}

		static struct pid_driver_api pid_api_funcs = {
		.pid_calc = single_pid_calc,
		.pid_reg_input = single_pid_reg_input,
		.pid_reg_time = single_pid_reg_time,
		.pid_reg_output = single_pid_reg_output,
		};

		// static uintptr_t pid_addr(const struct device dev, float curr) {
		// struct pid_single_data *data = dev->data;
		// data->curr = curr;
		// return (uintptr_t)data;
		// }

		#define PID_CONFIG_DEFINE(inst) \
		static const struct pid_single_config pid_single_config_##inst = \
		static const struct pid_config pid_single_config_##inst = \
		PID_SINGLE_DT_DRIVER_CONFIG_GET(DT_DRV_INST(inst));

		#define PID_DATA_DEFINE(inst) \
		@@ -113,9 +41,8 @@ static struct pid_driver_api pid_api_funcs = {
		#define PID_INST(inst) \
		PID_CONFIG_DEFINE(inst) \
		PID_DATA_DEFINE(inst) \
		PID_DEVICE_DT_DEFINE(DT_DRV_INST(inst), NULL, NULL, &pid_single_data_##inst, \
		&pid_single_config_##inst, POST_KERNEL, CONFIG_MOTOR_INIT_PRIORITY, \
		&pid_api_funcs);
		PID_DEVICE_DT_DEFINE(DT_DRV_INST(inst), NULL, NULL, NULL, &pid_single_config_##inst, \
		POST_KERNEL, CONFIG_MOTOR_INIT_PRIORITY, &pid_api_funcs);

		DT_INST_FOREACH_STATUS_OKAY(PID_INST)

include/zephyr/drivers/pid.h

+114 −100

Original line number	Diff line number	Diff line
		@@ -6,13 +6,13 @@
		#ifndef PID_H
		#define PID_H

		#include "zephyr/toolchain.h"
		#include <errno.h>
		#include <math.h>
		#include <stdbool.h>
		#include <stdint.h>
		#include <string.h>
		#include <sys/types.h>
		#include <zephyr/device.h>
		#include <zephyr/devicetree.h>

		// TODO: PID, ADRC, FSF, LQR, MPC, etc.

		@@ -26,10 +26,16 @@ extern "C" {
		#define NORMAL_PID 0U
		#define MIT_PID 1U

		struct pid_single_config {
		#define PID_NEW_INSTANCE(node_id, name, ref, curr, curr_time, prev_time, output) \
		struct pid_single_data DT_NODE_FULL_NAME_UNQUOTED(node_id)##name = {.ref = ref,.curr=curr, .pid_dev =DEVICE_DT_GET\
		\
		}

		struct pid_config {
		float k_p;
		float k_i;
		float k_d;
		bool mit;
		};

		struct pid_single_data {
		@@ -38,109 +44,117 @@ struct pid_single_data {
		float err_integral;
		float err_derivate;
		float ratio;
		struct device *pid_dev;
		int32_t *curr_time;
		int32_t *prev_time;
		float *output;
		};

		/**
		* @typedef motor_get_status()
		* @brief Callback API returning motor status
		*
		* @see get_status() for argument descriptions.
		*/
		typedef void (pid_api_calc_t)(const struct device dev);

		// typedef int32_t (pid_api_addr_t)(const struct device dev);

		typedef void (pid_api_reg_input_t)(const struct device dev, float curr, float ref);

		typedef void (pid_api_det_input_t)(const struct device dev, float curr, float ref);

		typedef const struct pid_single_config (pid_get_params_t)(const struct device *dev);

		typedef void (pid_api_reg_time_t)(const struct device dev, uint32_t *curr_time,
		uint32_t *prev_time);

		typedef void (pid_api_reg_output_t)(const struct device dev, float *output);

		/**
		* @brief Servo Motor driver API
		*/
		__subsystem struct pid_driver_api {
		pid_api_calc_t pid_calc;
		pid_api_reg_input_t pid_reg_input;
		pid_api_det_input_t pid_reg_detri;
		pid_api_reg_time_t pid_reg_time;
		pid_api_reg_output_t pid_reg_output;
		pid_get_params_t pid_get_params;
		struct pid_mit_data {
		float *ref;
		float *detri_ref;
		float *curr;
		float *detri_curr;
		float err_integral;
		float err_derivate;
		float ratio;
		int32_t *curr_time;
		int32_t *prev_time;
		float *output;
		};

		__syscall void pid_calc(const struct device *dev);

		static inline void z_impl_pid_calc(const struct device *dev) {
		const struct pid_driver_api api = (const struct pid_driver_api )dev->api;
		static bool float_equal(float a, float b)
		{
		return fabsf(a - b) < 0.0001f;
		}

		if (api->pid_calc != NULL) {
		api->pid_calc(dev);
		static void pid_calc(const struct device dev, void data)
		{
		if (dev == NULL) {
		return;
		}
		const struct pid_config *pid_para = dev->config;
		if (!pid_para->mit) {
		struct pid_single_data *pid_data = data;
		if (pid_data->curr == NULL) {
		return;
		}
		__syscall void pid_reg_input(const struct device dev, float curr, float *ref);

		static inline void z_impl_pid_reg_input(const struct device dev, float curr, float *ref) {
		const struct pid_driver_api api = (const struct pid_driver_api )dev->api;

		if (api->pid_reg_input != NULL) {
		api->pid_reg_input(dev, curr, ref);
		float kp = pid_para->k_p;
		float ki = pid_para->k_i;
		float kd = pid_para->k_d;
		float err = (pid_data->ref) - (pid_data->curr);
		float deltaT = k_cyc_to_us_near32((pid_data->curr_time) - (pid_data->prev_time));
		if (!float_equal(ki, 0)) {
		pid_data->err_integral += (err * deltaT) / (1000000 * ki);
		}
		if (!float_equal(kd, 0)) {
		pid_data->err_derivate = kd * err / deltaT;
		}

		__syscall void pid_reg_det_input(const struct device dev, float curr, float *ref);

		static inline void z_impl_pid_reg_det_input(const struct device dev, float curr, float *ref) {
		const struct pid_driver_api api = (const struct pid_driver_api )dev->api;

		if (api->pid_reg_detri != NULL) {
		api->pid_reg_detri(dev, curr, ref);
		// LOG_INF("integral: %d, derivate: %d", to16t(ki * (err * deltaT) / 1000000),
		// to16t(kd * 1000000 * err / deltaT));
		(pid_data->output) = kp (err + pid_data->err_integral + pid_data->err_derivate);

		} else {
		struct pid_mit_data *pid_data = data;
		if (pid_data->curr == NULL) {
		return;
		}
		float kp = pid_para->k_p;
		float ki = pid_para->k_i;
		float kd = pid_para->k_d;
		float err = (pid_data->ref) - (pid_data->curr);
		float deltaT = k_cyc_to_us_near32((pid_data->curr_time) - (pid_data->prev_time));
		if (!float_equal(ki, 0)) {
		pid_data->err_integral += (err * deltaT) / (1000000 * ki);
		}
		if (!float_equal(kd, 0)) {
		pid_data->err_derivate =
		kd * ((pid_data->detri_ref) - (pid_data->detri_curr)) / deltaT;
		}

		__syscall void pid_reg_time(const struct device dev, uint32_t curr_time, uint32_t *prev_time);

		static inline void z_impl_pid_reg_time(const struct device dev, uint32_t curr_time,
		uint32_t *prev_time) {
		const struct pid_driver_api api = (const struct pid_driver_api )dev->api;

		if (api->pid_reg_time != NULL) {
		api->pid_reg_time(dev, curr_time, prev_time);
		(pid_data->output) = kp (err + pid_data->err_integral + pid_data->err_derivate);
		return;
		}
		}

		__syscall void pid_reg_output(const struct device dev, float output);

		static inline void z_impl_pid_reg_output(const struct device dev, float output) {
		const struct pid_driver_api api = (const struct pid_driver_api )dev->api;

		if (api->pid_reg_output != NULL) {
		api->pid_reg_output(dev, output);
		static void pid_register(struct pid_single_data data, float curr, float ref, uint32_t curr_cyc,
		uint32_t prev_cyc, float output)
		{
		if (data == NULL) {
		return;
		}
		data->curr = curr;
		data->ref = ref;
		data->curr_time = curr_cyc;
		data->prev_time = prev_cyc;
		data->output = output;
		}

		__syscall const struct pid_single_config pid_get_params(const struct device dev);

		static inline const struct pid_single_config z_impl_pid_get_params(const struct device dev) {
		const struct pid_driver_api api = (const struct pid_driver_api )dev->api;

		if (api->pid_get_params != NULL) {
		return api->pid_get_params(dev);
		static void mit_register(struct pid_mit_data data, float curr, float ref, float detri_curr,
		float detri_ref, uint32_t curr_cyc, uint32_t prev_cyc, float output)
		{
		if (data == NULL) {
		return;
		}
		data->curr = curr;
		data->ref = ref;
		data->detri_curr = detri_curr;
		data->detri_ref = detri_ref;
		data->curr_time = curr_cyc;
		data->prev_time = prev_cyc;
		data->output = output;
		}

		static const struct pid_single_config pid_get_params(const struct device dev)
		{
		if (dev == NULL) {
		return NULL;
		}
		return dev->config;
		}

		#ifdef __cplusplus
		}
		#endif

		#include <zephyr/syscalls/pid.h>

		#endif // PID_H
		No newline at end of file

Admin message