diff --git a/Makefile b/Makefile
index e8337696569..99991fe6a41 100644
--- a/Makefile
+++ b/Makefile
@@ -214,6 +214,7 @@ include $(ROOT)/make/tools.mk
 # Tool names
 #
 CROSS_CC    = $(ARM_SDK_PREFIX)gcc
+CROSS_CXX   = $(ARM_SDK_PREFIX)g++
 OBJCOPY     = $(ARM_SDK_PREFIX)objcopy
 SIZE        = $(ARM_SDK_PREFIX)size
 
@@ -231,14 +232,18 @@ endif
 
 DEBUG_FLAGS = -ggdb3 -DDEBUG
 
+CFLAGS_CC   = -std=gnu99 \
+              -Wstrict-prototypes
+
+CFLAGS_CXX  = -std=c++11 \
+              -fno-rtti
+
 CFLAGS      += $(ARCH_FLAGS) \
               $(LTO_FLAGS) \
               $(addprefix -D,$(OPTIONS)) \
               $(addprefix -I,$(INCLUDE_DIRS)) \
               $(DEBUG_FLAGS) \
-              -std=gnu99 \
               -Wall -Wextra -Wunsafe-loop-optimizations -Wdouble-promotion \
-              -Wstrict-prototypes \
               -Werror=switch \
               -ffunction-sections \
               -fdata-sections \
@@ -327,16 +332,16 @@ $(GENERATED_SETTINGS): $(SETTINGS_GENERATOR) $(SETTINGS_FILE) $(STAMP)
 CFLAGS                  += -I$(TARGET_OBJ_DIR)
 
 $(STAMP): .FORCE
-	$(V1) CPP_PATH="$(ARM_SDK_DIR)/bin" CFLAGS="$(CFLAGS)" TARGET=$(TARGET) ruby $(BUILD_STAMP) $(SETTINGS_FILE) $(STAMP)
+	$(V1) CPP_PATH="$(ARM_SDK_DIR)/bin" CFLAGS="$(CFLAGS_CC) $(CFLAGS)" TARGET=$(TARGET) ruby $(BUILD_STAMP) $(SETTINGS_FILE) $(STAMP)
 
 # Use a pattern rule, since they're different than normal rules.
 # See https://www.gnu.org/software/make/manual/make.html#Pattern-Examples
 %generated.h %generated.c:
 	$(V1) echo "settings.yaml -> settings_generated.h, settings_generated.c" "$(STDOUT)"
-	$(V1) CPP_PATH="$(ARM_SDK_DIR)/bin" CFLAGS="$(CFLAGS)" TARGET=$(TARGET) ruby $(SETTINGS_GENERATOR) . $(SETTINGS_FILE) -o $(TARGET_OBJ_DIR)
+	$(V1) CPP_PATH="$(ARM_SDK_DIR)/bin" CFLAGS="$(CFLAGS_CC) $(CFLAGS)" TARGET=$(TARGET) ruby $(SETTINGS_GENERATOR) . $(SETTINGS_FILE) -o $(TARGET_OBJ_DIR)
 
 settings-json:
-	$(V0) CPP_PATH="$(ARM_SDK_DIR)/bin" CFLAGS="$(CFLAGS)" TARGET=$(TARGET) ruby $(SETTINGS_GENERATOR) . $(SETTINGS_FILE) --json settings.json
+	$(V0) CPP_PATH="$(ARM_SDK_DIR)/bin" CFLAGS="$(CFLAGS_CC) $(CFLAGS)" TARGET=$(TARGET) ruby $(SETTINGS_GENERATOR) . $(SETTINGS_FILE) --json settings.json
 
 clean-settings:
 	$(V1) $(RM) $(GENERATED_SETTINGS)
@@ -365,11 +370,15 @@ $(TARGET_ELF): $(TARGET_OBJS)
 $(TARGET_OBJ_DIR)/%.o: %.c
 	$(V1) mkdir -p $(dir $@)
 	$(V1) echo %% $(notdir $<) "$(STDOUT)"
