diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py
index 51c28d0d23..d4bcd7de8c 100644
--- a/Tools/ardupilotwaf/boards.py
+++ b/Tools/ardupilotwaf/boards.py
@@ -327,7 +327,6 @@ class chibios(Board):
             '-Wno-missing-field-initializers',
             '-Wno-trigraphs',
             '-Os',
-            '-g',
             '-fno-strict-aliasing',
             '-fomit-frame-pointer',
             '-falign-functions=16',
@@ -355,7 +354,6 @@ class chibios(Board):
         env.LINKFLAGS = [
             '-mcpu=cortex-m4',
             '-Os',
-            '-g',
             '-fomit-frame-pointer',
             '-falign-functions=16',
             '-ffunction-sections',
@@ -380,6 +378,14 @@ class chibios(Board):
             '-Wl,--gc-sections,--no-warn-mismatch,--library-path=/ld,--script=ldscript.ld,--defsym=__process_stack_size__=0x400,--defsym=__main_stack_size__=0x400',
         ]
 
+        if cfg.env.DEBUG:
+            env.CFLAGS += [
+                '-g',
+            ]
+            env.LINKFLAGS += [
+                '-g',
+            ]
+
         env.LIB += ['gcc', 'm']
 
         env.GIT_SUBMODULES += [