diff --git a/libuavcan/include/uavcan/internal/map.hpp b/libuavcan/include/uavcan/internal/map.hpp
index 97b6ca454a..704166f76a 100644
--- a/libuavcan/include/uavcan/internal/map.hpp
+++ b/libuavcan/include/uavcan/internal/map.hpp
@@ -5,6 +5,7 @@
 #pragma once
 
 #include <cassert>
+#include <cstdlib>
 #include <stdint.h>
 #include <uavcan/internal/linked_list.hpp>
 #include <uavcan/internal/impl_constants.hpp>
@@ -173,7 +174,7 @@ public:
     }
 
     /// If entry with the same key already exists, it will be replaced
-    bool insert(const Key& key, const Value& value)
+    Value* insert(const Key& key, const Value& value)
     {
         assert(!(key == Key()));
         remove(key);
@@ -182,18 +183,18 @@ public:
         if (kv)
         {
             *kv = KVPair(key, value);
-            return true;
+            return &kv->value;
         }
 
         void* const praw = allocator_->allocate(sizeof(KVGroup));
         if (praw == NULL)
-            return false;
+            return NULL;
 
         KVGroup* const kvg = new (praw) KVGroup();
         assert(kvg);
         kvg->kvs[0] = KVPair(key, value);
         list_.insert(kvg);
-        return true;
+        return &kvg->kvs[0].value;
     }
 
     void remove(const Key& key)
diff --git a/libuavcan/test/map.cpp b/libuavcan/test/map.cpp
index 21c47900d5..f587a96871 100644
--- a/libuavcan/test/map.cpp
+++ b/libuavcan/test/map.cpp
@@ -43,17 +43,17 @@ TEST(Map, Basic)
     ASSERT_EQ(0, pool.getNumUsedBlocks());
 
     // Static insertion
-    ASSERT_TRUE(map->insert("1", "a"));
-    ASSERT_TRUE(map->insert("2", "b"));
+    ASSERT_EQ("a", *map->insert("1", "a"));
+    ASSERT_EQ("b", *map->insert("2", "b"));
     ASSERT_EQ(0, pool.getNumUsedBlocks());
     ASSERT_EQ(2, map->getNumStaticPairs());
     ASSERT_EQ(0, map->getNumDynamicPairs());
 
     // Dynamic insertion
-    ASSERT_TRUE(map->insert("3", "c"));
+    ASSERT_EQ("c", *map->insert("3", "c"));
     ASSERT_EQ(1, pool.getNumUsedBlocks());
 
-    ASSERT_TRUE(map->insert("4", "d"));
+    ASSERT_EQ("d", *map->insert("4", "d"));
     ASSERT_EQ(1, pool.getNumUsedBlocks());       // Assuming that at least 2 items fit one block
     ASSERT_EQ(2, map->getNumStaticPairs());
     ASSERT_EQ(2, map->getNumDynamicPairs());
@@ -104,12 +104,16 @@ TEST(Map, Basic)
     {
         const std::string key   = toString(i);
         const std::string value = toString(i);
-        const bool res = map->insert(key, value);  // Will override some from the above
-        if (!res)
+        std::string* res = map->insert(key, value);  // Will override some from the above
+        if (res == NULL)
         {
             ASSERT_LT(2, i);
             break;
         }
+        else
+        {
+            ASSERT_EQ(value, *res);
+        }
         max_key_integer = i;
     }
     std::cout << "Max key/value: " << max_key_integer << std::endl;