Merge branch 'can-tests' into apocanlypse

.travis.yml

@@ -159,7 +159,7 @@ before_script:
   #- cmake "$TRAVIS_BUILD_DIR/examples/cpp/exampleTechnosoftIpos" && make
 
 script:
-  - echo "No unit tests available"
+  - cd "$TRAVIS_BUILD_DIR/build/tests" && ctest -V
 
 after_success:
   #-- Code coverage

libraries/CanBusSharerLib/CMakeLists.txt

@@ -7,7 +7,6 @@ if(ENABLE_CanBusSharerLib)
                                        CanOpen.cpp
                                        CanSenderDelegate.hpp
                                        SdoClient.hpp
-                                       SdoClient-inl.hpp
                                        SdoClient.cpp
                                        PdoProtocol.hpp
                                        PdoProtocol.cpp

libraries/CanBusSharerLib/CanOpen.cpp

@@ -56,8 +56,7 @@ bool CanOpen::consumeMessage(std::uint16_t cobId, const std::uint8_t * data, std
     switch (op)
     {
     case 0x80:
-        _emcy->accept(data);
-        return true;
+        return _emcy->accept(data);
     case 0x180:
         return _tpdo1->accept(data, size);
     case 0x280:

libraries/CanBusSharerLib/CanUtils.cpp

@@ -18,8 +18,8 @@ std::string CanUtils::msgToStr(std::uint8_t id, std::uint16_t cob, std::size_t l
 
     tmp << std::hex << std::setfill('0') << std::setw(2) << static_cast<int>(data[len - 1]);
     tmp << ". canId(";
-    tmp << std::dec << id;
-    tmp << ") via(";
+    tmp << std::dec << static_cast<int>(id);
+    tmp << ") via(0x";
     tmp << std::hex << cob;
     tmp << ").";
 

libraries/CanBusSharerLib/CanUtils.hpp

@@ -24,14 +24,17 @@ template<typename T_int, typename T_frac>
 void encodeFixedPoint(double value, T_int * integer, T_frac * fractional)
 {
     static_assert(std::is_integral<T_int>::value && std::is_integral<T_frac>::value, "Integral required.");
-    *integer = static_cast<T_int>(value);
-    *fractional = std::abs(value - *integer) * (1 << 8 * sizeof(T_frac));
+    static_assert(std::is_unsigned<T_frac>::value, "Unsigned fractional type required.");
+    double _int;
+    *fractional = std::round(std::modf(value, &_int) * (1 << 8 * sizeof(T_frac)));
+    *integer = static_cast<T_int>(_int);
 }
 
 template<typename T_int, typename T_frac>
 double decodeFixedPoint(T_int integer, T_frac fractional)
 {
     static_assert(std::is_integral<T_int>::value && std::is_integral<T_frac>::value, "Integral required.");
+    static_assert(std::is_unsigned<T_frac>::value, "Unsigned fractional type required.");
     double frac = static_cast<double>(fractional) / (1 << 8 * sizeof(T_frac));
     return integer + (integer >= 0 ? frac : -frac);
 }

libraries/CanBusSharerLib/DriveStatusMachine.cpp

@@ -14,19 +14,15 @@ namespace std
     template<>
     struct hash<roboticslab::DriveState>
     {
-        std::size_t operator ()(roboticslab::DriveState state)
-        {
-            return static_cast<std::size_t>(state);
-        }
+        std::size_t operator()(roboticslab::DriveState state)
+        { return static_cast<std::size_t>(state); }
     };
 
     template<>
     struct hash<roboticslab::DriveTransition>
     {
-        std::size_t operator ()(roboticslab::DriveTransition transition)
-        {
-            return static_cast<std::size_t>(transition);
-        }
+        std::size_t operator()(roboticslab::DriveTransition transition)
+        { return static_cast<std::size_t>(transition); }
     };
 }
 
@@ -36,10 +32,8 @@ namespace
     struct pair_hash
     {
         template<typename T1, typename T2>
-        std::size_t operator ()(const std::pair<T1, T2> & pair) const
-        {
-            return std::hash<T1>()(pair.first) ^ std::hash<T2>()(pair.second);
-        }
+        std::size_t operator()(const std::pair<T1, T2> & pair) const
+        { return std::hash<T1>()(pair.first) ^ std::hash<T2>()(pair.second); }
     };
 
     DriveState parseDriveState(const std::bitset<16> & bits)
@@ -93,7 +87,7 @@ namespace
             bits.reset(7);
             break;
         case DriveTransition::SWITCH_ON: // xxxx.xxxx.0xxx.0111
-        case DriveTransition::DISABLE_OPERATION:
+        //case DriveTransition::DISABLE_OPERATION:
             bits.set(0);
             bits.set(1);
             bits.set(2);
@@ -127,7 +121,7 @@ namespace
         {{DriveState::READY_TO_SWITCH_ON, DriveTransition::SWITCH_ON}, DriveState::SWITCHED_ON}, // 3
         {{DriveState::SWITCHED_ON, DriveTransition::ENABLE_OPERATION}, DriveState::OPERATION_ENABLED}, // 4
         {{DriveState::OPERATION_ENABLED, DriveTransition::DISABLE_OPERATION}, DriveState::SWITCHED_ON}, // 5
-        {{DriveState::OPERATION_ENABLED, DriveTransition::SWITCH_ON}, DriveState::SWITCHED_ON}, // 5 (alias)
+        //{{DriveState::OPERATION_ENABLED, DriveTransition::SWITCH_ON}, DriveState::SWITCHED_ON}, // 5 (alias)
         {{DriveState::SWITCHED_ON, DriveTransition::SHUTDOWN}, DriveState::READY_TO_SWITCH_ON}, // 6
         {{DriveState::READY_TO_SWITCH_ON, DriveTransition::DISABLE_VOLTAGE}, DriveState::SWITCH_ON_DISABLED}, // 7
         {{DriveState::OPERATION_ENABLED, DriveTransition::SHUTDOWN}, DriveState::READY_TO_SWITCH_ON}, // 8
@@ -146,12 +140,12 @@ namespace
         {{DriveState::READY_TO_SWITCH_ON, DriveState::OPERATION_ENABLED}, {DriveTransition::SWITCH_ON, DriveTransition::ENABLE_OPERATION}},
         {{DriveState::SWITCHED_ON, DriveState::OPERATION_ENABLED}, {DriveTransition::ENABLE_OPERATION}},
         {{DriveState::OPERATION_ENABLED, DriveState::SWITCHED_ON}, {DriveTransition::DISABLE_OPERATION}},
+        //{{DriveState::OPERATION_ENABLED, DriveState::SWITCHED_ON}, {DriveTransition::SWITCH_ON}},
         {{DriveState::OPERATION_ENABLED, DriveState::READY_TO_SWITCH_ON}, {DriveTransition::SHUTDOWN}},
         {{DriveState::OPERATION_ENABLED, DriveState::SWITCH_ON_DISABLED}, {DriveTransition::DISABLE_VOLTAGE}},
         {{DriveState::SWITCHED_ON, DriveState::READY_TO_SWITCH_ON}, {DriveTransition::SHUTDOWN}},
         {{DriveState::SWITCHED_ON, DriveState::SWITCH_ON_DISABLED}, {DriveTransition::DISABLE_VOLTAGE}},
         {{DriveState::READY_TO_SWITCH_ON, DriveState::SWITCH_ON_DISABLED}, {DriveTransition::DISABLE_VOLTAGE}},
-        {{DriveState::READY_TO_SWITCH_ON, DriveState::SWITCHED_ON}, {DriveTransition::SWITCH_ON}},
         {{DriveState::QUICK_STOP_ACTIVE, DriveState::SWITCH_ON_DISABLED}, {DriveTransition::DISABLE_VOLTAGE}}
     };
 }