-	$(V1) $(CROSS_CC) -c -o $@ $(CFLAGS) $<
+	$(V1) $(CROSS_CC) -c -o $@ $(CFLAGS) $(CFLAGS_CC) $<
 ifeq ($(GENERATE_ASM), 1)
 	$(V1) $(CROSS_CC) -S -fverbose-asm -Wa,-aslh -o $(patsubst %.o,%.txt.S,$@) -g $(ASM_CFLAGS) $<
 endif
 
+$(TARGET_OBJ_DIR)/%.o: %.cpp
+	$(V1) mkdir -p $(dir $@)
+	$(V1) echo %% $(notdir $<) "$(STDOUT)"
+	$(V1) $(CROSS_CXX) -c -o $@ $(CFLAGS) $(CFLAGS_CXX) $<
 
 # Assemble
 $(TARGET_OBJ_DIR)/%.o: %.s
diff --git a/make/source.mk b/make/source.mk
index a130f5e27e6..73c45815a79 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -1,6 +1,10 @@
+COMMON_CPP_SRC = \
+            scheduler/schedulerTask.cpp \
+            main.cpp
+
 COMMON_SRC = \
             $(TARGET_DIR_SRC) \
-            main.c \
+            $(COMMON_CPP_SRC) \
             target/common_hardware.c \
             build/assert.c \
             build/build_config.c \
diff --git a/src/main/common/base.h b/src/main/common/base.h
new file mode 100644
index 00000000000..0893b012688
--- /dev/null
+++ b/src/main/common/base.h
@@ -0,0 +1,49 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+    #define EXTERN_C extern "C"
+
+    #include <cstddef>
+    #include <cstdint>
+    #include <new>
+
+    // All classes should be derived from this
+    class base_c {
+    protected:
+        static void * operator new(std::size_t);
+        static void * operator new [] (std::size_t);
+
+    public:
+        static inline void * operator new(std::size_t s, void *p) { (void)s; return p; }
+    };
+
+#else
+
+    #define EXTERN_C extern
+
+#endif
+
diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h
index b61c5877d59..d28814502f6 100644
--- a/src/main/drivers/serial.h
+++ b/src/main/drivers/serial.h
@@ -23,6 +23,8 @@ typedef struct {
     ioTag_t txPin;
 } serialPortPins_t;
 
+#include <common/base.h>
+
 typedef enum portMode_t {
     MODE_RX = 1 << 0,
     MODE_TX = 1 << 1,
@@ -100,19 +102,19 @@ struct serialPortVTable {
     void (*endWrite)(serialPort_t *instance);
 };
 
-void serialWrite(serialPort_t *instance, uint8_t ch);
-uint32_t serialRxBytesWaiting(const serialPort_t *instance);
-uint32_t serialTxBytesFree(const serialPort_t *instance);
-void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count);
-uint8_t serialRead(serialPort_t *instance);
-void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate);
-void serialSetMode(serialPort_t *instance, portMode_t mode);
-bool isSerialTransmitBufferEmpty(const serialPort_t *instance);
-void serialPrint(serialPort_t *instance, const char *str);
-uint32_t serialGetBaudRate(serialPort_t *instance);
-bool serialIsConnected(const serialPort_t *instance);
+EXTERN_C void serialWrite(serialPort_t *instance, uint8_t ch);
+EXTERN_C uint32_t serialRxBytesWaiting(const serialPort_t *instance);
+EXTERN_C uint32_t serialTxBytesFree(const serialPort_t *instance);
+EXTERN_C void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count);
+EXTERN_C uint8_t serialRead(serialPort_t *instance);
+EXTERN_C void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate);
+EXTERN_C void serialSetMode(serialPort_t *instance, portMode_t mode);
+EXTERN_C bool isSerialTransmitBufferEmpty(const serialPort_t *instance);
+EXTERN_C void serialPrint(serialPort_t *instance, const char *str);
+EXTERN_C uint32_t serialGetBaudRate(serialPort_t *instance);
+EXTERN_C bool serialIsConnected(const serialPort_t *instance);
 
 // A shim that adapts the bufWriter API to the serialWriteBuf() API.
