Query full scales on init

libraries/YarpPlugins/Jr3Mbed/DeviceDriverImpl.cpp

@@ -9,8 +9,6 @@
 
 using namespace roboticslab;
 
-constexpr auto FULL_SCALE = 16384; // 2^14
-
 constexpr auto DEFAULT_ACK_TIMEOUT = 0.01; // [s]
 constexpr auto DEFAULT_MONITOR_PERIOD = 0.1; // [s]
 constexpr auto DEFAULT_FILTER = 0.0; // [Hz], 0.0 = no filter
@@ -82,6 +80,8 @@ bool Jr3Mbed::open(yarp::os::Searchable & config)
             return false;
         }
 
+        yCIInfo(JR3M, id()) << "Using full scales from configuration file:" << fullScalesValue.toString();
+
         const auto * fullScales = fullScalesValue.asList();
 
         for (int i = 0; i < 3; i++)
@@ -96,8 +96,8 @@ bool Jr3Mbed::open(yarp::os::Searchable & config)
     }
     else
     {
-        yCIError(JR3M, id()) << R"(Missing "fullScales" property)";
-        return false;
+        yCIInfo(JR3M, id()) << R"(Missing "fullScales" property, will be queried from sensor on startup)";
+        shouldQueryFullScales = true;
     }
 
     if (jr3Group.check("asyncPeriod", "period of asynchronous publishing mode (seconds)"))

libraries/YarpPlugins/Jr3Mbed/ICanBusSharerImpl.cpp

@@ -48,16 +48,23 @@ bool Jr3Mbed::initialize()
         return false;
     }
 
+    bool ret = false;
+
     switch (mode)
     {
     case jr3_mode::SYNC:
-        return sendStartSyncCommand(filter);
+        ret = sendStartSyncCommand(filter);
     case jr3_mode::ASYNC:
-        return sendStartAsyncCommand(filter, asyncPeriod);
+        ret = sendStartAsyncCommand(filter, asyncPeriod);
     default:
         yCIError(JR3M, id()) << "Unknown mode:" << static_cast<int>(mode);
         return false;
     }
+
+    ret = ret && (!shouldQueryFullScales || queryFullScales());
+    ret = ret && sendCommand("zero offsets", can_ops::ZERO_OFFS);
+
+    return ret;
 }
 
 // -----------------------------------------------------------------------------
@@ -86,7 +93,7 @@ bool Jr3Mbed::notifyMessage(const can_message & message)
         return true;
     }
     case can_ops::ACK:
