Skip to content

Commit 8439d14

Browse files
committed
Updates for vesc status.
Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>
1 parent a667112 commit 8439d14

File tree

3 files changed

+53
-34
lines changed

3 files changed

+53
-34
lines changed

app/melm/boards/mr_canhubk3_s32k344.overlay

+2
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,8 @@
1111
vesc_can_actuators: vesc_can_actuators {
1212
compatible = "cerebri,vesc_can-actuators";
1313
device = <&flexcan0>;
14+
pub-wheel-odometry;
15+
status-rate = <50>;
1416
motor0 {
1517
vesc-id = <60>;
1618
input-type = "velocity";

drivers/actuate/vesc_can/main.c

+42-34
Original file line numberDiff line numberDiff line change
@@ -59,7 +59,7 @@ struct actuator_vesc_can {
5959
vesc_can_type_t type;
6060
struct can_filter rx_filter;
6161
struct context *ctx;
62-
double position;
62+
double rotation;
6363
};
6464

6565
struct context {
@@ -79,16 +79,32 @@ struct context {
7979
bool ready;
8080
bool fd;
8181
const char *label;
82+
uint16_t status_rate;
83+
bool enable_pub_wheel_odom;
8284
};
8385

8486
static int actuate_vesc_can_stop(struct context *ctx)
85-
{
87+
{
88+
int err = 0;
89+
struct can_bus_err_cnt err_cnt;
90+
enum can_state state;
8691
ctx->ready = false;
87-
int err = can_stop(ctx->device);
92+
err = can_get_state(ctx->device, &state, &err_cnt);
8893
if (err != 0) {
89-
LOG_ERR("%s - failed to stop (%d)\n", ctx->label, err);
94+
LOG_ERR("%s - failed to get CAN controller state (%d)\n", ctx->label, err);
95+
ctx->ready = false;
9096
return err;
9197
}
98+
99+
// make sure stopped
100+
if (state != CAN_STATE_STOPPED) {
101+
err = can_stop(ctx->device);
102+
if (err != 0) {
103+
LOG_ERR("%s - failed to stop (%d)\n", ctx->label, err);
104+
return err;
105+
}
106+
}
107+
92108
return err;
93109
}
94110

@@ -101,9 +117,6 @@ static int actuate_vesc_can_init(struct context *ctx)
101117
zros_pub_init(&ctx->pub_wheel_odometry, &ctx->node, &topic_wheel_odometry,
102118
&ctx->wheel_odometry);
103119

104-
enum can_state state;
105-
struct can_bus_err_cnt err_cnt;
106-
107120
if (ctx->ready) {
108121
LOG_ERR("%s - already initialized", ctx->label);
109122
return 0;
@@ -114,19 +127,12 @@ static int actuate_vesc_can_init(struct context *ctx)
114127
LOG_ERR("%s - device not ready\n", ctx->label);
115128
ctx->ready = false;
116129
return -1;
117-
} // check state
118-
err = can_get_state(ctx->device, &state, &err_cnt);
119-
if (err != 0) {
120-
LOG_ERR("%s - failed to get CAN controller state (%d)\n", ctx->label, err);
121-
ctx->ready = false;
122-
return err;
123130
}
124131

132+
actuate_vesc_can_stop(ctx);
133+
125134
// set device mode
126135
if (ctx->fd) {
127-
if (state != CAN_STATE_STOPPED) {
128-
actuate_vesc_can_stop(ctx);
129-
}
130136
err = can_set_mode(ctx->device, CAN_MODE_FD);
131137
if (err != 0) {
132138
LOG_ERR("%s - set mode FD failed (%d)\n", ctx->label, err);
@@ -135,12 +141,10 @@ static int actuate_vesc_can_init(struct context *ctx)
135141
}
136142

137143
// start device
138-
if ((state == CAN_STATE_STOPPED) || (state == CAN_STATE_BUS_OFF)) {
139-
err = can_start(ctx->device);
140-
if (err != 0) {
141-
LOG_ERR("%s - start failed\n", ctx->label);
142-
return err;
143-
}
144+
err = can_start(ctx->device);
145+
if (err != 0) {
146+
LOG_ERR("%s - start failed\n", ctx->label);
147+
return err;
144148
}
145149

146150
// add receive callback
@@ -152,8 +156,8 @@ static int actuate_vesc_can_init(struct context *ctx)
152156
act->rx_filter.mask = CAN_EXT_ID_MASK;
153157
err = can_add_rx_filter(ctx->device, actuate_vesc_can_rx_callback, act,
154158
&act->rx_filter);
155-
if (err != 0) {
156-
LOG_ERR("%s - callback setup failed\n", ctx->label);
159+
if (err < 0) {
160+
LOG_ERR("%s - callback setup failed (%d)\n", ctx->label, err);
157161
return err;
158162
}
159163
}
@@ -181,20 +185,20 @@ static void actuate_vesc_can_rx_callback(const struct device *dev, struct can_fr
181185
void *user_data)
182186
{
183187
struct actuator_vesc_can *act = (struct actuator_vesc_can *)user_data;
184-
const double dt = 1.0 / 50;
185-
int64_t data = *(frame->data);
186-
act->position += dt * data / act->pole_pair;
187188
struct context *ctx = act->ctx;
188-
if (act->id == ctx->actuator_vesc_cans[ctx->num_actuators].id) {
189-
double mean_position = 0;
189+
int64_t data = (frame->data[0] << 24) + (frame->data[1] << 16) + (frame->data[2] << 8) + frame->data[3];
190+
act->rotation += 2 * M_PI * data / (act->pole_pair * ctx->status_rate * 60);
191+
//LOG_INF("Data: %ld", data);
192+
//LOG_INF("Angular: %f", act->rotation);
193+
if (act->id == ctx->actuator_vesc_cans[0].id) {
194+
double mean_rotation = 0;
190195
for (int i = 0; i < ctx->num_actuators; i++) {
191-
mean_position += ctx->actuator_vesc_cans[i].position;
196+
mean_rotation += ctx->actuator_vesc_cans[i].rotation;
192197
}
193-
mean_position /= ctx->num_actuators;
198+
mean_rotation /= ctx->num_actuators;
194199
ctx->wheel_odometry.has_stamp = true;
195200
stamp_msg(&ctx->wheel_odometry.stamp, k_uptime_ticks());
196-
ctx->wheel_odometry.rotation = mean_position;
197-
zros_pub_update(&ctx->pub_wheel_odometry);
201+
ctx->wheel_odometry.rotation = mean_rotation;
198202
}
199203
}
200204

@@ -273,6 +277,8 @@ static void actuate_vesc_can_run(void *p0, void *p1, void *p2)
273277
zros_sub_update(&ctx->sub_actuators);
274278
}
275279

280+
zros_pub_update(&ctx->pub_wheel_odometry);
281+
276282
// update vesc_can
277283
actuate_vesc_can_update(ctx);
278284
}
@@ -341,7 +347,7 @@ static int actuate_vesc_can_device_init(const struct device *dev)
341347
.type = DT_ENUM_IDX(node_id, input_type), \
342348
.rx_filter = {}, \
343349
.ctx = NULL, \
344-
.position = 0, \
350+
.rotation = 0, \
345351
},
346352

347353
#define VESC_CAN_ACTUATORS_DEFINE(inst) \
@@ -363,6 +369,8 @@ static int actuate_vesc_can_device_init(const struct device *dev)
363369
.device = DEVICE_DT_GET(DT_INST_PROP(inst, device)), \
364370
.ready = false, \
365371
.fd = DT_INST_PROP(inst, fd), \
372+
.status_rate = DT_INST_PROP(inst, status_rate), \
373+
.enable_pub_wheel_odom = DT_INST_PROP(inst, pub_wheel_odometry), \
366374
.label = DT_NODE_FULL_NAME(DT_DRV_INST(inst)), \
367375
}; \
368376
VESC_CAN_ACTUATOR_SHELL(inst); \

dts/bindings/cerebri,vesc_can-actuators.yaml

+9
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,15 @@ properties:
1818
type: boolean
1919
description: enable CAN FD mode
2020

21+
pub-wheel-odometry:
22+
type: boolean
23+
description: Publishes wheel odometry on the average of all wheels
24+
25+
status-rate:
26+
required: true
27+
type: int
28+
description: vesc can status publication rate in Hz. If status-rate <0> disables.
29+
2130
child-binding:
2231
description: VESC_CAN actuators node
2332
properties:

0 commit comments

Comments
 (0)