diff --git a/CMakeLists.txt b/CMakeLists.txt
index 382f23f..0d5a23e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -69,7 +69,7 @@ if(CATKIN_ENABLE_TESTING)
 
   find_package(catkin REQUIRED COMPONENTS rostest)
   add_rostest(tests/reader_writer_100hz.test)
-#  add_rostest(tests/reader_writer_100hz_extended.test)
+  add_rostest(tests/reader_writer_100hz_extended.test)
 endif()
 
 install(
diff --git a/include/kvaser_interface/kvaser_interface.h b/include/kvaser_interface/kvaser_interface.h
index 407d5cc..d4ba424 100644
--- a/include/kvaser_interface/kvaser_interface.h
+++ b/include/kvaser_interface/kvaser_interface.h
@@ -82,10 +82,30 @@ class MsgFlags
   public:
     friend bool operator==(const MsgFlags& lhs, const MsgFlags& rhs);
 
+    void clear()
+    {
+      rtr = false;
+      std_id = false;
+      ext_id = false;
+      wakeup_mode = false;
+      nerr_active = false;
+      error_frame = false;
+      tx_ack = false;
+      tx_rq = false;
+      msg_delayed = false;
+      single_shot = false;
+      tx_nack = false;
+      arb_lost = false;
+      fd_msg = false;
+      fd_bitrate_switch = false;
+      fd_sndr_err_pass_md = false;
+    }
+
     bool rtr = false;
     bool std_id = false;
     bool ext_id = false;
     bool wakeup_mode = false;
+    bool nerr_active = false;
     bool error_frame = false;
     bool tx_ack = false;
     bool tx_rq = false;
@@ -104,6 +124,7 @@ bool operator==(const MsgFlags& lhs, const MsgFlags& rhs)
     lhs.std_id == rhs.std_id &&
     lhs.ext_id == rhs.ext_id &&
     lhs.wakeup_mode == rhs.wakeup_mode &&
+    lhs.nerr_active == rhs.nerr_active &&
     lhs.error_frame == rhs.error_frame &&
     lhs.tx_ack == rhs.tx_ack &&
     lhs.tx_rq == rhs.tx_rq &&
@@ -121,6 +142,21 @@ class MsgErrFlags
   public:
     friend bool operator==(const MsgErrFlags& lhs, const MsgErrFlags& rhs);
 
+    void clear()
+    {
+      has_err = false;
+      hw_overrun_err = false;
+      sw_overrun_err = false;
+      stuff_err = false;
+      form_err = false;
+      crc_err = false;
+      bit0_err = false;
+      bit1_err = false;
+      any_overrun_err = false;
+      any_bit_err = false;
+      any_rx_err = false;
+    }
+
     bool has_err = false;
     bool hw_overrun_err = false;
     bool sw_overrun_err = false;
@@ -286,7 +322,8 @@ class KvaserCanUtils
     static std::vector<std::shared_ptr<KvaserChannel>> getChannels();
     static std::vector<std::shared_ptr<KvaserChannel>> getChannelsOnCard(const uint64_t &serialNo);
     static std::string returnStatusDesc(const ReturnStatuses &ret);
-    static void setMsgFlags(CanMsg *msg, const uint32_t &flags);
+    static void setMsgFromFlags(CanMsg *msg, const uint32_t &flags);
+    static void setFlagsFromMsg(const CanMsg &msg, uint32_t *flags);
 };
 
 }  // namespace CAN
diff --git a/src/kvaser_interface.cpp b/src/kvaser_interface.cpp
index e8fd17e..78a2c47 100644
--- a/src/kvaser_interface.cpp
+++ b/src/kvaser_interface.cpp
@@ -171,6 +171,14 @@ ReturnStatuses KvaserCan::read(CanMsg *msg)
     return ReturnStatuses::CHANNEL_CLOSED;
   }
 
