From a4ca9f1703079d3117f400cb67ad367abb85a054 Mon Sep 17 00:00:00 2001
From: Simon Rasmussen <zetlon@gmail.com>
Date: Thu, 13 Apr 2017 10:34:51 +0200
Subject: [PATCH] Changed masterboard IO data to bitset

---
 include/ur_modern_driver/bin_parser.h      | 10 ++++++++++
 include/ur_modern_driver/ur/master_board.h | 10 ++++++----
 src/ur/master_board.cpp                    | 10 +++++-----
 3 files changed, 21 insertions(+), 9 deletions(-)

diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h
index e5692f5..dbf6df5 100644
--- a/include/ur_modern_driver/bin_parser.h
+++ b/include/ur_modern_driver/bin_parser.h
@@ -6,6 +6,8 @@
 #include <cstddef>
 #include <cstring>
 #include <string>
+#include <array>
+#include <bitset>
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/types.h"
 
@@ -144,6 +146,14 @@ public:
     }
   }
 
+  template <typename T, size_t N>
+  void parse(std::bitset<N>& set)
+  {
+    T val;
+    parse(val);
+    set = std::bitset<N>(val);
+  }
+
   void consume()
   {
     buf_pos_ = buf_end_;
diff --git a/include/ur_modern_driver/ur/master_board.h b/include/ur_modern_driver/ur/master_board.h
index a0d9e7a..c625cbf 100644
--- a/include/ur_modern_driver/ur/master_board.h
+++ b/include/ur_modern_driver/ur/master_board.h
@@ -2,6 +2,7 @@
 
 #include <inttypes.h>
 #include <cstddef>
+#include <bitset>
 #include "ur_modern_driver/bin_parser.h"
 #include "ur_modern_driver/types.h"
 #include "ur_modern_driver/ur/state.h"
@@ -41,8 +42,9 @@ public:
   virtual bool parseWith(BinParser& bp);
   virtual bool consumeWith(URStatePacketConsumer& consumer);
 
-  int16_t digital_input_bits;
-  int16_t digital_output_bits;
+  
+  std::bitset<10> digital_input_bits;
+  std::bitset<10> digital_output_bits;
 
   uint8_t master_safety_state;
   bool master_on_off_state;
@@ -62,8 +64,8 @@ public:
   virtual bool parseWith(BinParser& bp);
   virtual bool consumeWith(URStatePacketConsumer& consumer);
 
-  int32_t digital_input_bits;
-  int32_t digital_output_bits;
+  std::bitset<18> digital_input_bits;
+  std::bitset<18> digital_output_bits;
 
   uint8_t safety_mode;
   bool in_reduced_mode;
diff --git a/src/ur/master_board.cpp b/src/ur/master_board.cpp
index ecada69..991b6ac 100644
--- a/src/ur/master_board.cpp
+++ b/src/ur/master_board.cpp
@@ -23,8 +23,8 @@ bool MasterBoardData_V1_X::parseWith(BinParser& bp)
   if (!bp.checkSize<MasterBoardData_V1_X>())
     return false;
 
-  bp.parse(digital_input_bits);
-  bp.parse(digital_output_bits);
+  bp.parse<uint16_t, 10>(digital_input_bits);
+  bp.parse<uint16_t, 10>(digital_output_bits);
 
   SharedMasterBoardData::parseWith(bp);
 
@@ -49,8 +49,8 @@ bool MasterBoardData_V3_0__1::parseWith(BinParser& bp)
   if (!bp.checkSize<MasterBoardData_V3_0__1>())
     return false;
 
-  bp.parse(digital_input_bits);
-  bp.parse(digital_output_bits);
+  bp.parse<uint32_t, 18>(digital_input_bits);
+  bp.parse<uint32_t, 18>(digital_output_bits);
 
   SharedMasterBoardData::parseWith(bp);
 
@@ -59,7 +59,7 @@ bool MasterBoardData_V3_0__1::parseWith(BinParser& bp)
   bp.parse(euromap67_interface_installed);
 
   if (euromap67_interface_installed)
-  {
+  {  
     if (!bp.checkSize(MasterBoardData_V3_0__1::EURO_SIZE))
       return false;