DJI driver switched to work_queue; DM driver is not yet working with new PID Driver. (62193dfb) · Commits · Wenxi XU / Mambo

.idea/workspace.xml

0 → 100644

+96 −0

Original line number	Diff line number	Diff line
		<?xml version="1.0" encoding="UTF-8"?>
		<project version="4">
		<component name="AutoImportSettings">
		<option name="autoReloadType" value="SELECTIVE" />
		</component>
		<component name="CMakePresetLoader"><![CDATA[{
		"useNewFormat": true
		}]]></component>
		<component name="CMakeProjectFlavorService">
		<option name="flavorId" value="CMakeWestFlavor" />
		</component>
		<component name="CMakeReloadState">
		<option name="reloaded" value="true" />
		</component>
		<component name="CMakeRunConfigurationManager">
		<generated />
		</component>
		<component name="CMakeSettings">
		<configurations>
		<configuration PROFILE_NAME="West" ENABLED="true" GENERATION_DIR="build" CONFIG_NAME="Debug" />
		</configurations>
		</component>
		<component name="ChangeListManager">
		<list default="true" id="fea9a754-35ec-49eb-b24a-646dbc68b7b3" name="更改" comment="" />
		<option name="SHOW_DIALOG" value="false" />
		<option name="HIGHLIGHT_CONFLICTS" value="true" />
		<option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
		<option name="LAST_RESOLUTION" value="IGNORE" />
		</component>
		<component name="ClangdSettings">
		<option name="formatViaClangd" value="false" />
		</component>
		<component name="ProjectApplicationVersion">
		<option name="ide" value="CLion" />
		<option name="majorVersion" value="2024" />
		<option name="minorVersion" value="2.2" />
		<option name="productBranch" value="Classic" />
		</component>
		<component name="ProjectColorInfo"><![CDATA[{
		"customColor": "",
		"associatedIndex": 8
		}]]></component>
		<component name="ProjectId" id="2mnepAanzZuacMPDtTnbIojKRg2" />
		<component name="ProjectViewState">
		<option name="hideEmptyMiddlePackages" value="true" />
		<option name="showLibraryContents" value="true" />
		</component>
		<component name="PropertiesComponent"><![CDATA[{
		"keyToString": {
		"RunOnceActivity.CMakeWestChangeProjectRootActivity": "true",
		"RunOnceActivity.RadMigrateCodeStyle": "true",
		"RunOnceActivity.ShowReadmeOnStart": "true",
		"RunOnceActivity.cidr.known.project.marker": "true",
		"RunOnceActivity.readMode.enableVisualFormatting": "true",
		"RunOnceActivity.west.python.interpreter.configured": "true",
		"cf.first.check.clang-format": "false",
		"cidr.known.project.marker": "true",
		"node.js.detected.package.eslint": "true",
		"node.js.detected.package.tslint": "true",
		"node.js.selected.package.eslint": "(autodetect)",
		"node.js.selected.package.tslint": "(autodetect)",
		"nodejs_package_manager_path": "npm",
		"settings.editor.selected.configurable": "WestSettings",
		"vue.rearranger.settings.migration": "true"
		}
		}]]></component>
		<component name="RunManager">
		<configuration default="true" type="CLionExternalRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="false" PASS_PARENT_ENVS_2="true">
		<method v="2">
		<option name="CLION.EXTERNAL.BUILD" enabled="true" />
		</method>
		</configuration>
		<configuration name="West" type="WestRunConfiguration" factoryName="WestRunConfiguration" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="motor" TARGET_NAME="West" CONFIG_NAME="West">
		<method v="2" />
		</configuration>
		</component>
		<component name="SpellCheckerSettings" RuntimeDictionaries="0" Folders="0" CustomDictionaries="0" DefaultDictionary="应用程序级" UseSingleDictionary="true" transferred="true" />
		<component name="TaskManager">
		<task active="true" id="Default" summary="默认任务">
		<changelist id="fea9a754-35ec-49eb-b24a-646dbc68b7b3" name="更改" comment="" />
		<created>1727716948230</created>
		<option name="number" value="Default" />
		<option name="presentableId" value="Default" />
		<updated>1727716948230</updated>
		<workItem from="1727716949668" duration="627000" />
		</task>
		<servers />
		</component>
		<component name="TypeScriptGeneratedFilesManager">
		<option name="version" value="3" />
		</component>
		<component name="XSLT-Support.FileAssociations.UIState">
		<expand />
		<select />
		</component>
		</project>
		No newline at end of file

