diff --git a/conf/Makefile.chibios b/conf/Makefile.chibios
index 5beb004bd0..2dc1fa1467 100644
--- a/conf/Makefile.chibios
+++ b/conf/Makefile.chibios
@@ -100,7 +100,7 @@ ifeq (,$(findstring $(RTOS_DEBUG),0 FALSE))
 else
   CH_OPT ?= 2 -ggdb
 endif
-  USE_OPT = -std=gnu99 -O$(CH_OPT) \
+  USE_OPT = -O$(CH_OPT) \
 	-falign-functions=16 -fomit-frame-pointer \
 	-W -Wall \
 	$(PPRZ_DEFINITION)
@@ -108,7 +108,7 @@ endif
 
 # C specific options here (added to USE_OPT).
 ifeq ($(USE_COPT),)
-  USE_COPT =
+  USE_COPT = -std=gnu99 
 endif
 
 # C++ specific options here (added to USE_OPT).
@@ -224,7 +224,8 @@ CSRC = $(STARTUPSRC) \
        $(FATFSSRC) \
        $(CHIBIOS_BOARD_MAIN)
 
-ECSRC = $($(TARGET).srcs)
+ECSRC = $(filter %.c, $($(TARGET).srcs))
+ECPPSRC = $(filter %.cpp, $($(TARGET).srcs))
 
 # C++ sources that can be compiled in ARM or THUMB mode depending on the global
 # setting.
@@ -275,8 +276,11 @@ CPPC = $(TRGT)g++
 # Enable loading with g++ only if you need C++ runtime support.
 # NOTE: You can use C++ even without C++ support if you are careful. C++
 #       runtime support makes code size explode.
+ifeq ($(ECPPSRC),)
 LD   = $(TRGT)gcc
-#LD   = $(TRGT)g++
+else
+LD   = $(TRGT)g++
+endif
 CP   = $(TRGT)objcopy
 AS   = $(TRGT)gcc -x assembler-with-cpp
 AR   = $(TRGT)ar
diff --git a/conf/Makefile.stm32 b/conf/Makefile.stm32
index 3a9b04f89e..3b9db227e1 100644
--- a/conf/Makefile.stm32
+++ b/conf/Makefile.stm32
@@ -48,6 +48,7 @@ DEBUG_FLAGS ?= -ggdb3
 
 CSTANDARD   ?= -std=gnu99
 CXXSTANDARD ?= -std=c++98
+CINCS = $(INCLUDES) -I$(PAPARAZZI_SRC)/sw/include
 
 # input files
 SRCS = $($(TARGET).srcs)
@@ -107,6 +108,9 @@ CXXFLAGS += $(FLOAT_ABI)
 CXXFLAGS += $(ARCH_FLAGS)
 CXXFLAGS += $(USER_CFLAGS)
 CXXFLAGS += $(WARN_FLAGS)
+CXXFLAGS += $(CINCS)
+CXXFLAGS += $($(TARGET).CFLAGS)
+CXXFLAGS += $(BOARD_CFLAGS)
 
 
 CFLAGS += -I. -I./$(ARCH) -I../ext/libopencm3/include $(INCLUDES)
@@ -128,9 +132,11 @@ CFLAGS += $(BOARD_CFLAGS)
 ifneq ($(ARCH_L), )
 ifeq ($(ARCH_L),f4)
 CFLAGS += -DSTM32F4
+CXXFLAGS += -DSTM32F4
 endif
 else
 CFLAGS += -DSTM32F1
+CXXFLAGS += -DSTM32F1
 endif
 
 # with cortex-m3 and m4 unaligned data can still be read
@@ -227,7 +233,7 @@ sym: $(OBJDIR)/$(TARGET).sym
 .PRECIOUS : $(OBJS) $(AOBJ)
 %.elf: $(OBJS) $(AOBJ) | $(OBJDIR)
 	@echo LD $@
-	$(Q)$(LD) $(LDFLAGS) $($(TARGET).LDFLAGS) -o $@ $(COBJ) $(AOBJ) $(LDLIBS)
+	$(Q)$(LD) $(LDFLAGS) $($(TARGET).LDFLAGS) -o $@ $(OBJS) $(AOBJ) $(LDLIBS)
 
 # Compile: create object files from C source files.
 $(OBJDIR)/%.o : %.c $(OBJDIR)/../Makefile.ac
diff --git a/conf/chibios/chibios_extra_rules.mk b/conf/chibios/chibios_extra_rules.mk
index 5f9d3e42c6..cde53e79e5 100644
--- a/conf/chibios/chibios_extra_rules.mk
+++ b/conf/chibios/chibios_extra_rules.mk
@@ -19,6 +19,21 @@ else
 	@$(CC) -c $(CFLAGS) $(TOPT) -I. $(IINCDIR) $< -o $@
 endif
 
-OBJS	  += $(ECOBJS)
+ECPPOBJS		= $(sort $(addprefix $(OBJDIR)/, $(ECPPSRC:.cpp=.o)))
+
+$(ECPPOBJS) : $(OBJDIR)/%.o : %.cpp Makefile
+ifeq ($(USE_VERBOSE_COMPILE),yes)
+	@echo
+	VPATH=
+	test -d $(dir $@) || mkdir -p $(dir $@)
+	$(CPPC) -c $(CPPFLAGS) $(TOPT) -I. $(IINCDIR) $< -o $@
+else
+	@echo Compiling $(<F)
+	@VPATH=
+	@test -d $(dir $@) || mkdir -p $(dir $@)
+	@$(CPPC) -c $(CPPFLAGS) $(TOPT) -I. $(IINCDIR) $< -o $@
+endif
+
+OBJS	  += $(ECOBJS) $(ECPPOBJS)
 
 # *** EOF ***