@@ -237,7 +231,7 @@ bool DriveStatusMachine::requestState(DriveState goalState)
     return true;
 }
 
-DriveState parseStatusword(std::uint16_t statusword)
+DriveState DriveStatusMachine::parseStatusword(std::uint16_t statusword)
 {
     return parseDriveState(statusword);
 }

libraries/CanBusSharerLib/DriveStatusMachine.hpp

@@ -35,8 +35,8 @@ enum class DriveTransition
     DISABLE_VOLTAGE,
     QUICK_STOP,
     ENABLE_OPERATION,
-    DISABLE_OPERATION, // same as switch on
-    FAULT_RESET
+    FAULT_RESET,
+    DISABLE_OPERATION = SWITCH_ON
 };
 
 class DriveStatusMachine

libraries/CanBusSharerLib/EmcyConsumer.cpp

@@ -90,8 +90,13 @@ bool EmcyConsumer::configure(std::uint16_t inhibitTime)
     return sdo->download("Inhibit time EMCY", inhibitTime, 0x1015);
 }
 
-void EmcyConsumer::accept(const std::uint8_t * data)
+bool EmcyConsumer::accept(const std::uint8_t * data)
 {
+    if (!callback)
+    {
+        return false;
+    }
+
     std::uint16_t code;
     std::uint8_t reg;
     std::uint8_t msef[5];
@@ -103,4 +108,5 @@ void EmcyConsumer::accept(const std::uint8_t * data)
     code_t codeToMsg = std::make_pair(code, codeRegistry->codeToMessage(code));
 
     callback(codeToMsg, reg, msef);
+    return true;
 }

