diff --git a/libraries/YarpPlugins/Jr3Mbed/DeviceDriverImpl.cpp b/libraries/YarpPlugins/Jr3Mbed/DeviceDriverImpl.cpp index 3d5214973..627729a49 100644 --- a/libraries/YarpPlugins/Jr3Mbed/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/Jr3Mbed/DeviceDriverImpl.cpp @@ -64,7 +64,7 @@ bool Jr3Mbed::open(yarp::os::Searchable & config) return false; } - ackStateObserver = new TypedStateObserver(ackTimeout); + ackStateObserver = new TypedStateObserver(ackTimeout); if (!config.check("fullScales", "full scales for each axis")) // id-specific { diff --git a/libraries/YarpPlugins/Jr3Mbed/ICanBusSharerImpl.cpp b/libraries/YarpPlugins/Jr3Mbed/ICanBusSharerImpl.cpp index 8df130b44..2b5679c4d 100644 --- a/libraries/YarpPlugins/Jr3Mbed/ICanBusSharerImpl.cpp +++ b/libraries/YarpPlugins/Jr3Mbed/ICanBusSharerImpl.cpp @@ -86,7 +86,7 @@ bool Jr3Mbed::notifyMessage(const can_message & message) return true; } case can_ops::ACK: - return message.len == 1 && ackStateObserver->notify(message.data[0]); + return message.len >= 1 && ackStateObserver->notify(message.data, 1); case can_ops::FORCES: { auto [forces, counter] = parseData(message); diff --git a/libraries/YarpPlugins/Jr3Mbed/Jr3Mbed.cpp b/libraries/YarpPlugins/Jr3Mbed/Jr3Mbed.cpp index b19ea4fb7..17bd636a9 100644 --- a/libraries/YarpPlugins/Jr3Mbed/Jr3Mbed.cpp +++ b/libraries/YarpPlugins/Jr3Mbed/Jr3Mbed.cpp @@ -20,15 +20,15 @@ bool Jr3Mbed::performRequest(const std::string & cmd, const can_message & msg, b return false; } - std::uint8_t response; + std::uint8_t response[7]; - if (!ackStateObserver->await(&response)) + if (!ackStateObserver->await(response)) { yCIWarning((quiet ? JR3M_QUIET : JR3M), id()) << "Command" << cmd << "timed out"; return false; } - if (response != static_cast(jr3_state::READY)) + if (response[0] != static_cast(jr3_state::READY)) { yCIError(JR3M, id()) << "Sensor is in error state"; status = yarp::dev::MAS_ERROR; diff --git a/libraries/YarpPlugins/Jr3Mbed/Jr3Mbed.hpp b/libraries/YarpPlugins/Jr3Mbed/Jr3Mbed.hpp index 73f17304e..0d6b26f41 100644 --- a/libraries/YarpPlugins/Jr3Mbed/Jr3Mbed.hpp +++ b/libraries/YarpPlugins/Jr3Mbed/Jr3Mbed.hpp @@ -65,27 +65,28 @@ class Jr3Mbed : public yarp::dev::DeviceDriver, // keep this in sync with the firmware enum class can_ops : std::uint16_t { - ACK = 0x100, - START_SYNC = 0x180, - START_ASYNC = 0x200, - STOP = 0x280, - ZERO_OFFS = 0x300, - SET_FILTER = 0x380, - GET_FS = 0x400, - GET_STATE = 0x480, - RESET = 0x500, - FORCES = 0x580, - MOMENTS = 0x600, - BOOTUP = 0x700, + ACK = 0x100, + START_SYNC = 0x180, + START_ASYNC = 0x200, + STOP = 0x280, + ZERO_OFFS = 0x300, + SET_FILTER = 0x380, + GET_STATE = 0x400, + GET_FS_FORCES = 0x480, + GET_FS_MOMENTS = 0x500, + RESET = 0x580, + FORCES = 0x600, + MOMENTS = 0x680, + BOOTUP = 0x700, }; - enum class jr3_mode - { SYNC, ASYNC, INVALID }; - // keep this in sync with the firmware enum class jr3_state : std::uint8_t { READY = 0x00, NOT_INITIALIZED = 0x01 }; + enum class jr3_mode + { SYNC, ASYNC, INVALID }; + constexpr unsigned int getCommandId(can_ops op) const { return canId + static_cast(op); } @@ -106,7 +107,7 @@ class Jr3Mbed : public yarp::dev::DeviceDriver, jr3_mode mode {jr3_mode::INVALID}; ICanSenderDelegate * sender {nullptr}; - TypedStateObserver * ackStateObserver {nullptr}; + TypedStateObserver * ackStateObserver {nullptr}; mutable std::mutex mtx;