diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py
index 6caf3f4a90..0e6679de83 100644
--- a/Tools/ardupilotwaf/ardupilotwaf.py
+++ b/Tools/ardupilotwaf/ardupilotwaf.py
@@ -104,6 +104,7 @@ def ap_program(bld,
     if use_legacy_defines:
         kw['defines'].extend(_get_legacy_defines(bld.path.name))
 
+    kw['cxxflags'] = kw.get('cxxflags', []) + ['-include', 'AP_Config.h']
     kw['features'] = kw.get('features', []) + bld.env.AP_PROGRAM_FEATURES
 
     program_groups = Utils.to_list(program_groups)
@@ -167,6 +168,7 @@ def ap_stlib(bld, **kw):
         lib_sources = lib_node.ant_glob(SOURCE_EXTS + UTILITY_SOURCE_EXTS)
         sources.extend(lib_sources)
 
+    kw['cxxflags'] = kw.get('cxxflags', []) + ['-include', 'AP_Config.h']
     kw['features'] = kw.get('features', []) + bld.env.AP_STLIB_FEATURES
     kw['source'] = sources
     kw['target'] = kw['name']
diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py
index fe01d35af4..3b344719b5 100644
--- a/Tools/ardupilotwaf/boards.py
+++ b/Tools/ardupilotwaf/boards.py
@@ -52,6 +52,8 @@ class Board:
             else:
                 cfg.env[k] = val
 
+        cfg.load('cxx_checks')
+
     def configure_env(self, cfg, env):
         # Use a dictionary instead of the convetional list for definitions to
         # make easy to override them. Convert back to list before consumption.
diff --git a/Tools/ardupilotwaf/cxx_checks.py b/Tools/ardupilotwaf/cxx_checks.py
new file mode 100644
index 0000000000..ed9c7a4b7b
--- /dev/null
+++ b/Tools/ardupilotwaf/cxx_checks.py
@@ -0,0 +1,84 @@
+"""
+WAF Tool that checks cxx parameters, creating the AP_Config
+header file.
+
+This tool needs compiler_cxx to be loaded, make sure you
+load them before this tool.
+
+Example::
+    def configure(cfg):
+        cfg.load('cxx_checks')
+"""
+
+def configure(cfg):
+    cfg.check_cxx(fragment='''
+                  #include <cmath>
+
+                  int main() {
+                    return std::isfinite(1.0f);
+                  }''',
+                  define_name="HAVE_CMATH_ISFINITE",
+                  mandatory=False)
+
+    cfg.check_cxx(fragment='''
+                  #include <cmath>
+
+                  int main() {
+                    return std::isinf(1.0f);
+                  }''',
+                  define_name="HAVE_CMATH_ISINF",
+                  mandatory=False)
+
+    cfg.check_cxx(fragment='''
+                  #include <cmath>
+
+                  int main() {
+                    return std::isnan(1.0f);
+                  }''',
+                  define_name="HAVE_CMATH_ISNAN",
+                  mandatory=False)
+
+    # NEED_CMATH_FUNCTION_STD_NAMESPACE checks are needed due to
+    # new gcc versions being more restrictive.
+    #
+    # Here we check if we need to add 'using std::function' to
+    # the function.
+    #
+    # Without these checks, in some cases, gcc points this as
+    # overloads or function duplication in scope.
+
+    cfg.check_cxx(fragment='''
+                  #include <math.h>
+                  #include <cmath>
+
+                  using std::isfinite;
+
+                  int main() {
+                    return isfinite((double)1);
+                  }''',
+                  define_name="NEED_CMATH_ISFINITE_STD_NAMESPACE",
+                  mandatory=False)
+
+    cfg.check_cxx(fragment='''
+                  #include <math.h>
+                  #include <cmath>
+
+                  using std::isinf;
+
+                  int main() {
+                    return isinf((double)1);
+                  }''',
+                  define_name="NEED_CMATH_ISINF_STD_NAMESPACE",
+                  mandatory=False)
+
+    cfg.check_cxx(fragment='''
+                  #include <math.h>
+                  #include <cmath>
+
+                  using std::isnan;
+
+                  int main() {
+                    return isnan((double)1);
+                  }''',
+                  define_name="NEED_CMATH_ISNAN_STD_NAMESPACE",
+                  mandatory=False)
diff --git a/wscript b/wscript
index bbe372d8da..171180f874 100644
--- a/wscript
+++ b/wscript
@@ -114,6 +114,8 @@ def configure(cfg):
     if cfg.options.submodule_update:
         cfg.env.SUBMODULE_UPDATE = True
 
+    cfg.write_config_header(os.path.join(cfg.variant, 'AP_Config.h'))
+
 def collect_dirs_to_recurse(bld, globs, **kw):
     dirs = []
     globs = Utils.to_list(globs)
@@ -217,6 +219,11 @@ def _build_recursion(bld):
         bld.recurse(d)
 
 def build(bld):
+    config_header = Utils.h_file(bld.bldnode.make_node('AP_Config.h').abspath())
+
+    bld.env.CCDEPS = config_header
+    bld.env.CXXDEPS = config_header
+
     bld.post_mode = Build.POST_LAZY
 
     bld.load('ardupilotwaf')