Commit dd27f870 authored by Wenxi Xu's avatar Wenxi Xu
Browse files

小改一下去掉了warning

parent 52d37784

include/zephyr/drivers/chassis.h

+3 −3
Original line number Diff line number Diff line
@@ -138,7 +138,7 @@ static inline chassis_status_t *z_impl_chassis_get_status(const struct device *d 
	return NULL;

}



static void chassis_update_sensor(const struct device *dev, struct pos_data *pos_data)

__unused static void chassis_update_sensor(const struct device *dev, struct pos_data *pos_data)

{

	chassis_data_t *data = dev->data;

	data->prevTime = data->currTime;
@@ -149,10 +149,10 @@ static void chassis_update_sensor(const struct device *dev, struct pos_data *pos 
						: data->chassis_sensor_data.Yaw;

}



static void chassis_set_enabled(const struct device *dev, bool enabled)

__unused static void chassis_set_enabled(const struct device *dev, bool enabled)

{

	chassis_data_t *data = dev->data;

	chassis_cfg_t *cfg = dev->config;

	chassis_cfg_t *cfg = (chassis_cfg_t *)dev->config;

	data->enabled = enabled;

	if (!enabled) {

		for (int i = 0; i < CHASSIS_WHEEL_COUNT; i++) {

include/zephyr/drivers/motor.h

+3 −57
Original line number Diff line number Diff line
@@ -26,14 +26,12 @@ 
#include <sys/_intsup.h>

#include <zephyr/kernel.h>

#include <zephyr/kernel/thread.h>

#include <errno.h>

#include <zephyr/sys/util.h>



#include <stdint.h>

#include <zephyr/device.h>

#include <zephyr/devicetree.h>

#include <zephyr/logging/log.h>

#include <string.h>



#ifdef __cplusplus

extern "C" {
@@ -42,8 +40,6 @@ 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 电机工作模式枚举

 *
@@ -87,6 +83,8 @@ struct motor_driver_config { 
typedef float (*motor_slip_cb_t)(const struct device *dev);



struct motor_driver_data {

	void *spec_data;



	float angle;

	float rpm;

	float alpha;
@@ -97,62 +95,11 @@ 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 获取电机当前速度的回调函数
@@ -240,7 +187,7 @@ typedef void (*motor_limit_torque_t)(const struct device *dev, float max_torque, 
__subsystem struct motor_driver_api {

	motor_api_stat_speed_t motor_get_speed;

	motor_api_stat_torque_t motor_get_torque;

	motor_api_stat_torque_t motor_get_angle;

	motor_api_stat_angle_t motor_get_angle;

	motor_api_speed_t motor_set_speed;

	motor_api_torque_t motor_set_torque;

	motor_api_angle_t motor_set_angle;
@@ -458,7 +405,6 @@ 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,                                                                  \

include/zephyr/drivers/pid.h

+16 −22
Original line number Diff line number Diff line
@@ -29,7 +29,7 @@ extern "C" { 
#define STATIC_VOID __attribute__((unused)) static void

#define STATIC      __attribute__((unused)) static



#define PID_INS_CAT_NAME(node_name, name) DT_CAT(node_name, name)

#define PID_INS_CAT_NAME(node_name, name) DT_CAT3(node_name, _, name)

#define PID_INS_NAME(node_id, name)       PID_INS_CAT_NAME(DT_NODE_FULL_NAME_UNQUOTED(node_id), name)



#define PID_NEW_INSTANCE(node_id, name)                                                            \
@@ -58,7 +58,7 @@ struct pid_data { 
	float err_integral;

	float err_derivate;

	float ratio;

	struct device *pid_dev;

	const struct device *pid_dev;

	uint32_t *curr_time;

	uint32_t *prev_time;

	float *output;
@@ -88,7 +88,7 @@ STATIC_VOID pid_calc(struct pid_data *data) 
		return;

	}

	if (!pid_para->mit) {

		if (!isnanf(ki) && !float_equal(ki, 0)) {

		if ((ki == 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) {
@@ -98,8 +98,8 @@ STATIC_VOID pid_calc(struct pid_data *data) 
				}

			}

		}

		if (!isnanf(kd)) {

			if (isnanf(pid_para->detri_lpf)) {

		if ((kd == kd)) {

			if ((pid_para->detri_lpf != pid_para->detri_lpf)) {

				data->err_derivate =

					kd * (*(data->detri_ref) - *(data->detri_curr)) / deltaT;

			} else {
@@ -121,7 +121,7 @@ STATIC_VOID pid_calc(struct pid_data *data) 
		}

		return;

	} else {

		if (!isnanf(ki) && !float_equal(ki, 0)) {

		if ((ki == 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) {
@@ -131,8 +131,8 @@ STATIC_VOID pid_calc(struct pid_data *data) 
				}

			}

		}

		if (!isnanf(kd)) {

			if (isnanf(pid_para->detri_lpf)) {

		if ((kd == kd)) {

			if ((pid_para->detri_lpf != pid_para->detri_lpf)) {

				data->err_derivate =

					kd * (*(data->detri_ref) - *(data->detri_curr)) / deltaT;

			} else {
@@ -170,15 +170,9 @@ STATIC float pid_calc_in(struct pid_data *data, float error, float deltaT_us) 
	if (deltaT_us == 0) {

		return 0;

	}

	float *out;

	if (data->output != NULL) {

		out = data->output;

	} else {

		float o;

		out = &o;

	}

	float out;

	if (!pid_para->mit) {

		if (!isnanf(ki) && !float_equal(ki, 0)) {

		if ((ki == ki) && !float_equal(ki, 0)) {

			data->err_integral += (error * deltaT_us) / ki;

			if (pid_para->integral_limit != 0) {

				if (fabsf(data->err_integral) > pid_para->integral_limit) {
@@ -188,8 +182,8 @@ STATIC float pid_calc_in(struct pid_data *data, float error, float deltaT_us) 
				}

			}

		}

		if (!isnanf(kd)) {

			if (isnanf(pid_para->detri_lpf)) {

		if ((kd == kd)) {

			if ((pid_para->detri_lpf != pid_para->detri_lpf)) {

				data->err_derivate =

					kd * (*(data->detri_ref) - *(data->detri_curr)) / deltaT_us;

			} else {
@@ -202,12 +196,12 @@ STATIC float pid_calc_in(struct pid_data *data, float error, float deltaT_us) 
		}

		//   LOG_INF("integral: %d, derivate: %d", to16t(ki * (err * deltaT) / 1000000),

		//           to16t(kd * 1000000 * err / deltaT));

		*out = kp * (error + data->err_integral + data->err_derivate) +

		out = kp * (error + data->err_integral + data->err_derivate) +

		      pid_para->output_offset;

		if (pid_para->output_limit != 0 && fabsf(*out) > pid_para->output_limit) {

			*out = *out > 0 ? pid_para->output_limit : -pid_para->output_limit;

		if (pid_para->output_limit != 0 && fabsf(out) > pid_para->output_limit) {

			out = out > 0 ? pid_para->output_limit : -pid_para->output_limit;

		}

		return *out;

		return out;

	}

	return NAN;

}

include/zephyr/drivers/wheel.h

+2 −2
Original line number Diff line number Diff line
@@ -25,8 +25,8 @@ extern "C" { 


#define DT_WHEEL_CONFIG_GET(inst)                                                                  \

	{                                                                                          \

		.angle_offset = DT_STRING_UNQUOTED(DT_DRV_INST(inst), angle_offset),               \

		.wheel_radius = DT_STRING_UNQUOTED(DT_DRV_INST(inst), wheel_radius) * 0.1f,        \

		.angle_offset = (float)DT_STRING_UNQUOTED(DT_DRV_INST(inst), angle_offset),        \

		.wheel_radius = (float)DT_STRING_UNQUOTED(DT_DRV_INST(inst), wheel_radius) * 0.1f, \

	}



typedef struct {

CRA Git | Maintained and supported by SUSTech CRA and CCSE