diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py
index c232e765d7..cf8c408efb 100644
--- a/Tools/ardupilotwaf/ardupilotwaf.py
+++ b/Tools/ardupilotwaf/ardupilotwaf.py
@@ -156,6 +156,22 @@ def find_tests(bld, use=[]):
             use=use,
         )
 
+def find_benchmarks(bld, use=[]):
+    if not bld.env.HAS_GBENCHMARK:
+        return
+
+    includes = [bld.srcnode.abspath() + '/benchmarks/']
+
+    for f in bld.path.ant_glob(incl='*.cpp'):
+        target = f.change_ext('.' + bld.env.BOARD)
+        bld.program(
+            features=['gbenchmark'],
+            target=target,
+            includes=includes,
+            source=[f],
+            use=use,
+        )
+
 def test_summary(bld):
     from io import BytesIO
     import sys
diff --git a/wscript b/wscript
index f0b632acf5..9ff82b03b2 100644
--- a/wscript
+++ b/wscript
@@ -163,6 +163,13 @@ def configure(cfg):
     cfg.load('compiler_cxx compiler_c')
     cfg.load('clang_compilation_database')
     cfg.load('waf_unit_test')
+    cfg.load('gbenchmark')
+
+    cfg.start_msg('Benchmarks')
+    if cfg.env.HAS_GBENCHMARK:
+        cfg.end_msg('enabled')
+    else:
+        cfg.end_msg('disabled', color='YELLOW')
 
     cfg.env.HAS_GTEST = cfg.check_cxx(
         lib='gtest',
@@ -234,11 +241,17 @@ def build(bld):
     board_tests = ['libraries/%s/**/tests' % l for l in bld.env.AP_LIBRARIES]
     tests.extend(collect_dirs_to_recurse(bld, board_tests))
 
+    benchmarks = collect_dirs_to_recurse(bld,
+                                         '**/benchmarks',
+                                         excl='modules Tools libraries/AP_HAL_* libraries/SITL')
+    board_benchmarks = ['libraries/%s/**/benchmarks' % l for l in bld.env.AP_LIBRARIES]
+    benchmarks.extend(collect_dirs_to_recurse(bld, board_benchmarks))
+
     hal_examples = []
     for l in bld.env.AP_LIBRARIES:
         hal_examples.extend(collect_dirs_to_recurse(bld, 'libraries/' + l + '/examples/*'))
 
-    for d in vehicles + tools + examples + hal_examples + tests:
+    for d in vehicles + tools + examples + hal_examples + tests + benchmarks:
         bld.recurse(d)
 
     if bld.cmd == 'check':