+  // Make sure the incoming message is empty
+  msg->id = 0;
+  msg->dlc = 0;
+  msg->flags.clear();
+  msg->error_flags.clear();
+  msg->data.clear();
+  msg->timestamp = 0;
+
   int64_t id_proxy = 0;
   uint32_t flags = 0;
   char data[64];
@@ -188,7 +196,7 @@ ReturnStatuses KvaserCan::read(CanMsg *msg)
     msg->data.emplace_back(std::move(data[i]));
   }
 
-  KvaserCanUtils::setMsgFlags(msg, flags);
+  KvaserCanUtils::setMsgFromFlags(msg, flags);
 
   switch (ret)
   {
@@ -239,7 +247,7 @@ ReturnStatuses KvaserCan::write(CanMsg &&msg)
     return ReturnStatuses::DLC_PAYLOAD_MISMATCH;
 
   uint32_t flags = 0;
-  KvaserCanUtils::setMsgFlags(&msg, flags);
+  KvaserCanUtils::setFlagsFromMsg(msg, &flags);
 
   canStatus ret = canWrite(*handle, msg.id, &msg.data[0], msg.dlc, flags);
 
@@ -559,13 +567,14 @@ std::string KvaserCanUtils::returnStatusDesc(const ReturnStatuses& ret)
   return status_string;
 }
 
-void KvaserCanUtils::setMsgFlags(CanMsg *msg, const uint32_t &flags)
+void KvaserCanUtils::setMsgFromFlags(CanMsg *msg, const uint32_t &flags)
 {
   // Regular CAN message flags
   msg->flags.rtr = ((flags & canMSG_RTR) > 0);
   msg->flags.std_id = ((flags & canMSG_STD) > 0);
   msg->flags.ext_id = ((flags & canMSG_EXT) > 0);
   msg->flags.wakeup_mode = ((flags & canMSG_WAKEUP) > 0);
+  msg->flags.nerr_active = ((flags & canMSG_NERR) > 0);
   msg->flags.error_frame = ((flags & canMSG_ERROR_FRAME) > 0);
   msg->flags.tx_ack = ((flags & canMSG_TXACK) > 0);
   msg->flags.tx_rq = ((flags & canMSG_TXRQ) > 0);
@@ -592,3 +601,34 @@ void KvaserCanUtils::setMsgFlags(CanMsg *msg, const uint32_t &flags)
   msg->error_flags.any_bit_err = ((flags & canMSGERR_BIT) > 0);
   msg->error_flags.any_rx_err = ((flags & canMSGERR_BUSERR) > 0);
 }