libraries/CanBusSharerLib/EmcyConsumer.hpp

@@ -33,7 +33,7 @@ class EmcyConsumer final
 
     bool configure(std::uint16_t inhibitTime);
 
-    void accept(const std::uint8_t * data);
+    bool accept(const std::uint8_t * data);
 
     template<typename T>
     void setErrorCodeRegistry()

libraries/CanBusSharerLib/NmtProtocol.cpp

@@ -17,6 +17,11 @@ bool NmtProtocol::setupHeartbeat(std::uint16_t period)
 
 bool NmtProtocol::accept(const std::uint8_t * data)
 {
+    if (!callback)
+    {
+        return false;
+    }
+
     NmtState state;
 
     switch (data[0])

libraries/CanBusSharerLib/NmtProtocol.hpp

@@ -13,7 +13,7 @@
 namespace roboticslab
 {
 
-enum class NmtService
+enum class NmtService : std::uint8_t
 {
     START_REMOTE_NODE = 1,
     STOP_REMOTE_NODE = 2,
@@ -22,7 +22,7 @@ enum class NmtService
     RESET_COMMUNICATION = 130
 };
 
-enum class NmtState
+enum class NmtState : std::uint8_t
 {
     BOOTUP = 0,
     STOPPED = 4,

libraries/CanBusSharerLib/PdoProtocol.cpp

@@ -133,12 +133,12 @@ bool PdoProtocol::configure(const PdoConfiguration & conf)
         bits.set(30, !*conf.priv->rtr);
     }
 
-    if (!sdo->download(std::string("COB-ID ") + pdoType, bits.to_ulong(), commIdx, 0x01))
+    if (!sdo->download<std::uint32_t>(std::string("COB-ID ") + pdoType, bits.to_ulong(), commIdx, 0x01))
     {
         return false;
     }
 
-    if (conf.priv->transmissionType && !sdo->download("Transmission type", (std::uint8_t)*conf.priv->transmissionType, commIdx, 0x02))
+    if (conf.priv->transmissionType && !sdo->download("Transmission type", static_cast<std::uint8_t>(*conf.priv->transmissionType), commIdx, 0x02))
     {
         return false;
     }
@@ -197,7 +197,7 @@ bool PdoProtocol::configure(const PdoConfiguration & conf)
     {
         bits.reset(31);
 
-        if (!sdo->download(std::string("COB-ID ") + pdoType, bits.to_ulong(), commIdx, 0x01))
+        if (!sdo->download<std::uint32_t>(std::string("COB-ID ") + pdoType, bits.to_ulong(), commIdx, 0x01))
         {
             return false;
         }

libraries/CanBusSharerLib/PdoProtocol.hpp

@@ -162,7 +162,7 @@ class TransmitPdo final : public PdoProtocol
     { }
 
     bool accept(const std::uint8_t * data, unsigned int size)
-    { return callback(data, size); }
+    { return (bool)callback && callback(data, size); }
 
     template<typename... Ts, typename Fn>
     void registerHandler(const Fn & fn)

libraries/CanBusSharerLib/SdoClient.cpp

@@ -91,7 +91,7 @@ namespace
 
 bool SdoClient::send(const std::uint8_t * msg)
 {
-    return sender->prepareMessage({cobRx, 8, msg});
+    return sender->prepareMessage({getCobIdRx(), 8, msg});
 }
 
 std::string SdoClient::msgToStr(std::uint16_t cob, const std::uint8_t * msgData)
@@ -116,7 +116,7 @@ bool SdoClient::uploadInternal(const std::string & name, void * data, std::uint3
 
     std::bitset<8> bitsReceived(responseMsg[0]);
 
-    if (bitsReceived[1]) // expedited trasfer
+    if (bitsReceived[1]) // expedited transfer
     {
         if (bitsReceived[0]) // data size is indicated in 'n'
         {
@@ -195,7 +195,7 @@ bool SdoClient::downloadInternal(const std::string & name, const void * data, st
         indicationBits.set(1); // e: transfer type
         const std::uint8_t n = 4 - size;
         indicationMsg[0] = indicationBits.to_ulong() + (n << 2);
-        std::memcpy(indicationMsg + 4, &data, size);
+        std::memcpy(indicationMsg + 4, data, size);
 
         std::uint8_t confirmMsg[8];
         return performTransfer(name, indicationMsg, confirmMsg);
@@ -242,7 +242,7 @@ bool SdoClient::downloadInternal(const std::string & name, const void * data, st
                 return false;
             }
 
-            if (!std::bitset<8>(confirmMsg[0])[4] != bitsSent[4])
+            if (std::bitset<8>(confirmMsg[0])[4] != bitsSent[4])
             {
                 CD_ERROR("SDO segmented download: toggle bit mismatch.\n");
                 return false;
@@ -259,6 +259,38 @@ bool SdoClient::downloadInternal(const std::string & name, const void * data, st
     return true;
 }
 
+bool SdoClient::upload(const std::string & name, std::string & s, std::uint16_t index, std::uint8_t subindex)
+{
+    const std::uint32_t maxLen = 100; // arbitrary high value
+    char buf[maxLen] = {0};
+
+    if (!uploadInternal(name, buf, maxLen, index, subindex))
+    {
+        return false;
+    }
+
+    s = buf;
+    return true;
+}
+
+bool SdoClient::upload(const std::string & name, const std::function<void(const std::string & s)> & fn, std::uint16_t index, std::uint8_t subindex)
+{
+    std::string s;
+
+    if (!upload(name, s, index, subindex))
+    {
+        return false;
+    }
+
+    fn(s);
+    return true;
+}
+
+bool SdoClient::download(const std::string & name, const std::string & s, std::uint16_t index, std::uint8_t subindex)
+{
+    return downloadInternal(name, s.data(), s.size(), index, subindex);
+}
+
 bool SdoClient::performTransfer(const std::string & name, const std::uint8_t * req, std::uint8_t * resp)
 {
     const std::string & reqStr = msgToStr(cobRx, req);