Allowing enable and disabling (4610fd1f) · Commits · Wenxi XU / Mambo

drivers/wheel/steerwheel.c

+8 −0

Original line number	Diff line number	Diff line
		@@ -208,11 +208,19 @@ static inline wheel_status_t steerwheel_get_target(const struct device dev)
		return &data->target;
		}

		static inline void steerwheel_disable(const struct device *dev)
		{
		const steerwheel_cfg_t *cfg = dev->config;
		motor_set_torque(cfg->steer_motor, 0);
		motor_set_torque(cfg->wheel_motor, 0);
		}

		struct wheel_driver_api steerwheel_driver_api = {
		.wheel_set_speed = steerwheel_set_speed,
		.wheel_set_static = steerwheel_set_static,
		.wheel_get_speed = steerwheel_get_speed,
		.wheel_get_target = steerwheel_get_target,
		.wheel_disable = steerwheel_disable,
		};

		#define STEERWHEEL_DEVICE_DT_DEFINE(node_id, init_fn, pm, data, config, level, prio, api, ...) \

include/zephyr/drivers/chassis.h

+24 −8

Original line number	Diff line number	Diff line
		@@ -8,14 +8,14 @@

		#include "zephyr/drivers/pid.h"
		#include "zephyr/toolchain.h"
		#include "zephyr/zbus/zbus.h"
		#include <stdint.h>
		#include <string.h>
		#include <sys/types.h>
		#include <zephyr/kernel.h>
		#include <zephyr/device.h>
		#include <zephyr/drivers/wheel.h>

		#ifndef RADS_TO_RPM
		#ifndef RADPS_TO_RPM
		#define RADPS_TO_RPM 6.28318531f
		#endif

		@@ -47,10 +47,8 @@ struct pos_data {

		typedef struct {
		float targetYaw;
		float currentYaw;

		float targetGyro;
		float currentGyro;

		float targetXSpeed;
		float targetYSpeed;
		@@ -67,10 +65,10 @@ typedef struct {

		chassis_status_t chassis_status;

		float pid_input;
		float static0;
		bool track_angle;
		bool enabled;

		struct zbus_channel *chassis_sensor_zbus;
		struct pos_data chassis_sensor_data;
		} chassis_data_t;

		typedef struct {
		@@ -148,7 +146,25 @@ static inline chassis_status_t z_impl_chassis_get_status(const struct device d
		static void chassis_update_sensor(const struct device dev, struct pos_data pos_data)
		{
		chassis_data_t *data = dev->data;
		zbus_chan_pub(data->chassis_sensor_zbus, &pos_data, K_MSEC(5));
		data->prevTime = data->currTime;
		data->currTime = k_cycle_get_32();
		memcpy(&data->chassis_sensor_data, pos_data, sizeof(struct pos_data));
		data->chassis_sensor_data.Yaw = data->chassis_sensor_data.Yaw < 0
		? data->chassis_sensor_data.Yaw + 360
		: data->chassis_sensor_data.Yaw;
		}

		static void chassis_set_enabled(const struct device *dev, bool enabled)
		{
		chassis_data_t *data = dev->data;
		chassis_cfg_t *cfg = dev->config;
		data->enabled = enabled;
		if (!enabled) {
		for (int i = 0; i < CHASSIS_WHEEL_COUNT; i++) {
		struct device *wheel = cfg->wheels[i];
		wheel_disable(wheel);
		}
		}
		}

		#ifdef __cplusplus

include/zephyr/drivers/wheel.h

+13 −1

Original line number	Diff line number	Diff line
		@@ -26,7 +26,7 @@ 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), \
		.wheel_radius = DT_STRING_UNQUOTED(DT_DRV_INST(inst), wheel_radius) * 0.1f, \
		}

		typedef struct {
		@@ -45,6 +45,7 @@ typedef void (wheel_api_set_speed_t)(const struct device dev, float speed, flo
		typedef int (wheel_api_set_static_t)(const struct device dev, float angle);
		typedef wheel_status_t (wheel_api_get_speed_t)(const struct device *dev);
		typedef wheel_status_t (wheel_api_get_target_t)(const struct device *dev);
		typedef void (wheel_api_disable_t)(const struct device dev);

		/* API Structure */
		__subsystem struct wheel_driver_api {
		@@ -52,6 +53,7 @@ __subsystem struct wheel_driver_api {
		wheel_api_set_static_t wheel_set_static;
		wheel_api_get_speed_t wheel_get_speed;
		wheel_api_get_target_t wheel_get_target;
		wheel_api_disable_t wheel_disable;
		};

		/* Syscalls and inline implementations */
		@@ -95,6 +97,16 @@ static inline wheel_status_t z_impl_wheel_get_target(const struct device dev)
		return api->wheel_get_target(dev);
		}

		__syscall void wheel_disable(const struct device *dev);
		static inline void z_impl_wheel_disable(const struct device *dev)
		{
		const struct wheel_driver_api api = (const struct wheel_driver_api )dev->api;
		if (api->wheel_disable == NULL) {
		return;
		}
		api->wheel_disable(dev);
		}

		#ifdef __cplusplus
		}
		#endif

Admin message