-void serialWriteBufShim(void *instance, const uint8_t *data, int count);
-void serialBeginWrite(serialPort_t *instance);
-void serialEndWrite(serialPort_t *instance);
+EXTERN_C void serialWriteBufShim(void *instance, const uint8_t *data, int count);
+EXTERN_C void serialBeginWrite(serialPort_t *instance);
+EXTERN_C void serialEndWrite(serialPort_t *instance);
diff --git a/src/main/drivers/serial_softserial.h b/src/main/drivers/serial_softserial.h
index 6db4d4908b2..fd447bf135d 100644
--- a/src/main/drivers/serial_softserial.h
+++ b/src/main/drivers/serial_softserial.h
@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include <common/base.h>
+
 #define SOFTSERIAL_BUFFER_SIZE 256
 
 typedef enum {
@@ -27,9 +29,9 @@ typedef enum {
 serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baud, portMode_t mode, portOptions_t options);
 
 // serialPort API
-void softSerialWriteByte(serialPort_t *instance, uint8_t ch);
-uint32_t softSerialRxBytesWaiting(const serialPort_t *instance);
-uint32_t softSerialTxBytesFree(const serialPort_t *instance);
-uint8_t softSerialReadByte(serialPort_t *instance);
-void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate);
-bool isSoftSerialTransmitBufferEmpty(const serialPort_t *s);
+EXTERN_C void softSerialWriteByte(serialPort_t *instance, uint8_t ch);
+EXTERN_C uint32_t softSerialRxBytesWaiting(const serialPort_t *instance);
+EXTERN_C uint32_t softSerialTxBytesFree(const serialPort_t *instance);
+EXTERN_C uint8_t softSerialReadByte(serialPort_t *instance);
+EXTERN_C void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate);
+EXTERN_C bool isSoftSerialTransmitBufferEmpty(const serialPort_t *s);
diff --git a/src/main/fc/fc_init.h b/src/main/fc/fc_init.h
index 701716dad8d..6801dd64c3e 100644
--- a/src/main/fc/fc_init.h
+++ b/src/main/fc/fc_init.h
@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include <common/base.h>
+
 typedef enum {
     SYSTEM_STATE_INITIALISING   = 0,
     SYSTEM_STATE_CONFIG_LOADED  = (1 << 0),
@@ -26,5 +28,5 @@ typedef enum {
     SYSTEM_STATE_READY          = (1 << 7)
 } systemState_e;
 
-extern uint8_t systemState;
-void init(void);
+EXTERN_C uint8_t systemState;
+EXTERN_C void init(void);
diff --git a/src/main/main.c b/src/main/main.cpp
similarity index 81%
rename from src/main/main.c
rename to src/main/main.cpp
index 7a46b485d79..520661672bf 100644
--- a/src/main/main.c
+++ b/src/main/main.cpp
@@ -18,7 +18,9 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include "platform.h"
+#include <platform.h>
+#include <common/base.h>
+
 #include "build/debug.h"
 #include "drivers/serial.h"
 #include "drivers/serial_softserial.h"
@@ -31,7 +33,6 @@
 serialPort_t *loopbackPort;
 #endif
 
-
 static void loopbackInit(void)
 {
 #ifdef SOFTSERIAL_LOOPBACK
@@ -53,7 +54,19 @@ static void processLoopback(void)
 #endif
 }
 
-int main(void)
+/* CRT init function, called by __libc_init_array() before passing control to main */
+extern "C" void __attribute__ ((weak)) _init(void)
+{
+}
+
+/* Prevent demangle from being pulled in */
+extern "C" void __cxa_pure_virtual(void)
+{
+    // FIXME: Crash safely here
+    while (true);
+}
+
+extern "C" int main(void)
 {
     init();
     loopbackInit();
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index cf42a7ea5fe..93fb97b0f5c 100755
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -17,6 +17,7 @@
 
 #pragma once
 
+#include "common/base.h"
 #include "common/time.h"
 
 //#define SCHEDULER_DEBUG
@@ -144,20 +145,20 @@ typedef struct {
 #endif
 } cfTask_t;
 
-extern cfTask_t cfTasks[TASK_COUNT];
-extern uint16_t averageSystemLoadPercent;
+EXTERN_C cfTask_t cfTasks[TASK_COUNT];
+EXTERN_C uint16_t averageSystemLoadPercent;
 
-void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo);
-void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t *taskInfo);
-void rescheduleTask(cfTaskId_e taskId, timeDelta_t newPeriodUs);
-void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState);
-timeDelta_t getTaskDeltaTime(cfTaskId_e taskId);
-void schedulerResetTaskStatistics(cfTaskId_e taskId);
+EXTERN_C void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo);
+EXTERN_C void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t *taskInfo);
+EXTERN_C void rescheduleTask(cfTaskId_e taskId, timeDelta_t newPeriodUs);
+EXTERN_C void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState);
+EXTERN_C timeDelta_t getTaskDeltaTime(cfTaskId_e taskId);
+EXTERN_C void schedulerResetTaskStatistics(cfTaskId_e taskId);
 
