diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py
index 707263ce94..e0e84beaa8 100644
--- a/Tools/ardupilotwaf/boards.py
+++ b/Tools/ardupilotwaf/boards.py
@@ -406,7 +406,6 @@ class chibios(Board):
             '-Werror=unused-but-set-variable',
             '-Wno-missing-field-initializers',
             '-Wno-trigraphs',
-            '-Os',
             '-fno-strict-aliasing',
             '-fomit-frame-pointer',
             '-falign-functions=16',
@@ -435,7 +434,6 @@ class chibios(Board):
         bldnode = cfg.bldnode.make_node(self.name)
         env.BUILDROOT = bldnode.make_node('').abspath()
         env.LINKFLAGS = cfg.env.CPU_FLAGS + [
-            '-Os',
             '-fomit-frame-pointer',
             '-falign-functions=16',
             '-ffunction-sections',
diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py
index de78d79a80..98d216e9a0 100644
--- a/Tools/ardupilotwaf/chibios.py
+++ b/Tools/ardupilotwaf/chibios.py
@@ -233,6 +233,18 @@ def load_env_vars(env):
     if env.ENABLE_ASSERTS:
         env.CHIBIOS_BUILD_FLAGS += ' ENABLE_ASSERTS=yes'
 
+def setup_optimization(env):
+    '''setup optimization flags for build'''
+    if env.DEBUG:
+        OPTIMIZE = "-Og"
+    elif env.OPTIMIZE:
+        OPTIMIZE = env.OPTIMIZE
+    else:
+        OPTIMIZE = "-Os"
+    env.CFLAGS += [ OPTIMIZE ]
+    env.CXXFLAGS += [ OPTIMIZE ]
+    env.CHIBIOS_BUILD_FLAGS += ' USE_COPT=%s' % OPTIMIZE
+
 def configure(cfg):
     cfg.find_program('make', var='MAKE')
     #cfg.objcopy = cfg.find_program('%s-%s'%(cfg.env.TOOLCHAIN,'objcopy'), var='OBJCOPY', mandatory=True)
@@ -305,6 +317,7 @@ def configure(cfg):
     load_env_vars(cfg.env)
     if env.HAL_WITH_UAVCAN:
         setup_can_build(cfg)
+    setup_optimization(cfg.env)
 
 def pre_build(bld):
     '''pre-build hook to change dynamic sources'''