+
+void KvaserCanUtils::setFlagsFromMsg(const CanMsg &msg, uint32_t *flags)
+{
+  // Regular CAN message flags
+  msg.flags.rtr ? *flags |= canMSG_RTR : *flags &= ~canMSG_RTR;
+  msg.flags.std_id ? *flags |= canMSG_STD : *flags &= ~canMSG_STD;
+  msg.flags.ext_id ? *flags |= canMSG_EXT : *flags &= ~canMSG_EXT;
+  msg.flags.wakeup_mode ? *flags |= canMSG_WAKEUP : *flags &= ~canMSG_WAKEUP;
+  msg.flags.nerr_active ? *flags |= canMSG_NERR : *flags &= ~canMSG_NERR;
+  msg.flags.error_frame ? *flags |= canMSG_ERROR_FRAME : *flags &= ~canMSG_ERROR_FRAME;
+  msg.flags.tx_ack ? *flags |= canMSG_TXACK : *flags &= ~canMSG_TXACK;
+  msg.flags.tx_rq ? *flags |= canMSG_TXRQ : *flags &= ~canMSG_TXRQ;
+  msg.flags.msg_delayed ? *flags |= canMSG_DELAY_MSG : *flags &= ~canMSG_DELAY_MSG;
+  msg.flags.single_shot ? *flags |= canMSG_SINGLE_SHOT : *flags &= ~canMSG_SINGLE_SHOT;
+  msg.flags.tx_nack ? *flags |= canMSG_TXNACK : *flags &= ~canMSG_TXNACK;
+  msg.flags.arb_lost ? *flags |= canMSG_ABL : *flags &= ~canMSG_ABL;
+
+  // CAN FD *flags
+  msg.flags.fd_msg ? *flags |= canFDMSG_FDF : *flags &= ~canFDMSG_FDF;
+  msg.flags.fd_bitrate_switch ? *flags |= canFDMSG_BRS : *flags &= ~canFDMSG_BRS;
+  msg.flags.fd_sndr_err_pass_md ? *flags |= canFDMSG_ESI : *flags &= ~canFDMSG_ESI;
+
+  // Error *flags
+  msg.error_flags.hw_overrun_err ? *flags |= canMSGERR_HW_OVERRUN : *flags &= ~canMSGERR_HW_OVERRUN;
+  msg.error_flags.sw_overrun_err ? *flags |= canMSGERR_SW_OVERRUN : *flags &= ~canMSGERR_SW_OVERRUN;
+  msg.error_flags.stuff_err ? *flags |= canMSGERR_STUFF : *flags &= ~canMSGERR_STUFF;
+  msg.error_flags.form_err ? *flags |= canMSGERR_FORM : *flags &= ~canMSGERR_FORM;
+  msg.error_flags.crc_err ? *flags |= canMSGERR_CRC : *flags &= ~canMSGERR_CRC;
+  msg.error_flags.bit0_err ? *flags |= canMSGERR_BIT0 : *flags &= ~canMSGERR_BIT0;
+  msg.error_flags.bit1_err ? *flags |= canMSGERR_BIT1 : *flags &= ~canMSGERR_BIT1;
+}
diff --git a/tests/kvaser_interface_tests.cpp b/tests/kvaser_interface_tests.cpp
index 333b2d9..be7c732 100644
--- a/tests/kvaser_interface_tests.cpp
+++ b/tests/kvaser_interface_tests.cpp
@@ -6,6 +6,7 @@
 */
 
 #include <kvaser_interface/kvaser_interface.h>
+#include <canstat.h>
 #include <gtest/gtest.h>
 
 using AS::CAN::CanMsg;
@@ -29,12 +30,13 @@ TEST(KvaserCanUtils, getChannels)
   ASSERT_EQ(chans.size(), 2);
 }
 
