@@ -59,7 +59,7 @@ struct actuator_vesc_can {
59
59
vesc_can_type_t type ;
60
60
struct can_filter rx_filter ;
61
61
struct context * ctx ;
62
- double position ;
62
+ double rotation ;
63
63
};
64
64
65
65
struct context {
@@ -79,16 +79,32 @@ struct context {
79
79
bool ready ;
80
80
bool fd ;
81
81
const char * label ;
82
+ uint16_t status_rate ;
83
+ bool enable_pub_wheel_odom ;
82
84
};
83
85
84
86
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 ;
86
91
ctx -> ready = false;
87
- int err = can_stop (ctx -> device );
92
+ err = can_get_state (ctx -> device , & state , & err_cnt );
88
93
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;
90
96
return err ;
91
97
}
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
+
92
108
return err ;
93
109
}
94
110
@@ -101,9 +117,6 @@ static int actuate_vesc_can_init(struct context *ctx)
101
117
zros_pub_init (& ctx -> pub_wheel_odometry , & ctx -> node , & topic_wheel_odometry ,
102
118
& ctx -> wheel_odometry );
103
119
104
- enum can_state state ;
105
- struct can_bus_err_cnt err_cnt ;
106
-
107
120
if (ctx -> ready ) {
108
121
LOG_ERR ("%s - already initialized" , ctx -> label );
109
122
return 0 ;
@@ -114,19 +127,12 @@ static int actuate_vesc_can_init(struct context *ctx)
114
127
LOG_ERR ("%s - device not ready\n" , ctx -> label );
115
128
ctx -> ready = false;
116
129
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 ;
123
130
}
124
131
132
+ actuate_vesc_can_stop (ctx );
133
+
125
134
// set device mode
126
135
if (ctx -> fd ) {
127
- if (state != CAN_STATE_STOPPED ) {
128
- actuate_vesc_can_stop (ctx );
129
- }
130
136
err = can_set_mode (ctx -> device , CAN_MODE_FD );
131
137
if (err != 0 ) {
132
138
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)
135
141
}
136
142
137
143
// 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 ;
144
148
}
145
149
146
150
// add receive callback
@@ -152,8 +156,8 @@ static int actuate_vesc_can_init(struct context *ctx)
152
156
act -> rx_filter .mask = CAN_EXT_ID_MASK ;
153
157
err = can_add_rx_filter (ctx -> device , actuate_vesc_can_rx_callback , act ,
154
158
& 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 );
157
161
return err ;
158
162
}
159
163
}
@@ -181,20 +185,20 @@ static void actuate_vesc_can_rx_callback(const struct device *dev, struct can_fr
181
185
void * user_data )
182
186
{
183
187
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 ;
187
188
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 ;
190
195
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 ;
192
197
}
193
- mean_position /= ctx -> num_actuators ;
198
+ mean_rotation /= ctx -> num_actuators ;
194
199
ctx -> wheel_odometry .has_stamp = true;
195
200
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 ;
198
202
}
199
203
}
200
204
@@ -273,6 +277,8 @@ static void actuate_vesc_can_run(void *p0, void *p1, void *p2)
273
277
zros_sub_update (& ctx -> sub_actuators );
274
278
}
275
279
280
+ zros_pub_update (& ctx -> pub_wheel_odometry );
281
+
276
282
// update vesc_can
277
283
actuate_vesc_can_update (ctx );
278
284
}
@@ -341,7 +347,7 @@ static int actuate_vesc_can_device_init(const struct device *dev)
341
347
.type = DT_ENUM_IDX(node_id, input_type), \
342
348
.rx_filter = {}, \
343
349
.ctx = NULL, \
344
- .position = 0, \
350
+ .rotation = 0, \
345
351
},
346
352
347
353
#define VESC_CAN_ACTUATORS_DEFINE (inst ) \
@@ -363,6 +369,8 @@ static int actuate_vesc_can_device_init(const struct device *dev)
363
369
.device = DEVICE_DT_GET(DT_INST_PROP(inst, device)), \
364
370
.ready = false, \
365
371
.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), \
366
374
.label = DT_NODE_FULL_NAME(DT_DRV_INST(inst)), \
367
375
}; \
368
376
VESC_CAN_ACTUATOR_SHELL(inst); \
0 commit comments