diff --git a/src/ODriveCAN.cpp b/src/ODriveCAN.cpp index d84ffe6..52b2150 100644 --- a/src/ODriveCAN.cpp +++ b/src/ODriveCAN.cpp @@ -171,6 +171,34 @@ void ODriveCAN::onReceive(uint32_t id, uint8_t length, const uint8_t* data) { Serial.println(F("missing callback")); break; } + case Get_Temperature_msg_t::cmd_id: { + Get_Temperature_msg_t temperature; + temperature.decode_buf(data); + if (temperature_callback_) + temperature_callback_(temperature, temperature_user_data_); + break; + } + case Get_Bus_Voltage_Current_msg_t::cmd_id: { + Get_Bus_Voltage_Current_msg_t bus_vi; + bus_vi.decode_buf(data); + if (busVI_callback_) + busVI_callback_(bus_vi, busVI_user_data_); + break; + } + case Get_Iq_msg_t::cmd_id: { + Get_Iq_msg_t iq; + iq.decode_buf(data); + if (currents_callback_) + currents_callback_(iq, currents_user_data_); + break; + } + case Get_Error_msg_t::cmd_id: { + Get_Error_msg_t error; + error.decode_buf(data); + if (error_callback_) + error_callback_(error, error_user_data_); + break; + } default: { if (requested_msg_id_ == REQUEST_PENDING) return; diff --git a/src/ODriveCAN.h b/src/ODriveCAN.h index 8b6d06d..ef21221 100644 --- a/src/ODriveCAN.h +++ b/src/ODriveCAN.h @@ -171,6 +171,7 @@ class ODriveCAN { * @brief Requests ODrive DC bus voltage and current * * This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply + * May trigger onBusVI callback if it's registered. */ bool getBusVI(Get_Bus_Voltage_Current_msg_t& msg, uint16_t timeout_ms = 10); @@ -222,6 +223,38 @@ class ODriveCAN { torques_user_data_ = user_data; } + /** + * @brief Registers a callback for ODrive temperature feedback. + */ + void onTemperature(void (*callback)(Get_Temperature_msg_t& feedback, void* user_data), void* user_data = nullptr) { + temperature_callback_ = callback; + temperature_user_data_ = user_data; + } + + /** + * @brief Registers a callback for ODrive bus voltage/current feedback. + */ + void onBusVI(void (*callback)(Get_Bus_Voltage_Current_msg_t& feedback, void* user_data), void* user_data = nullptr) { + busVI_callback_ = callback; + busVI_user_data_ = user_data; + } + + /** + * @brief Registers a callback for ODrive currents feedback. + */ + void onCurrents(void (*callback)(Get_Iq_msg_t& feedback, void* user_data), void* user_data = nullptr) { + currents_callback_ = callback; + currents_user_data_ = user_data; + } + + /** + * @brief Registers a callback for ODrive error messages. + */ + void onError(void (*callback)(Get_Error_msg_t& msg, void* user_data), void* user_data = nullptr) { + error_callback_ = callback; + error_user_data_ = user_data; + } + /** * @brief Processes received CAN messages for the ODrive. */ @@ -332,8 +365,16 @@ class ODriveCAN { void* axis_state_user_data_; void* feedback_user_data_; void* torques_user_data_; + void* temperature_user_data_; + void* busVI_user_data_; + void* currents_user_data_; + void* error_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; + void (*temperature_callback_)(Get_Temperature_msg_t& feedback, void* user_data) = nullptr; + void (*busVI_callback_)(Get_Bus_Voltage_Current_msg_t& feedback, void* user_data) = nullptr; + void (*currents_callback_)(Get_Iq_msg_t& feedback, void* user_data) = nullptr; + void (*error_callback_)(Get_Error_msg_t& msg, void* user_data) = nullptr; };