-/*
 TEST(KvaserCanUtils, setFlags)
 {
+  // All flags start false
   CanMsg msg;
 
-  KvaserCanUtils::setMsgFlags(&msg, 0xFFFFFFFF);
+  // Set all flags in msg to true
+  KvaserCanUtils::setMsgFromFlags(&msg, 0xFFFFFFFF);
   ASSERT_EQ(msg.flags.rtr, true);
   ASSERT_EQ(msg.flags.std_id, true);
   ASSERT_EQ(msg.flags.ext_id, true);
@@ -61,7 +63,8 @@ TEST(KvaserCanUtils, setFlags)
   ASSERT_EQ(msg.error_flags.any_bit_err, true);
   ASSERT_EQ(msg.error_flags.any_rx_err, true);
 
-  KvaserCanUtils::setMsgFlags(&msg, 0x00000000);
+  // Set all flags in msg to false
+  KvaserCanUtils::setMsgFromFlags(&msg, 0x00000000);
   ASSERT_EQ(msg.flags.rtr, false);
   ASSERT_EQ(msg.flags.std_id, false);
   ASSERT_EQ(msg.flags.ext_id, false);
@@ -87,8 +90,49 @@ TEST(KvaserCanUtils, setFlags)
   ASSERT_EQ(msg.error_flags.any_overrun_err, false);
   ASSERT_EQ(msg.error_flags.any_bit_err, false);
   ASSERT_EQ(msg.error_flags.any_rx_err, false);
+
+  uint32_t flags = 0;
+  // Set all flags in msg to true
+  KvaserCanUtils::setMsgFromFlags(&msg, 0xFFFFFFFF);
+
+  // Set all flags in int to true
+  KvaserCanUtils::setFlagsFromMsg(msg, &flags);
+
+  // Check information flags
+  ASSERT_GT((flags & canMSG_RTR), 0);
+  ASSERT_GT((flags & canMSG_STD), 0);
+  ASSERT_GT((flags & canMSG_EXT), 0);
+  ASSERT_GT((flags & canMSG_WAKEUP), 0);
+  ASSERT_GT((flags & canMSG_NERR), 0);
+  ASSERT_GT((flags & canMSG_ERROR_FRAME), 0);
+  ASSERT_GT((flags & canMSG_TXACK), 0);
+  ASSERT_GT((flags & canMSG_TXRQ), 0);
+  ASSERT_GT((flags & canMSG_DELAY_MSG), 0);
+  ASSERT_GT((flags & canMSG_SINGLE_SHOT), 0);
+  ASSERT_GT((flags & canMSG_TXNACK), 0);
+  ASSERT_GT((flags & canMSG_ABL), 0);
+
+  // Check CAN FD flags
+  ASSERT_GT((flags & canFDMSG_FDF), 0);
+  ASSERT_GT((flags & canFDMSG_BRS), 0);
+  ASSERT_GT((flags & canFDMSG_ESI), 0);
+
+  // Check error flags
+  ASSERT_GT((flags & canMSGERR_HW_OVERRUN), 0);
+  ASSERT_GT((flags & canMSGERR_SW_OVERRUN), 0);
+  ASSERT_GT((flags & canMSGERR_STUFF), 0);
+  ASSERT_GT((flags & canMSGERR_FORM), 0);
+  ASSERT_GT((flags & canMSGERR_CRC), 0);
+  ASSERT_GT((flags & canMSGERR_BIT0), 0);
+  ASSERT_GT((flags & canMSGERR_BIT1), 0);
+
+  // Set all flags in msg to false
+  KvaserCanUtils::setMsgFromFlags(&msg, 0x00000000);
+
+  // Set all flags in int to false
+  KvaserCanUtils::setFlagsFromMsg(msg, &flags);
+  ASSERT_EQ(flags, 0x00000000);
 }
-*/
 
 void dummyCb()
 {
@@ -193,7 +237,6 @@ TEST(KvaserCan, readWriteTests)
   // Check that sent and received messages are identical
   ASSERT_EQ(sent_msg, rcvd_msg);
 
-  /*
   // Send extended CAN ID with std flag false and extended flag true
   sent_msg.id = 0x15555555;
   sent_msg.dlc = 8;
@@ -208,7 +251,6 @@ TEST(KvaserCan, readWriteTests)
 
   // Check that sent and received messages are identical
   ASSERT_EQ(sent_msg, rcvd_msg);
-  */
 
   writer_stat = kv_can_writer.close();
   reader_stat = kv_can_reader.close();
@@ -256,7 +298,6 @@ TEST(KvaserCan, writeTests)
   writer_stat = kv_can_writer.write(CanMsg(sent_msg));
   ASSERT_EQ(writer_stat, ReturnStatuses::OK);
 
-  /*
   // Send extended CAN ID with 0 DLC and 0 payload bytes
   sent_msg.id = 0x15555555;
   sent_msg.dlc = 0;
@@ -288,7 +329,6 @@ TEST(KvaserCan, writeTests)
   sent_msg.flags.ext_id = false;
   writer_stat = kv_can_writer.write(CanMsg(sent_msg));
   ASSERT_EQ(writer_stat, ReturnStatuses::WRITE_FAILED);
-  */
 
   writer_stat = kv_can_writer.close();
   ASSERT_EQ(writer_stat, ReturnStatuses::OK);