Hello everyone,
I have been trying to implement a broadcasting function into the firmware 0.5.5, however I can’t seem to get it working. Could anyone better at embedded programming give me a hint? Or perhaps there is someone who already implemented it.
The assumption is the broadcast_id = 0x3f and all odrives and axes should react to it.
can_simple.cpp - added a broadcast_id to the renew_subscription function filter struct and added the broadcast_id to the message handling condition
bool CANSimple::renew_subscription(size_t i) {
Axis& axis = axes[i];
// TODO: remove these two lines (see comment in header)
node_ids_[i] = axis.config_.can.node_id;
broadcast_ids_[i] = axis.config_.can.broadcast_id;
extended_node_ids_[i] = axis.config_.can.is_extended;
MsgIdFilterSpecs filter = {
.id = {},
.broadcast_id = axis.config_.can.broadcast_id << NUM_CMD_ID_BITS,
.mask = (uint32_t)(0xffffffff << NUM_CMD_ID_BITS)};
if (axis.config_.can.is_extended) {
filter.id = (uint32_t)(axis.config_.can.node_id << NUM_CMD_ID_BITS);
} else {
filter.id = (uint16_t)(axis.config_.can.node_id << NUM_CMD_ID_BITS);
}
if (subscription_handles_[i]) {
canbus_->unsubscribe(subscription_handles_[i]);
}
return canbus_->subscribe(
filter, [](void* ctx, const can_Message_t& msg) {
((CANSimple*)ctx)->handle_can_message(msg);
},
this, &subscription_handles_[i]);
}
void CANSimple::handle_can_message(const can_Message_t& msg) {
// Frame
// nodeID | CMD
// 6 bits | 5 bits
uint32_t nodeID = get_node_id(msg.id);
for (auto& axis : axes) {
if (((axis.config_.can.node_id == nodeID) || (axis.config_.can.broadcast_id == nodeID)) && (axis.config_.can.is_extended == msg.isExt)) {
do_command(axis, msg);
return;
}
}
}
canbus.hpp - added a broadcast_id to the MsgIdFilterSpecs struct
struct MsgIdFilterSpecs {
std::variant<uint16_t, uint32_t> id;
uint32_t broadcast_id;
uint32_t mask;
};
odrive_can.cpp - added a hal_filter for the broadcast_id to the subscribe function (I know it is a bad implementation, but it should still work, correct?)
bool ODriveCAN::subscribe(const MsgIdFilterSpecs& filter, on_can_message_cb_t callback, void* ctx, CanSubscription** handle) {
auto it = std::find_if(subscriptions_.begin(), subscriptions_.end(), [](auto& subscription) {
return subscription.fifo == kCanFifoNone;
});
if (it == subscriptions_.end()) {
return false; // all subscription slots in use
}
it->callback = callback;
it->ctx = ctx;
it->fifo = CAN_RX_FIFO0; // TODO: make customizable
if (handle) {
*handle = &*it;
}
bool is_extended = filter.id.index() == 1;
uint32_t id = is_extended ?
((std::get<1>(filter.id) << 3) | (1 << 2)) :
(std::get<0>(filter.id) << 21);
uint32_t mask = (is_extended ? (filter.mask << 3) : (filter.mask << 21))
| (1 << 2); // care about the is_extended bit
CAN_FilterTypeDef hal_filter;
hal_filter.FilterActivation = ENABLE;
hal_filter.FilterBank = &*it - &subscriptions_[0];
hal_filter.FilterFIFOAssignment = it->fifo;
hal_filter.FilterIdHigh = (id >> 16) & 0xffff;
hal_filter.FilterIdLow = id & 0xffff;
hal_filter.FilterMaskIdHigh = (mask >> 16) & 0xffff;
hal_filter.FilterMaskIdLow = mask & 0xffff;
hal_filter.FilterMode = CAN_FILTERMODE_IDMASK;
hal_filter.FilterScale = CAN_FILTERSCALE_32BIT;
if (HAL_CAN_ConfigFilter(handle_, &hal_filter) != HAL_OK) {
return false;
}
uint32_t broadcast_id = filter.broadcast_id << 21;
uint32_t broadcast_mask = (filter.broadcast_id << 21);
CAN_FilterTypeDef hal_filter2;
hal_filter2.FilterActivation = ENABLE;
hal_filter2.FilterBank = 16;
hal_filter2.FilterFIFOAssignment = it->fifo;
hal_filter2.FilterIdHigh = (broadcast_id >> 16) & 0xffff;
hal_filter2.FilterIdLow = broadcast_id & 0xffff;
hal_filter2.FilterMaskIdHigh = (broadcast_mask >> 16) & 0xffff;
hal_filter2.FilterMaskIdLow = broadcast_mask & 0xffff;
hal_filter2.FilterMode = CAN_FILTERMODE_IDMASK;
hal_filter2.FilterScale = CAN_FILTERSCALE_32BIT;
if (HAL_CAN_ConfigFilter(handle_, &hal_filter2) != HAL_OK) {
return false;
}
return true;
}
axis.hpp - added the broadcast_id to the CANConfig_t struct
struct CANConfig_t {
uint32_t node_id = 0;
uint32_t broadcast_id = 0x3f;
bool is_extended = false;
uint32_t heartbeat_rate_ms = 0;
uint32_t encoder_rate_ms = 0;
};
Any help is appreciated!