CMakeLists.txt

+3 −0

Original line number	Diff line number	Diff line
		@@ -3,6 +3,9 @@ set(ENV{ZEPHYR_BASE} "${CMAKE_CURRENT_SOURCE_DIR}/../zephyr")

		# 导出编译命令
		set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
		set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -save-temps")

		set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -save-temps")

		set(CMAKE_C_STANDARD 11)
		set(CMAKE_CXX_STANDARD 14)

drivers/motor/dji/dji_macros.h

+1 −0

Original line number	Diff line number	Diff line
		@@ -87,6 +87,7 @@
		CONFIG_MOTOR_INIT_PRIORITY, &motor_api_funcs);

		#define DMOTOR_INST(inst) \
		MOTOR_DT_DRIVER_PID_DEFINE(DT_DRV_INST(inst)) \
		DMOTOR_CONFIG_INST(inst) \
		DMOTOR_DATA_INST(inst) \
		DMOTOR_DEFINE_INST(inst)

drivers/motor/dji/motor_dji.c

+96 −90

Original line number	Diff line number	Diff line
		@@ -4,7 +4,6 @@
		* SPDX-License-Identifier: Apache-2.0
		*/
		#include "motor_dji.h"
		#include "syscalls/pid.h"
		#include "zephyr/device.h"
		#include "zephyr/devicetree.h"
		#include "zephyr/spinlock.h"
		@@ -32,11 +31,8 @@ const struct device *can_devices[] = {

		#define CTRL_STRUCT_DATA(node_id) \
		{ \
		.can_dev = DT_GET_CANPHY_BY_BUS(node_id), \
		.flags = 0, \
		.full = {{false}}, \
		.can_dev = DT_GET_CANPHY_BY_BUS(node_id), .flags = 0, .full = {{false}}, \
		.mask = 0, \
		.thread_sem = &dji_thread_sem, \
		}

		static int frames_id(int tx_id)
		@@ -100,13 +96,9 @@ static int16_t to16t(float value)
		}
		}

		static void can_send_entry(void arg1, void arg2, void *arg3);
		struct motor_controller ctrl_structs[CAN_COUNT] = {
		DT_FOREACH_CHILD_STATUS_OKAY_SEP(CAN_BUS_PATH, CTRL_STRUCT_DATA, (, ))};

		K_THREAD_DEFINE(dji_motor_ctrl_thread, CAN_SEND_STACK_SIZE, can_send_entry, NULL, NULL, NULL,
		CAN_SEND_PRIORITY, 0, 0);

		static inline motor_id_t canbus_id(const struct device *dev)
		{
		for (int i = 0; i < MOTOR_COUNT; i++) {
		@@ -149,13 +141,12 @@ int dji_set_speed(const struct device *dev, float speed_rpm)
		}

		data->target_rpm = speed_rpm;
		for (int i = 0; i < sizeof(cfg->common.controller) / sizeof(cfg->common.controller[0]);
		i++) {
		if (cfg->common.controller[i] == NULL) {
		for (int i = 0; i < sizeof(cfg->common.pid_datas) / sizeof(cfg->common.pid_datas[0]); i++) {
		if (cfg->common.pid_datas[i]->pid_dev == NULL) {
		break;
		}
		if (strcmp(cfg->common.capabilities[i], "speed") == 0) {
		pid_calc(cfg->common.controller[i]);
		pid_calc(cfg->common.pid_datas[i]);
		data->current_mode_index = i;
		}
		}
		@@ -178,12 +169,12 @@ int dji_set_angle(const struct device *dev, float angle)

		data->target_angle = angle;

		for (int i = 0; i < SIZE_OF_ARRAY(cfg->common.controller); i++) {
		if (cfg->common.controller[i] == NULL) {
		for (int i = 0; i < SIZE_OF_ARRAY(cfg->common.pid_datas); i++) {
		if (cfg->common.pid_datas[i]->pid_dev == NULL) {
		break;
		}
		if (strcmp(cfg->common.capabilities[i], "angle") == 0) {
		pid_calc(cfg->common.controller[i]);
		pid_calc(cfg->common.pid_datas[i]);
		data->current_mode_index = i;
		}
		}
		@@ -203,13 +194,13 @@ int dji_set_torque(const struct device *dev, float torque)
		}

		data->target_torque = torque;
		for (int i = 0; i < SIZE_OF_ARRAY(cfg->common.controller); i++) {
		if (cfg->common.controller[i] == NULL) {
		for (int i = 0; i < SIZE_OF_ARRAY(cfg->common.pid_datas); i++) {
		if (cfg->common.pid_datas[i]->pid_dev == NULL) {
		data->current_mode_index = i + 1;
		break;
		}
		if (strcmp(cfg->common.capabilities[i], "torque") == 0) {
		pid_calc(cfg->common.controller[i]);
		pid_calc(cfg->common.pid_datas[i]);
		data->current_mode_index = i;
		}
		}
		@@ -272,44 +263,47 @@ int dji_init(const struct device *dev)
		*/
		if (dev) {
		const struct dji_motor_config *cfg = dev->config;
		struct dji_motor_data data = (struct dji_motor_data )dev->data;
		struct dji_motor_data *data = dev->data;
		uint8_t frame_id = frames_id(cfg->common.tx_id);
		uint8_t id = motor_id(dev);
		data->ctrl_struct->mask[frame_id] \|= id >= 4 ? 0xF0 : 0x0F;
		data->ctrl_struct->mask[frame_id] ^= 1 << id;
		data->ctrl_struct->rx_ids[id] = cfg->common.rx_id;

		data->ctrl_struct->full_handle.handler = dji_tx_handler;

		data->online = true;
		for (int i = 0;
		i < sizeof(cfg->common.controller) / sizeof(cfg->common.controller[0]); i++) {
		if (cfg->common.controller[i] == NULL) {
		i < sizeof(cfg->common.pid_datas) / sizeof(cfg->common.pid_datas[0]); i++) {
		if (cfg->common.pid_datas[i]->pid_dev == NULL) {
		if (i > 0) {
		pid_reg_output(cfg->common.controller[i - 1],
		pid_reg_output(cfg->common.pid_datas[i - 1],
		&data->target_torque);
		}
		break;
		}
		if (strcmp(cfg->common.capabilities[i], "speed") == 0) {
		pid_reg_input(cfg->common.controller[i], &data->common.rpm,
		pid_reg_input(cfg->common.pid_datas[i], &data->common.rpm,
		&data->target_rpm);
		if (i > 0) {
		pid_reg_output(cfg->common.controller[i - 1],
		pid_reg_output(cfg->common.pid_datas[i - 1],
		&data->target_rpm);
		}
		} else if (strcmp(cfg->common.capabilities[i], "angle") == 0) {
		pid_reg_input(cfg->common.controller[i], &data->pid_angle_input,
		pid_reg_input(cfg->common.pid_datas[i], &data->pid_angle_input,
		&data->pid_ref_input);
		} else if (strcmp(cfg->common.capabilities[i], "torque") == 0) {
		pid_reg_input(cfg->common.controller[i], &data->common.torque,
		pid_reg_input(cfg->common.pid_datas[i], &data->common.torque,
		&data->target_torque);
		if (i > 0) {
		pid_reg_output(cfg->common.controller[i - 1],
		pid_reg_output(cfg->common.pid_datas[i - 1],
		&data->target_torque);
		}
		} else {
		LOG_ERR("Unsupported motor mode");
		return -1;
		}
		pid_reg_time(cfg->common.controller[i], &(data->curr_time),
		pid_reg_time(cfg->common.pid_datas[i], &(data->curr_time),
		&(data->prev_time));
		}
		data->current_mode_index = 0;
		@@ -330,7 +324,11 @@ int dji_init(const struct device *dev)
		if (!device_is_ready(cfg->common.phy)) {
		return -1;
		}
		k_thread_start(dji_motor_ctrl_thread);
		if (dji_miss_handle_timer.expiry_fn == NULL) {
		dji_miss_handle_timer.expiry_fn = dji_miss_isr_handler;
		k_timer_init(&dji_miss_handle_timer, dji_miss_isr_handler, NULL);
		k_timer_start(&dji_miss_handle_timer, K_NO_WAIT, K_MSEC(2));
		}
		}
		return 0;
		}
		@@ -398,7 +396,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_work_submit_to_queue(&dji_work_queue, &ctrl_struct[bus_id].full_handle);
		}
		// k_thread_resume(dji_motor_ctrl_thread);
		k_spin_unlock(&motor_data->data_input_lock, key);
		@@ -513,8 +511,8 @@ static void motor_calc(const struct device *dev)
		// From the current mode index to the end of the controller array
		bool torque_proceeded = false;
		for (int i = data_temp->current_mode_index;
		i < SIZE_OF_ARRAY(config_temp->common.controller); i++) {
		if (config_temp->common.controller[i] == NULL) {
		i < SIZE_OF_ARRAY(config_temp->common.capabilities); i++) {
		if (config_temp->common.pid_datas[i]->pid_dev == NULL) {
		if (torque_proceeded) {
		break;
		}
		@@ -529,7 +527,7 @@ static void motor_calc(const struct device *dev)
		break;
		}

		pid_calc(config_temp->common.controller[i]);
		pid_calc(config_temp->common.pid_datas[i]);

		if (strcmp(config_temp->common.capabilities[i], "angle") == 0) {
		if (data_temp->target_rpm > data_temp->common.speed_limit[1]) {
		@@ -552,15 +550,32 @@ struct can_frame txframe;

		static struct k_sem dji_thread_sem;

		static void can_send_entry(void arg1, void arg2, void *arg3)
		void dji_miss_isr_handler(struct k_timer *dummy)
		{
		ARG_UNUSED(dummy);
		k_work_submit_to_queue(&dji_work_queue, &dji_miss_handle);
		}

		void dji_miss_handler(struct k_work *work)
		{
		ARG_UNUSED(work);
		int curr_time = k_cycle_get_32();
		for (int i = 0; i < CAN_COUNT; i++) {
		for (int j = 0; j < 8; j++) {
		if (ctrl_structs[i].motor_devs[j]) {
		dji_timeout_handle(ctrl_structs[i].motor_devs[i], curr_time,
		&ctrl_structs[i]);
		}
		}
		}
		}

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

		can_dev = (struct device *)ctrl_structs[i].can_dev;
		can_start(can_dev);
		@@ -574,61 +589,52 @@ static void can_send_entry(void arg1, void arg2, void *arg3)
		// mode and does not have an independent filter. If that is the issue, you
		// can ignore that.
		}
		int err = 0;
		while (1) {
		for (int8_t i = 0; i < CAN_COUNT; i++) {
		for (int j = 0; j < 5; j++) {
		if (ctrl_structs[i].full[j] == 1) {
		uint8_t id_temp = ctrl_structs[i].mapping[0][0];
		dji_miss_handle_timer.expiry_fn = dji_miss_isr_handler;
		}

		void dji_tx_handler(struct k_work *work)
		{
		struct motor_controller *ctrl_struct =
		CONTAINER_OF(work, struct motor_controller, full_handle);

		for (int i = 0; i < 5; i++) { // For each frame
		if (ctrl_struct->full[i]) {
		uint8_t id_temp = ctrl_struct->mapping[0][0];
		uint8_t data[8] = {0};
		bool packed = false;
		for (int k = 0; k < 4; k++) {
		id_temp = ctrl_structs[i].mapping[j][k];
		for (int j = 0; j < 4; j++) {
		id_temp = ctrl_struct->mapping[i][j];
		if (id_temp < 0) {
		continue;
		}
		const struct device *dev = ctrl_struct->motor_devs[id_temp];
		struct dji_motor_data *data_temp = dev->data;
		// Check if recieved just now.
		if (id_temp < 8 &&
		(ctrl_structs[i].flags & (1 << id_temp))) {
		motor_calc(ctrl_structs[i]
		.motor_devs[id_temp]);
		can_pack_add(
		data,
		ctrl_structs[i].motor_devs[id_temp],
		k);
		ctrl_structs[i].flags ^= 1 << id_temp;
		((ctrl_struct->flags & (1 << id_temp)) \|\| !data_temp->online)) {
		motor_calc(ctrl_struct->motor_devs[id_temp]);
		can_pack_add(data, ctrl_struct->motor_devs[id_temp], j);
		ctrl_struct->flags ^= 1 << id_temp;
		packed = true;
		}
		}
		if (packed) {
		txframe.id = txframe_id(j);
		txframe.id = txframe_id(i);
		txframe.dlc = 8;
		txframe.flags = 0;
		memcpy(txframe.data, data, sizeof(data));
		can_dev = (struct device *)ctrl_structs[i].can_dev;
		err = k_sem_take(&tx_queue_sem[i], K_NO_WAIT);
		const struct device *can_dev = ctrl_struct->can_dev;
		struct k_sem tx_queue_sem = ctrl_struct->tx_queue_sem;
		int err = k_sem_take(&tx_queue_sem, K_NO_WAIT);
		if (err == 0) {
		err = can_send(can_dev, &txframe, K_NO_WAIT,
		can_tx_callback,
		&tx_queue_sem[i]);
		can_tx_callback, &tx_queue_sem);
		}
		if (err != 0 && err != -EAGAIN && err != -EBUSY) {
		LOG_ERR("Error sending CAN frame (err %d)",
		err);
		}
		}
		}
		}
		}
		k_sem_take(&dji_thread_sem, K_MSEC(1));

		int curr_time = k_cycle_get_32();
		for (int i = 0; i < 2; i++) {
		for (int j = 0; j < 8; j++) {
		if (ctrl_structs[i].motor_devs[j]) {
		dji_timeout_handle(ctrl_structs[i].motor_devs[i], curr_time,
		&ctrl_structs[i]);
		LOG_ERR("Error sending CAN frame (err %d)", err);
		}
		}
		}
		// k_thread_suspend(dji_motor_ctrl_thread);
		}
		}

drivers/motor/dji/motor_dji.h

+20 −3

Original line number	Diff line number	Diff line
		@@ -11,6 +11,7 @@
		#include <stdint.h>
		#include "dji_ratios.h"
		#include "dji_macros.h"
		#include "zephyr/kernel.h"
		#include "zephyr/spinlock.h"
		#include <zephyr/device.h>
		#include <zephyr/drivers/can.h>
		@@ -31,6 +32,7 @@ typedef uint16_t allmotor_id_t;
		typedef uint16_t motor_id_t;

		static struct k_sem dji_thread_sem;
		struct k_work_q dji_work_queue;

		struct motor_controller {
		const struct device *can_dev;
		@@ -48,7 +50,10 @@ struct motor_controller {
		uint8_t flags;
		uint8_t mask[5];
		struct device *motor_devs[8];
		struct k_sem *thread_sem;

		struct k_work full_handle;

		struct k_sem tx_queue_sem;
		};

		struct dji_motor_data {
		@@ -101,8 +106,6 @@ struct dji_motor_config {
		// 全局变量声明
		extern struct motor_controller ctrl_structs[];

		struct k_sem tx_queue_sem[CAN_COUNT];

		// 函数声明
		void can_rx_callback(const struct device can_dev, struct can_frame frame, void *user_data);

		@@ -118,6 +121,20 @@ float dji_get_torque(const struct device *dev);
		int dji_init(const struct device *dev);
		void dji_control(const struct device *dev, enum motor_cmd cmd);

		void dji_tx_handler(struct k_work *work);
		void dji_miss_handler(struct k_work *work);

		void dji_miss_isr_handler(struct k_timer *dummy);

		void dji_init_handler(struct k_work *work);

		K_THREAD_STACK_DEFINE(dm_work_queue_stack, CAN_SEND_STACK_SIZE);

		K_WORK_DEFINE(dji_miss_handle, dji_miss_handler);
		K_WORK_DEFINE(dji_init_handle, dji_init_handler);

		K_TIMER_DEFINE(dji_miss_handle_timer, dji_miss_isr_handler, NULL);

		static const struct motor_driver_api motor_api_funcs = {
		.motor_get_speed = dji_get_speed,
		.motor_get_torque = dji_get_torque,

Admin message