-void schedulerInit(void);
-void scheduler(void);
-void taskSystem(timeUs_t currentTimeUs);
-void taskRunRealtimeCallbacks(timeUs_t currentTimeUs);
+EXTERN_C void schedulerInit(void);
+EXTERN_C void scheduler(void);
+EXTERN_C void taskSystem(timeUs_t currentTimeUs);
+EXTERN_C void taskRunRealtimeCallbacks(timeUs_t currentTimeUs);
 
 #define TASK_PERIOD_HZ(hz) (1000000 / (hz))
 #define TASK_PERIOD_MS(ms) ((ms) * 1000)
diff --git a/src/main/scheduler/schedulerTask.cpp b/src/main/scheduler/schedulerTask.cpp
new file mode 100644
index 00000000000..cecee9abf7b
--- /dev/null
+++ b/src/main/scheduler/schedulerTask.cpp
@@ -0,0 +1,48 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <common/base.h>
+#include <scheduler/schedulerTask.h>
+
+void task_c::setReady(bool _ready)
+{
+    ready = _ready;
+}
+
+bool task_c::isReady(void) const
+{
+    return ready;
+}
+
+void task_c::run(const timeUs_t currentTimeUs, const timeDelta_t deltaTimeUs)
+{
+    task(currentTimeUs, deltaTimeUs);
+    setReady(false);
+}
+
+void task_c::task(const timeUs_t currentTimeUs, const timeDelta_t deltaTimeUs)
+{
+    (void)currentTimeUs;
+    (void)deltaTimeUs;
+}
diff --git a/src/main/scheduler/schedulerTask.h b/src/main/scheduler/schedulerTask.h
new file mode 100644
index 00000000000..683b3f0d42d
--- /dev/null
+++ b/src/main/scheduler/schedulerTask.h
@@ -0,0 +1,41 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include <common/base.h>
+#include <common/time.h>
+
+// All schedulable objects should be derived from task_c
+class task_c : base_c {
+protected:
+    bool ready;
+    // Scheduler calls run(), which in turn calls task() main function
+    void run(const timeUs_t currentTimeUs, const timeDelta_t deltaTimeUs);
+
+public:
+    virtual bool isReady(void) const;
+    virtual void setReady(bool ready);
+    virtual void task(const timeUs_t currentTimeUs, const timeDelta_t deltaTimeUs);
+};
diff --git a/src/main/startup/startup_stm32f30x_md_gcc.S b/src/main/startup/startup_stm32f30x_md_gcc.S
index 466d2a7add8..d236b616c2a 100644
--- a/src/main/startup/startup_stm32f30x_md_gcc.S
+++ b/src/main/startup/startup_stm32f30x_md_gcc.S
@@ -133,7 +133,11 @@ LoopMarkHeapStack:
 
 /* Call the clock system intitialization function.*/
   bl  SystemInit
-/* Call the application's entry point.*/
+
+/* Call static constructors */
+  bl  __libc_init_array
+
+/* Call the application entry point */
   bl  main
   bx  lr
 
diff --git a/src/main/startup/startup_stm32f40xx.s b/src/main/startup/startup_stm32f40xx.s
index 764bc499cb1..b0fd633758c 100644
--- a/src/main/startup/startup_stm32f40xx.s
+++ b/src/main/startup/startup_stm32f40xx.s
@@ -151,7 +151,10 @@ LoopMarkHeapStack:
 /* Call the clock system intitialization function.*/ 
     bl  SystemInit  
 
