diff --git a/src/ODriveCAN.cpp b/src/ODriveCAN.cpp index 968085c..a59026a 100644 --- a/src/ODriveCAN.cpp +++ b/src/ODriveCAN.cpp @@ -155,6 +155,13 @@ void ODriveCAN::onReceive(uint32_t id, uint8_t length, const uint8_t* data) { feedback_callback_(estimates, feedback_user_data_); break; } + case Get_Torques_msg_t::cmd_id: { + Get_Torques_msg_t estimates; + estimates.decode_buf(data); + if (torques_callback_) + torques_callback_(estimates, torques_user_data_); + break; + } case Heartbeat_msg_t::cmd_id: { Heartbeat_msg_t status; status.decode_buf(data); diff --git a/src/ODriveCAN.h b/src/ODriveCAN.h index 45e0ba0..972dfb5 100644 --- a/src/ODriveCAN.h +++ b/src/ODriveCAN.h @@ -213,6 +213,14 @@ class ODriveCAN { axis_state_callback_ = callback; axis_state_user_data_ = user_data; } + + /** + * @brief Registers a callback for ODrive torques feedback processing. + */ + void onTorques(void (*callback)(Get_Torques_msg_t& feedback, void* user_data), void* user_data = nullptr) { + torques_callback_ = callback; + torques_user_data_ = user_data; + } /** * @brief Processes received CAN messages for the ODrive. @@ -322,7 +330,9 @@ class ODriveCAN { void* axis_state_user_data_; void* feedback_user_data_; + void* torques_user_data_; void (*axis_state_callback_)(Heartbeat_msg_t& feedback, void* user_data) = nullptr; void (*feedback_callback_)(Get_Encoder_Estimates_msg_t& feedback, void* user_data) = nullptr; + void (*torques_callback_)(Get_Torques_msg_t& feedback, void* user_data) = nullptr; };