Trying to apply slip detect in dji motor driver (f781d69c) · Commits · Wenxi XU / Mambo

drivers/motor/dji/motor_dji.c

+23 −5

Original line number	Diff line number	Diff line
		@@ -6,6 +6,7 @@
		#include "motor_dji.h"
		#include "zephyr/device.h"
		#include "zephyr/devicetree.h"
		#include "zephyr/drivers/motor.h"
		#include "zephyr/spinlock.h"
		#include "zephyr/toolchain.h"
		#include <string.h>
		@@ -274,7 +275,7 @@ int dji_init(const struct device *dev)
		/*
		Each motor find their own controller and write their own data,
		which cannot be done at the first init because the other motors have not
		been instantiated.
		been initialized.
		*/
		if (dev) {
		const struct dji_motor_config *cfg = dev->config;
		@@ -289,7 +290,7 @@ int dji_init(const struct device *dev)
		data->online = true;
		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) {
		if (cfg->common.pid_datas[i] == NULL) {
		if (i > 0) {
		pid_reg_output(cfg->common.pid_datas[i - 1],
		&data->target_torque);
		@@ -314,7 +315,7 @@ int dji_init(const struct device *dev)
		&data->target_torque);
		}
		} else {
		LOG_ERR("Unsupported motor mode");
		LOG_ERR("Unsupported motor mode: %s", cfg->common.capabilities[i]);
		return -1;
		}
		pid_reg_time(cfg->common.pid_datas[i], &(data->curr_time),
		@@ -521,11 +522,28 @@ static void motor_calc(const struct device *dev)
		// Add up to avoid circular overflow
		proceed_delta_degree(dev_temp);

		data_temp->common.rpm = data_temp->RAWrpm * convert[data_temp->convert_num][SPEED2RPM] /
		if (data_temp->common.sample_cnt < 500) {
		data_temp->common.sample_cnt++;
		}
		float rpm = data_temp->RAWrpm * convert[data_temp->convert_num][SPEED2RPM] /
		config_temp->gear_ratio;
		if (data_temp->common.sample_cnt == 1) {
		data_temp->common.alpha = rpm - data_temp->common.rpm;
		}
		data_temp->common.alpha = (1 - Alpha_alpha) * data_temp->common.alpha +
		Alpha_alpha * (rpm - data_temp->common.rpm);
		data_temp->common.rpm = rpm;
		data_temp->common.torque = data_temp->RAWcurrent *
		convert[data_temp->convert_num][CURRENT2TORQUE] *
		config_temp->gear_ratio;

		if (RLS_CUSUM_update(&data_temp->common)) {
		// LOG_ERR("Wheel is slipping");
		if (data_temp->common.slip_cb != NULL) {
		data_temp->common.slip_cb(dev_temp);
		}
		}

		for (int i = data_temp->current_mode_index;
		i < SIZE_OF_ARRAY(config_temp->common.capabilities); i++) {
		if (config_temp->common.pid_datas[i]->pid_dev == NULL) {

include/zephyr/drivers/motor.h

+61 −0

Original line number	Diff line number	Diff line
		@@ -41,6 +41,9 @@ extern "C" {

		#define RPM2RADPS(rpm) ((rpm) * 0.104719755f)
		#define RADPS2RPM(radps) ((radps) * 9.54929659f)

		#define RLS_alpha 0.9975f
		#define Alpha_alpha 0.9975f
		/**
		* @brief 电机工作模式枚举
		*
		@@ -81,9 +84,12 @@ struct motor_driver_config {
		struct pid_data *pid_datas[4];
		};

		typedef float (motor_slip_cb_t)(const struct device dev);

		struct motor_driver_data {
		float angle;
		float rpm;
		float alpha;
		float torque;
		float temperature;
		int round_cnt;
		@@ -91,8 +97,62 @@ struct motor_driver_data {
		float speed_limit[2];
		float torque_limit[2];

		float sum_T;
		float sum_alpha;
		float sum_Talpha;
		float sum_alpha_square;
		float RLS_k;
		float RLS_b;
		float cusum_accumulator;
		float cusum_max;
		uint16_t sample_cnt;

		motor_slip_cb_t slip_cb;

		enum motor_mode mode;
		};

		static inline bool RLS_CUSUM_update(struct motor_driver_data *data)
		{
		float torque = data->torque;
		float alpha = data->alpha;
		if (data->torque < 0) {
		torque = -data->torque;
		alpha = -data->alpha;
		}
		// 更新指数加权统计量
		data->sum_T = RLS_alpha * data->sum_T + torque;
		data->sum_alpha = RLS_alpha * data->sum_alpha + alpha;
		data->sum_Talpha = RLS_alpha * data->sum_Talpha + torque * alpha;
		data->sum_alpha_square = RLS_alpha * data->sum_alpha_square + alpha * alpha;

		// 计算线性回归参数
		float denominator =
		data->sum_alpha_square -
		(data->sum_alpha * data->sum_alpha) / (1 / (1 - alpha)); // 等效样本数=1/(1-alpha)
		if (fabsf(denominator) > 1e-6f) { // 避免除零
		data->RLS_k =
		(data->sum_Talpha - data->sum_T * data->sum_alpha / (1 / (1 - alpha))) /
		denominator;
		data->RLS_b = (data->sum_T - data->RLS_k * data->sum_alpha) / (1 / (1 - alpha));
		}

		// 计算残差
		float residual = torque - (data->RLS_k * alpha + data->RLS_b);

		// 更新CUSUM统计量
		data->cusum_accumulator += residual;
		data->cusum_accumulator = fmaxf(data->cusum_accumulator, 0); // 仅关注正向累积
		data->cusum_max = fmaxf(data->cusum_max, data->cusum_accumulator);

		// 触发条件（阈值需实验确定）
		if (data->cusum_max > 5.0f) { // 示例阈值
		data->cusum_accumulator = 0; // 重置检测器
		data->cusum_max = 0;
		return true;
		}
		return false;
		}
		/**
		* @typedef motor_api_stat_speed_t
		* @brief 获取电机当前速度的回调函数
		@@ -398,6 +458,7 @@ static inline void z_impl_motor_limit_torque(const struct device *dev, float max
		#define MOTOR_DT_DRIVER_DATA_GET(node_id) \
		{ \
		.angle = 0, \
		.alpha = 0, \
		.rpm = 0, \
		.torque = 0, \
		.temperature = 0, \

Admin message