Merge pull request #3 from ryanborchert/kvaser_interface_impl

Kvaser interface impl
NewEagleRaptor · Nov 12, 2018 · 67954bc · 67954bc
2 parents 53fa416 + 2220c72
commit 67954bc
Show file tree

Hide file tree

Showing 60 changed files with 1,918 additions and 463 deletions.
diff --git a/.gitignore b/.gitignore
@@ -55,3 +55,5 @@ CATKIN_IGNORE
 *.dae
 *.stl
 *.pngnode_modules/
+.vscode/settings.json
+.vscode/settings.json
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -3,4 +3,4 @@
         "system_error": "cpp",
         "type_traits": "cpp"
     }
-}
+} 
diff --git a/src/dbc/include/dbc/DbcBuilder.h b/src/dbc/include/dbc/DbcBuilder.h
@@ -157,19 +157,19 @@ namespace NewEagle
       {
         std::string name = parser.ReadCIdentifier();
         char mux = parser.ReadNextChar("mux");
-        int32_t multiplexMode = -1;
+        NewEagle::MultiplexerMode multiplexMode = NewEagle::NONE;
         int32_t muxSwitch = 0;
 
         switch (mux) {
           case ':':
-            multiplexMode = 0;
+            multiplexMode = NewEagle::NONE;
             break;
           case 'M':
-            multiplexMode = 1;
+            multiplexMode = NewEagle::MUX_SWITCH;
             parser.SeekSeparator(':');
             break;
           case 'm':
-            multiplexMode = 2;
+            multiplexMode = NewEagle::MUX_SIGNAL;
             muxSwitch = parser.ReadInt();
             parser.SeekSeparator(':');
             break;
@@ -232,9 +232,19 @@ namespace NewEagle
 
         // Need to include Min, Max, DataType, MuxSwitch, Unit, Receiver
         // Find a way to include the DLC...
-        NewEagle::DbcSignal signal(8, gain, offset, startBit, endianness, length, sign, name);
-        signal.SetDataType(type);
-        return signal;
+        NewEagle::DbcSignal* signal;
+
+        if (NewEagle::MUX_SIGNAL == multiplexMode) {
+          //NewEagle::DbcSignal signal(8, gain, offset, startBit, endianness, length, sign, name, multiplexMode, muxSwitch);
+          signal = new NewEagle::DbcSignal(8, gain, offset, startBit, endianness, length, sign, name, multiplexMode, muxSwitch);
+        }
+        else {
+          //NewEagle::DbcSignal signal(8, gain, offset, startBit, endianness, length, sign, name, multiplexMode);
+          signal = new NewEagle::DbcSignal(8, gain, offset, startBit, endianness, length, sign, name, multiplexMode);
+        }
+
+        signal->SetDataType(type);
+        return NewEagle::DbcSignal(*signal);
       }
 }
 

diff --git a/src/dbc/include/dbc/DbcMessage.h b/src/dbc/include/dbc/DbcMessage.h
@@ -92,6 +92,7 @@ namespace NewEagle
      uint32_t GetRawId();
      void SetComment(NewEagle::DbcMessageComment comment);
      std::map<std::string, NewEagle::DbcSignal>* GetSignals();
+     bool AnyMultiplexedSignals();
 
    private:
      std::map<std::string, NewEagle::DbcSignal> _signals;

diff --git a/src/dbc/include/dbc/DbcSignal.h b/src/dbc/include/dbc/DbcSignal.h
@@ -67,6 +67,13 @@ namespace NewEagle
     DOUBLE = 2
   };
 
+  enum MultiplexerMode
+  {
+    NONE = 0,
+    MUX_SWITCH = 1,
+    MUX_SIGNAL = 2
+  };
+
   class DbcSignal {
     public:
       DbcSignal(
@@ -77,7 +84,20 @@ namespace NewEagle
         ByteOrder endianness,
         uint8_t length,
         SignType sign,
-        std::string name);
+        std::string name, 
+        MultiplexerMode multiplexerMode);
+
+      DbcSignal(
+        uint8_t dlc,
+        double gain,
+        double offset,
+        uint8_t startBit,
+        ByteOrder endianness,
+        uint8_t length,
+        SignType sign,
+        std::string name, 
+        MultiplexerMode multiplexerMode,
+        int32_t multiplexerSwitch);
 
       ~DbcSignal();
 