-        return message.len >= 1 && ackStateObserver->notify(message.data, 1);
+        return ackStateObserver->notify(message.data, message.len);
     case can_ops::FORCES:
     {
         auto [forces, counter] = parseData(message);

libraries/YarpPlugins/Jr3Mbed/Jr3Mbed.cpp

@@ -3,6 +3,7 @@
 #include "Jr3Mbed.hpp"
 
 #include <cstdint>
+#include <cstring> // std::memcpy
 
 #include <yarp/os/LogStream.h>
 
@@ -12,16 +13,16 @@ using namespace roboticslab;
 
 // -----------------------------------------------------------------------------
 
-bool Jr3Mbed::performRequest(const std::string & cmd, const can_message & msg, bool quiet)
+bool Jr3Mbed::performRequest(const std::string & cmd, const can_message & msg, std::uint8_t * response, bool quiet)
 {
+    yCIInfo((quiet ? JR3M_QUIET : JR3M), id()) << "Sending" << cmd << "command";
+
     if (!sender || !sender->prepareMessage(msg))
     {
         yCIWarning((quiet ? JR3M_QUIET : JR3M), id()) << "Unable to register" << cmd << "command";
         return false;
     }
 
-    std::uint8_t response[7];
-
     if (!ackStateObserver->await(response))
     {
         yCIWarning((quiet ? JR3M_QUIET : JR3M), id()) << "Command" << cmd << "timed out";
@@ -50,13 +51,13 @@ bool Jr3Mbed::sendStartSyncCommand(double _filter)
 {
     std::string cmd = "start sync";
     std::uint16_t filter = _filter * 100;
-    yCIInfo(JR3M, id()) << "Sending" << cmd << "command with filter" << filter * 0.01 << "Hz";
 
     unsigned char data[sizeof(filter)];
     std::memcpy(data, &filter, sizeof(filter));
 
     can_message msg {getCommandId(can_ops::START_SYNC), sizeof(filter), data};
-    return performRequest(cmd, msg);
+    std::uint8_t response[1];
+    return performRequest(cmd, msg, response);
 }
 
 // -----------------------------------------------------------------------------
@@ -66,30 +67,69 @@ bool Jr3Mbed::sendStartAsyncCommand(double _filter, double _period)
     std::string cmd = "start async";
     std::uint16_t filter = _filter * 100;
     std::uint32_t period = _period * 1e6;
-    yCIInfo(JR3M, id()) << "Sending" << cmd << "command with filter" << filter * 0.01 << "Hz and period" << period * 1e-3 << "ms";
 
     unsigned char data[sizeof(filter) + sizeof(period)];
     std::memcpy(data, &filter, sizeof(filter));
     std::memcpy(data + sizeof(filter), &period, sizeof(period));
 
     can_message msg {getCommandId(can_ops::START_ASYNC), sizeof(filter) + sizeof(period), data};
-    return performRequest(cmd, msg);
+    std::uint8_t response[1];
+    return performRequest(cmd, msg, response);
 }
 
 // -----------------------------------------------------------------------------
 
 bool Jr3Mbed::sendCommand(const std::string & cmd, can_ops op)
 {
-    yCIInfo(JR3M, id()) << "Sending" << cmd << "command";
     can_message msg {getCommandId(op), 0, nullptr};
-    return performRequest(cmd, msg);
+    std::uint8_t response[1];
+    return performRequest(cmd, msg, response);
 }
 
 // -----------------------------------------------------------------------------
 
 bool Jr3Mbed::ping()
 {
-    return performRequest("ping", {getCommandId(can_ops::GET_STATE), 0, nullptr}, true);
+    can_message msg {getCommandId(can_ops::GET_STATE), 0, nullptr};
+    std::uint8_t response[1];
+    return performRequest("ping", msg, response, true);
+}
+
+// -----------------------------------------------------------------------------
+
+bool Jr3Mbed::queryFullScales()
+{
+    std::uint8_t response[7];
+    std::uint16_t value;
+
+    can_message msgForces {getCommandId(can_ops::GET_FS_FORCES), 0, nullptr};
+
+    if (!performRequest("get full scales (forces)", msgForces, response))
+    {
+        return false;
+    }
+
+    for (int i = 0; i < 3; i++)
+    {
+        std::memcpy(&value, response + 1 + (2 * i), sizeof(value));
+        scales[i] = value / static_cast<double>(FULL_SCALE);
+    }
+
+    can_message msgMoments {getCommandId(can_ops::GET_FS_MOMENTS), 0, nullptr};
+
+    if (!performRequest("get full scales (forces)", msgMoments, response))
+    {
+        return false;
+    }
+
+    for (int i = 0; i < 3; i++)
+    {
+        std::memcpy(&value, response + 1 + (2 * i), sizeof(value));
+        scales[i + 3] = value / (static_cast<double>(FULL_SCALE) * 10);
+    }
+
+    shouldQueryFullScales = false;
+    return true;
 }
 
 // -----------------------------------------------------------------------------

libraries/YarpPlugins/Jr3Mbed/Jr3Mbed.hpp

@@ -90,11 +90,12 @@ class Jr3Mbed : public yarp::dev::DeviceDriver,
     constexpr unsigned int getCommandId(can_ops op) const
     { return canId + static_cast<unsigned int>(op); }
 
-    bool performRequest(const std::string & cmd, const can_message & msg, bool quiet = false);
+    bool performRequest(const std::string & cmd, const can_message & msg, std::uint8_t * response, bool quiet = false);
     bool sendStartSyncCommand(double filter);
     bool sendStartAsyncCommand(double filter, double delay);
     bool sendCommand(const std::string & cmd, can_ops op);
     bool ping();
+    bool queryFullScales();
 
     unsigned int canId {0};
 
@@ -103,6 +104,7 @@ class Jr3Mbed : public yarp::dev::DeviceDriver,
 
     std::string name;
     std::array<double, 6> scales;
+    bool shouldQueryFullScales {false};
 
     jr3_mode mode {jr3_mode::INVALID};
 
@@ -120,6 +122,8 @@ class Jr3Mbed : public yarp::dev::DeviceDriver,
     std::uint16_t integrityCounter {0};
 
     yarp::os::Timer * monitorThread {nullptr};
+
+    static constexpr unsigned int FULL_SCALE = 16384; // 2^14
 };
 
 } // namespace roboticslab