-/* Call the application's entry point.*/
+/* Call static constructors */
+  bl  __libc_init_array
+
+/* Call the application entry point */
   bl  main
   bx  lr   
 
diff --git a/src/main/startup/startup_stm32f411xe.s b/src/main/startup/startup_stm32f411xe.s
index bf174f25baf..92f87deb5a4 100644
--- a/src/main/startup/startup_stm32f411xe.s
+++ b/src/main/startup/startup_stm32f411xe.s
@@ -151,7 +151,10 @@ LoopMarkHeapStack:
 /* Call the clock system intitialization function.*/
   bl  SystemInit  
 
-/* Call the application's entry point.*/
+/* Call static constructors */
+  bl  __libc_init_array
+
+/* Call the application entry point */
   bl  main
   bx  lr   
 
diff --git a/src/main/startup/startup_stm32f427xx.s b/src/main/startup/startup_stm32f427xx.s
index 520a7f7222a..ef77e98d97e 100755
--- a/src/main/startup/startup_stm32f427xx.s
+++ b/src/main/startup/startup_stm32f427xx.s
@@ -158,7 +158,10 @@ LoopMarkHeapStack:
 /* Call the clock system intitialization function.*/ 
     bl  SystemInit  
 
-/* Call the application's entry point.*/
+/* Call static constructors */
+  bl  __libc_init_array
+
+/* Call the application entry point */
   bl  main
   bx  lr   
 
diff --git a/src/main/startup/startup_stm32f446xx.s b/src/main/startup/startup_stm32f446xx.s
index 592a03de672..9e3b7d53f78 100644
--- a/src/main/startup/startup_stm32f446xx.s
+++ b/src/main/startup/startup_stm32f446xx.s
@@ -153,7 +153,10 @@ LoopMarkHeapStack:
 /* Call the clock system intitialization function.*/
   bl  SystemInit
 
-/* Call the application entry point.*/
+/* Call static constructors */
+  bl  __libc_init_array
+
+/* Call the application entry point */
   bl  main
   bx  lr
 
diff --git a/src/main/startup/startup_stm32f722xx.s b/src/main/startup/startup_stm32f722xx.s
index f856d04e3b2..79f1043392f 100644
--- a/src/main/startup/startup_stm32f722xx.s
+++ b/src/main/startup/startup_stm32f722xx.s
@@ -140,8 +140,8 @@ LoopMarkHeapStack:
 /* Call the clock system intitialization function.*/
   bl  SystemInit   
 /* Call static constructors */
-//    bl __libc_init_array
-/* Call the application's entry point.*/
+  bl  __libc_init_array
+/* Call the application entry point */
   bl  main
   bx  lr    
 .size  Reset_Handler, .-Reset_Handler
diff --git a/src/main/startup/startup_stm32f745xx.s b/src/main/startup/startup_stm32f745xx.s
index eb6aa25659a..600fd52555a 100644
--- a/src/main/startup/startup_stm32f745xx.s
+++ b/src/main/startup/startup_stm32f745xx.s
@@ -142,8 +142,10 @@ LoopMarkHeapStack:
   bl CopyFastCode
 
 /* Call the clock system initialization function.*/
-  bl  SystemInit   
-/* Call the application's entry point.*/
+  bl  SystemInit
+/* Call static constructors */
+  bl  __libc_init_array
+/* Call the application entry point */
   bl  main
   bx  lr    
 
diff --git a/src/main/startup/startup_stm32f746xx.s b/src/main/startup/startup_stm32f746xx.s
index 8ef14a877ff..5312a23c3d1 100644
--- a/src/main/startup/startup_stm32f746xx.s
+++ b/src/main/startup/startup_stm32f746xx.s
@@ -141,10 +141,11 @@ LoopMarkHeapStack:
 /* Call the clock system initialization function.*/
   bl  SystemInit   
 /* Call static constructors */
-//    bl __libc_init_array
-/* Call the application's entry point.*/
+  bl  __libc_init_array
+/* Call the application entry point */
   bl  main
-  bx  lr    
+  bx  lr
+
 .size  Reset_Handler, .-Reset_Handler
 
 /**