Make room for longer ack messages and full-scales getter ops

roboticslab-uc3m · Dec 28, 2023 · 083bf24 · 083bf24
1 parent 8aa1cdf
commit 083bf24
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 21 deletions.
diff --git 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<std::uint8_t>(ackTimeout);
+    ackStateObserver = new TypedStateObserver<std::uint8_t[]>(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
@@ -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
@@ -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<unsigned int>(jr3_state::READY))
+    if (response[0] != static_cast<unsigned int>(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
@@ -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<unsigned int>(op); }
 
@@ -106,7 +107,7 @@ class Jr3Mbed : public yarp::dev::DeviceDriver,
     jr3_mode mode {jr3_mode::INVALID};
 
     ICanSenderDelegate * sender {nullptr};
-    TypedStateObserver<std::uint8_t> * ackStateObserver {nullptr};
+    TypedStateObserver<std::uint8_t[]> * ackStateObserver {nullptr};
 
     mutable std::mutex mtx;