@@ -96,6 +116,8 @@ namespace NewEagle
       double GetInitialValue();
       DataType GetDataType();
       void SetDataType(DataType type);
+      MultiplexerMode GetMultiplexerMode() const;
+      int32_t GetMultiplexerSwitch() const;      
 
     private:
       uint8_t _dlc;
@@ -110,6 +132,8 @@ namespace NewEagle
       NewEagle::DbcSignalComment _comment;
       double _initialValue;
       DataType _type;
+      MultiplexerMode _multiplexerMode;
+      int32_t _multiplexerSwitch;
   };
 }
 

diff --git a/src/dbc/src/DbcBuilder.cpp b/src/dbc/src/DbcBuilder.cpp
@@ -64,7 +64,7 @@ namespace NewEagle
     NewEagle::DbcMessage currentMessage;
 
     int32_t cnt = 0;
-    int32_t yyy = 0;
+
     while (std::getline(f, line, '\n'))
     {
       lineNumber++;
@@ -86,7 +86,6 @@ namespace NewEagle
       {
         try
         {
-          yyy++;
           currentMessage =  ReadMessage(parser);
           currentMessage.SetRawText(line);
           dbc.AddMessage(currentMessage.GetName(), currentMessage);

diff --git a/src/dbc/src/DbcMessage.cpp b/src/dbc/src/DbcMessage.cpp
@@ -96,9 +96,36 @@ namespace NewEagle
     uint8_t *ptr = (uint8_t*)frame.data.elems;
     memset(ptr, 0x00, 8);
 
-    for(std::map<std::string, NewEagle::DbcSignal>::iterator it = _signals.begin(); it != _signals.end(); it++)
-    {
-      Pack(ptr, it->second);
+    if(!AnyMultiplexedSignals()) {
+      for(std::map<std::string, NewEagle::DbcSignal>::iterator it = _signals.begin(); it != _signals.end(); it++) {
+        Pack(ptr, it->second);
+      }    
+    }
+    else{
+      // Start by looping through an only setting signals that are not multiplexed
+      // While we're at it, we can pick out the mutliplexer switch.  
+      // Perform a second loop to find the mulitplexed signal based on the multiplexer switch.
+
+      NewEagle::DbcSignal* muxSwitch; // only one multiplexer switch per message is allowed
+
+      for(std::map<std::string, NewEagle::DbcSignal>::iterator it = _signals.begin(); it != _signals.end(); it++) {
+        if (NewEagle::NONE == it->second.GetMultiplexerMode()) {
+          Pack(ptr, it->second);
+        }                
+        if (NewEagle::MUX_SWITCH == it->second.GetMultiplexerMode()) {
+          muxSwitch = &it->second;
+          Pack(ptr, it->second);
+        }
+      }  
+
+      for(std::map<std::string, NewEagle::DbcSignal>::iterator it = _signals.begin(); it != _signals.end(); it++) {
+        if (NewEagle::MUX_SIGNAL == it->second.GetMultiplexerMode()) {
+          if (muxSwitch->GetResult() == it->second.GetMultiplexerSwitch()) {
+            Pack(ptr, it->second);
+          }
+
+        }                  
+      }
     }
 
     return frame;
@@ -108,11 +135,41 @@ namespace NewEagle
   {
     uint8_t *ptr = (uint8_t*)msg->data.elems;
 
-    for(std::map<std::string, NewEagle::DbcSignal>::iterator it = _signals.begin(); it != _signals.end(); it++)
-    {
-      double res = Unpack(ptr, it->second);
-      it->second.SetResult(res);
+    if(!AnyMultiplexedSignals()) {
+      for(std::map<std::string, NewEagle::DbcSignal>::iterator it = _signals.begin(); it != _signals.end(); it++) {
+        double res = Unpack(ptr, it->second);
+        it->second.SetResult(res);
+      }
     }
+    else{
+      // Start by looping through an only setting signals that are not multiplexed
+      // While we're at it, we can pick out the mutliplexer switch.  
+      // Perform a second loop to find the mulitplexed signal based on the multiplexer switch.
+
+      NewEagle::DbcSignal* muxSwitch; // only one multiplexer switch per message is allowed
+
+      for(std::map<std::string, NewEagle::DbcSignal>::iterator it = _signals.begin(); it != _signals.end(); it++) {
+        if (NewEagle::NONE == it->second.GetMultiplexerMode()) {
+          double res = Unpack(ptr, it->second);
+          it->second.SetResult(res);
+        }                
+        if (NewEagle::MUX_SWITCH == it->second.GetMultiplexerMode()) {
+          muxSwitch = &it->second;
+          double res = Unpack(ptr, it->second);
+          it->second.SetResult(res);
+        }
+      }  
+
+      for(std::map<std::string, NewEagle::DbcSignal>::iterator it = _signals.begin(); it != _signals.end(); it++) {
+        if (NewEagle::MUX_SIGNAL == it->second.GetMultiplexerMode()) {
+          if (muxSwitch->GetResult() == it->second.GetMultiplexerSwitch()) {
+            double res = Unpack(ptr, it->second);
+            it->second.SetResult(res);
+          }
+
+        }                  
+      }
+    }    
 
   }
 
@@ -135,10 +192,6 @@ namespace NewEagle
     NewEagle::DbcSignal* signal = &it->second;
 
     return signal;
-
-    // borchert-ne - keep around for now, Useful when debugging pack/unpack code.
-    //NewEagle::DbcSignal WhlAngleVelocity(8, 1.0, 0.0, 16, NewEagle::LITTLE_END, 16, NewEagle::SIGNED);
-    //return WhlAngleVelocity;
   }
 
   void DbcMessage::SetRawText(std::string rawText)
@@ -160,4 +213,17 @@ namespace NewEagle
   {
     return &_signals;
   }
+
+  bool DbcMessage::AnyMultiplexedSignals()
+  {
+    for(std::map<std::string, NewEagle::DbcSignal>::iterator it = _signals.begin(); it != _signals.end(); it++)
+    {
+      if (NewEagle::MUX_SWITCH == it->second.GetMultiplexerMode())
+      {
+        return true;
+      }
+    }
+
+    return false;
+  }
 }
diff --git a/src/dbc/src/DbcSignal.cpp b/src/dbc/src/DbcSignal.cpp
@@ -44,7 +44,8 @@ namespace NewEagle
     ByteOrder endianness,
     uint8_t length,
     SignType sign,
-    std::string name)
+    std::string name,
+    MultiplexerMode multiplexerMode)
   {
     _dlc = dlc;
     _gain  = gain;
@@ -54,6 +55,31 @@ namespace NewEagle
     _length = length;
     _sign = sign;
     _name = name;
+    _multiplexerMode = multiplexerMode;
+  }
+
+  DbcSignal::DbcSignal(
+    uint8_t dlc,
+    double gain,
+    double offset,
+    uint8_t startBit,
+    ByteOrder endianness,
+    uint8_t length,
+    SignType sign,
+    std::string name,
+    MultiplexerMode multiplexerMode,
+    int32_t multiplexerSwitch)
+  {
+    _dlc = dlc;
+    _gain  = gain;
+    _offset = offset;
+    _startBit = startBit;
+    _endianness = endianness;
+    _length = length;
+    _sign = sign;
+    _name = name;
+    _multiplexerMode = multiplexerMode;
+    _multiplexerSwitch = multiplexerSwitch;
   }
 
   DbcSignal::~DbcSignal()
@@ -133,4 +159,14 @@ namespace NewEagle
   {
     return _type;
   }
+
+  NewEagle::MultiplexerMode DbcSignal::GetMultiplexerMode() const
+  {
+    return _multiplexerMode;
+  }
+
+  int32_t DbcSignal::GetMultiplexerSwitch() const
+  {
+    return _multiplexerSwitch;
+  }
 }
-Original file line number
+Diff line change
@@ Expand Up / @@ -3,4 +3,4 @@ @@
             "system_error": "cpp",
             "type_traits": "cpp"
         }
-    }
+    }