diff --git a/Tools/ardupilotwaf/gbenchmark.py b/Tools/ardupilotwaf/gbenchmark.py
index 9ce2ee1b00..1d18961ca4 100644
--- a/Tools/ardupilotwaf/gbenchmark.py
+++ b/Tools/ardupilotwaf/gbenchmark.py
@@ -6,37 +6,10 @@ gbenchmark is a Waf tool for benchmark builds in Ardupilot
 """
 
 from waflib import Build, Context, Task
+from waflib.Configure import conf
 from waflib.TaskGen import feature, before_method, after_method
 from waflib.Errors import WafError
 
-def _configure_cmake(ctx, bldnode):
-    env = ctx.env
-    my_build_node = bldnode.find_dir(env.GBENCHMARK_BUILD_REL)
-    if not my_build_node:
-        bldnode.make_node(env.GBENCHMARK_BUILD_REL).mkdir()
-
-    prefix_node = bldnode.make_node(env.GBENCHMARK_PREFIX_REL)
-
-    cmake_vars = {
-        'CMAKE_BUILD_TYPE': 'Release',
-        'CMAKE_INSTALL_PREFIX:PATH': prefix_node.abspath(),
-    }
-
-    cmake_vars = ' '.join("-D%s='%s'" % v for v in cmake_vars.items())
-
-    cmd = '%s %s %s %s' % (
-              env.CMAKE[0],
-              env.GBENCHMARK_SRC,
-              cmake_vars,
-              env.GBENCHMARK_GENERATOR_OPTION
-          )
-
-    ctx.cmd_and_log(
-        cmd,
-        cwd=env.GBENCHMARK_BUILD,
-        quiet=Context.BOTH,
-    )
-
 def configure(cfg):
     env = cfg.env
     env.HAS_GBENCHMARK = False
@@ -49,30 +22,12 @@ def configure(cfg):
         )
         return
 
-    cfg.find_program('cmake', mandatory=False)
+    cfg.load('cmake')
 
-    if not env.CMAKE:
-        return
-
-    env.GBENCHMARK_CMAKE_GENERATOR = None
-
-    cfg.find_program(['ninja', 'ninja-build'], var='NINJA', mandatory=False)
-    if env.NINJA:
-        env.GBENCHMARK_CMAKE_GENERATOR = 'Ninja'
-
-    env.GBENCHMARK_GENERATOR_OPTION = ''
-    if env.GBENCHMARK_CMAKE_GENERATOR:
-        env.GBENCHMARK_GENERATOR_OPTION = '-G%s' % env.GBENCHMARK_CMAKE_GENERATOR
+    env.GBENCHMARK_PREFIX_REL = 'gbenchmark'
 
     bldnode = cfg.bldnode.make_node(cfg.variant)
-    prefix_node = bldnode.make_node('gbenchmark')
-    my_build_node = bldnode.make_node('gbenchmark_build')
-    my_src_node = cfg.srcnode.make_node('modules/gbenchmark')
-
-    env.GBENCHMARK_PREFIX_REL = prefix_node.path_from(bldnode)
-    env.GBENCHMARK_BUILD = my_build_node.abspath()
-    env.GBENCHMARK_BUILD_REL = my_build_node.path_from(bldnode)
-    env.GBENCHMARK_SRC = my_src_node.abspath()
+    prefix_node = bldnode.make_node(env.GBENCHMARK_PREFIX_REL)
 
     env.INCLUDES_GBENCHMARK = [prefix_node.make_node('include').abspath()]
     env.LIBPATH_GBENCHMARK = [prefix_node.make_node('lib').abspath()]
@@ -81,57 +36,32 @@ def configure(cfg):
     env.append_value('GIT_SUBMODULES', 'gbenchmark')
     env.HAS_GBENCHMARK = True
 
-class gbenchmark_build(Task.Task):
-    def __init__(self, *k, **kw):
-        super(gbenchmark_build, self).__init__(*k, **kw)
+@conf
+def libbenchmark(bld):
+    prefix_node = bld.bldnode.make_node(bld.env.GBENCHMARK_PREFIX_REL)
 
-        bldnode = self.generator.bld.bldnode
-        output_list = [
-            '%s/%s' % (self.env.GBENCHMARK_PREFIX_REL, path)
-            for path in (
-                'include/benchmark/benchmark.h',
-                'include/benchmark/macros.h',
-                'include/benchmark/benchmark_api.h',
-                'include/benchmark/reporter.h',
-                'lib/libbenchmark.a',
-            )
-        ]
-        self.outputs.extend([bldnode.make_node(f) for f in output_list])
+    gbenchmark_cmake = bld(
+        features='cmake_configure',
+        cmake_src='modules/gbenchmark',
+        cmake_bld='gbenchmark_build',
+        name='gbenchmark',
+        cmake_vars=dict(
+            CMAKE_BUILD_TYPE='Release',
+            CMAKE_INSTALL_PREFIX=prefix_node.abspath(),
+        )
+    )
 
-    def run(self):
-        bld = self.generator.bld
-        cmds = []
+    prefix_node = bld.bldnode.make_node(bld.env.GBENCHMARK_PREFIX_REL)
+    output_paths = (
+        'lib/libbenchmark.a',
+        'include/benchmark/benchmark.h',
+        'include/benchmark/macros.h',
+        'include/benchmark/benchmark_api.h',
+        'include/benchmark/reporter.h',
+    )
+    outputs = [prefix_node.make_node(path) for path in output_paths]
+    gbenchmark_cmake.cmake_build('install', target=outputs)
 
-        try:
-            # Generate build system first, if necessary
-            my_build_node = bld.bldnode.find_dir(self.env.GBENCHMARK_BUILD_REL)
-            if not (my_build_node and my_build_node.find_resource('CMakeCache.txt')):
-                _configure_cmake(bld, bld.bldnode)
-
-            # Build gbenchmark
-            cmd = '%s --build %s --target install' % (
-                    self.env.CMAKE[0],
-                    self.env.GBENCHMARK_BUILD
-                  )
-            bld.cmd_and_log(
-                cmd,
-                cwd=self.env.GBENCHMARK_BUILD,
-                quiet=Context.BOTH,
-            )
-            return 0
-        except WafError as e:
-            print(e)
-            if hasattr(e, 'stderr'):
-                print('')
-                print(e.stderr)
-            return 1
-
-    def __str__(self):
-        return 'Google Benchmark'
-
-gbenchmark_build = Task.always_run(Task.update_outputs(gbenchmark_build))
-
-build_task = None
 
 @feature('gbenchmark')
 @before_method('process_use')
@@ -142,12 +72,10 @@ def append_gbenchmark_use(self):
 
 @feature('gbenchmark')
 @after_method('process_source')
-def wait_for_gbenchmark_build(self):
-    global build_task
-
-    if not build_task:
-        build_task = self.create_task('gbenchmark_build')
+def wait_for_gbenchmark_install(self):
+    gbenchmark_install = self.bld.get_tgen_by_name('gbenchmark_install')
+    gbenchmark_install.post()
 
     for task in self.compiled_tasks:
-        task.set_run_after(build_task)
-        task.dep_nodes.extend(build_task.outputs)
+        task.set_run_after(gbenchmark_install.cmake_build_task)
+        task.dep_nodes.extend(gbenchmark_install.cmake_build_task.outputs)
diff --git a/wscript b/wscript
index 7b4c866480..9164f2922f 100644
--- a/wscript
+++ b/wscript
@@ -166,6 +166,9 @@ def _build_common_taskgens(bld):
 
     bld.libgtest()
 
+    if bld.env.HAS_GBENCHMARK:
+        bld.libbenchmark()
+
 def _build_recursion(bld):
     common_dirs_patterns = [
         # TODO: Currently each vehicle also generate its own copy of the