diff --git a/Makefile b/Makefile
index 8c80fd8d27..0fd2760637 100644
--- a/Makefile
+++ b/Makefile
@@ -363,8 +363,6 @@ COMMON_SRC = \
             $(TARGET_DIR_SRC) \
             main.c \
             mw.c \
-            scheduler.c \
-            scheduler_tasks.c \
             common/encoding.c \
             common/filter.c \
             common/maths.c \
@@ -414,6 +412,8 @@ COMMON_SRC = \
             rx/sumd.c \
             rx/sumh.c \
             rx/xbus.c \
+            scheduler/scheduler.c \
+            scheduler/scheduler_tasks.c \
             sensors/acceleration.c \
             sensors/battery.c \
             sensors/boardalignment.c \
diff --git a/src/main/config/config.c b/src/main/config/config.c
index ff7a2ac1b6..3d39672e59 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -435,6 +435,7 @@ static void resetConf(void)
 
 #ifdef USE_RTC6705
     masterConfig.vtx_channel = 19; // default to Boscam E channel 4
+    masterConfig.vtx_power   = 1;
 #endif
 
 #ifdef BOARD_HAS_VOLTAGE_DIVIDER
diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h
index 6d1c943179..eb336ed999 100644
--- a/src/main/config/config_master.h
+++ b/src/main/config/config_master.h
@@ -125,8 +125,12 @@ typedef struct master_t {
     uint8_t transponderData[6];
 #endif
 
-#ifdef OSD
+#ifdef USE_RTC6705
     uint8_t vtx_channel;
+    uint8_t vtx_power;
+#endif
+
+#ifdef OSD
     osd_profile osdProfile;
 #endif
 
diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c
index 57bbecdc44..11562cfc98 100644
--- a/src/main/drivers/io.c
+++ b/src/main/drivers/io.c
@@ -8,115 +8,115 @@
 
 // io ports defs are stored in array by index now
 struct ioPortDef_s {
-	rccPeriphTag_t rcc;
+    rccPeriphTag_t rcc;
 };
 
 #if defined(STM32F1)
 const struct ioPortDef_s ioPortDefs[] = {
-	{ RCC_APB2(IOPA) },
-	{ RCC_APB2(IOPB) },
-	{ RCC_APB2(IOPC) },
-	{ RCC_APB2(IOPD) },
-	{ RCC_APB2(IOPE) },
+    { RCC_APB2(IOPA) },
+    { RCC_APB2(IOPB) },
+    { RCC_APB2(IOPC) },
+    { RCC_APB2(IOPD) },
+    { RCC_APB2(IOPE) },
 {
 #if defined (STM32F10X_HD) || defined (STM32F10X_XL) || defined (STM32F10X_HD_VL)
-	RCC_APB2(IOPF),
+    RCC_APB2(IOPF),
 #else
-	0,
+    0,
 #endif
 },
 {
 #if defined (STM32F10X_HD) || defined (STM32F10X_XL) || defined (STM32F10X_HD_VL)
-	RCC_APB2(IOPG),
+    RCC_APB2(IOPG),
 #else
-	0,
+    0,
 #endif
 },
 };
 #elif defined(STM32F3)
 const struct ioPortDef_s ioPortDefs[] = {
-	{ RCC_AHB(GPIOA) },
-	{ RCC_AHB(GPIOB) },
-	{ RCC_AHB(GPIOC) },
-	{ RCC_AHB(GPIOD) },
-	{ RCC_AHB(GPIOE) },
-	{ RCC_AHB(GPIOF) },
+    { RCC_AHB(GPIOA) },
+    { RCC_AHB(GPIOB) },
+    { RCC_AHB(GPIOC) },
+    { RCC_AHB(GPIOD) },
+    { RCC_AHB(GPIOE) },
+    { RCC_AHB(GPIOF) },
 };
 #elif defined(STM32F4)
 const struct ioPortDef_s ioPortDefs[] = {
-	{ RCC_AHB1(GPIOA) },
-	{ RCC_AHB1(GPIOB) },
-	{ RCC_AHB1(GPIOC) },
-	{ RCC_AHB1(GPIOD) },
-	{ RCC_AHB1(GPIOE) },
-	{ RCC_AHB1(GPIOF) },
+    { RCC_AHB1(GPIOA) },
+    { RCC_AHB1(GPIOB) },
+    { RCC_AHB1(GPIOC) },
+    { RCC_AHB1(GPIOD) },
+    { RCC_AHB1(GPIOE) },
+    { RCC_AHB1(GPIOF) },
 };
 # endif
 
 ioRec_t* IO_Rec(IO_t io)
 {
-	return io;
+    return io;
 }
 
 GPIO_TypeDef* IO_GPIO(IO_t io)
 {
-	ioRec_t *ioRec = IO_Rec(io);
-	return ioRec->gpio;
+    ioRec_t *ioRec = IO_Rec(io);
+    return ioRec->gpio;
 }
 
 uint16_t IO_Pin(IO_t io)
 {
-	ioRec_t *ioRec = IO_Rec(io);
-	return ioRec->pin;
+    ioRec_t *ioRec = IO_Rec(io);
+    return ioRec->pin;
 }
 
 // port index, GPIOA == 0
 int IO_GPIOPortIdx(IO_t io)
 {
-	if (!io)
-		return -1;
-	return (((size_t)IO_GPIO(io) - GPIOA_BASE) >> 10);     // ports are 0x400 apart
+    if (!io)
+        return -1;
+    return (((size_t)IO_GPIO(io) - GPIOA_BASE) >> 10);     // ports are 0x400 apart
 }
 
 int IO_EXTI_PortSourceGPIO(IO_t io)
 {
-	return IO_GPIOPortIdx(io);
+    return IO_GPIOPortIdx(io);
 }
 
 int IO_GPIO_PortSource(IO_t io)
 {
-	return IO_GPIOPortIdx(io);
+    return IO_GPIOPortIdx(io);
 }
 
 // zero based pin index
 int IO_GPIOPinIdx(IO_t io)
 {
-	if (!io)
-		return -1;
-	return 31 - __builtin_clz(IO_Pin(io));  // CLZ is a bit faster than FFS
+    if (!io)
+        return -1;
+    return 31 - __builtin_clz(IO_Pin(io));  // CLZ is a bit faster than FFS
 }
 
 int IO_EXTI_PinSource(IO_t io)
 {
-	return IO_GPIOPinIdx(io);
+    return IO_GPIOPinIdx(io);
 }
 
 int IO_GPIO_PinSource(IO_t io)
 {
-	return IO_GPIOPinIdx(io);
+    return IO_GPIOPinIdx(io);
 }
 
 // mask on stm32f103, bit index on stm32f303
 uint32_t IO_EXTI_Line(IO_t io)
 {
-	if (!io)
-		return 0;
+    if (!io)
+        return 0;
 #if defined(STM32F1)
-	return 1 << IO_GPIOPinIdx(io);
+    return 1 << IO_GPIOPinIdx(io);
 #elif defined(STM32F3)
-	return IO_GPIOPinIdx(io);
+    return IO_GPIOPinIdx(io);
 #elif defined(STM32F4)
-	return 1 << IO_GPIOPinIdx(io);
+    return 1 << IO_GPIOPinIdx(io);
 #else
 # error "Unknown target type"
 #endif
@@ -124,151 +124,151 @@ uint32_t IO_EXTI_Line(IO_t io)
 
 bool IORead(IO_t io)
 {
-	if (!io)
-		return false;
-	return !! (IO_GPIO(io)->IDR & IO_Pin(io));
+    if (!io)
+        return false;
+    return !! (IO_GPIO(io)->IDR & IO_Pin(io));
 }
 
 void IOWrite(IO_t io, bool hi)
 {
-	if (!io)
-		return;
+    if (!io)
+        return;
 #ifdef STM32F4
-	if (hi) {
-		IO_GPIO(io)->BSRRL = IO_Pin(io);
-	}
-	else {
-		IO_GPIO(io)->BSRRH = IO_Pin(io);
-	}
+    if (hi) {
+        IO_GPIO(io)->BSRRL = IO_Pin(io);
+    }
+    else {
+        IO_GPIO(io)->BSRRH = IO_Pin(io);
+    }
 #else
-	IO_GPIO(io)->BSRR = IO_Pin(io) << (hi ? 0 : 16);
+    IO_GPIO(io)->BSRR = IO_Pin(io) << (hi ? 0 : 16);
 #endif
 }
 
 void IOHi(IO_t io)
 {
-	if (!io)
-		return;
+    if (!io)
+        return;
 #ifdef STM32F4
-	IO_GPIO(io)->BSRRL = IO_Pin(io);
+    IO_GPIO(io)->BSRRL = IO_Pin(io);
 #else
-	IO_GPIO(io)->BSRR = IO_Pin(io);
+    IO_GPIO(io)->BSRR = IO_Pin(io);
 #endif
 }
 
 void IOLo(IO_t io)
 {
-	if (!io)
-		return;
+    if (!io)
+        return;
 #ifdef STM32F4
-	IO_GPIO(io)->BSRRH = IO_Pin(io);
+    IO_GPIO(io)->BSRRH = IO_Pin(io);
 #else
-	IO_GPIO(io)->BRR = IO_Pin(io);
+    IO_GPIO(io)->BRR = IO_Pin(io);
 #endif
 }
 
 void IOToggle(IO_t io)
 {
-	if (!io)
-		return;
-	uint32_t mask = IO_Pin(io);
-	// Read pin state from ODR but write to BSRR because it only changes the pins
-	// high in the mask value rather than all pins. XORing ODR directly risks
-	// setting other pins incorrectly because it change all pins' state.
+    if (!io)
+        return;
+    uint32_t mask = IO_Pin(io);
+    // Read pin state from ODR but write to BSRR because it only changes the pins
+    // high in the mask value rather than all pins. XORing ODR directly risks
+    // setting other pins incorrectly because it change all pins' state.
 #ifdef STM32F4
-	if (IO_GPIO(io)->ODR & mask) {
-		IO_GPIO(io)->BSRRH = mask;
-	} else {
-		IO_GPIO(io)->BSRRL = mask;
-	}
+    if (IO_GPIO(io)->ODR & mask) {
+        IO_GPIO(io)->BSRRH = mask;
+    } else {
+        IO_GPIO(io)->BSRRL = mask;
+    }
 #else
-	if (IO_GPIO(io)->ODR & mask)
-		mask <<= 16;   // bit is set, shift mask to reset half
+    if (IO_GPIO(io)->ODR & mask)
+        mask <<= 16;   // bit is set, shift mask to reset half
 
-	IO_GPIO(io)->BSRR = mask;
+    IO_GPIO(io)->BSRR = mask;
 #endif
 }
 
 // claim IO pin, set owner and resources
 void IOInit(IO_t io, resourceOwner_t owner, resourceType_t resources)
 {
-	ioRec_t *ioRec = IO_Rec(io);
-	if (owner != OWNER_FREE)   // pass OWNER_FREE to keep old owner
-		ioRec->owner = owner;
-	ioRec->resourcesUsed |= resources;
+    ioRec_t *ioRec = IO_Rec(io);
+    if (owner != OWNER_FREE)   // pass OWNER_FREE to keep old owner
+        ioRec->owner = owner;
+    ioRec->resourcesUsed |= resources;
 }
 
 void IORelease(IO_t io)
 {
-	ioRec_t *ioRec = IO_Rec(io);
-	ioRec->owner = OWNER_FREE;
+    ioRec_t *ioRec = IO_Rec(io);
+    ioRec->owner = OWNER_FREE;
 }
 
 resourceOwner_t IOGetOwner(IO_t io)
 {
-	ioRec_t *ioRec = IO_Rec(io);
-	return ioRec->owner;
+    ioRec_t *ioRec = IO_Rec(io);
+    return ioRec->owner;
 }
 
 resourceType_t IOGetResources(IO_t io)
 {
-	ioRec_t *ioRec = IO_Rec(io);
-	return ioRec->resourcesUsed;
+    ioRec_t *ioRec = IO_Rec(io);
+    return ioRec->resourcesUsed;
 }
 
 #if defined(STM32F1)
 
 void IOConfigGPIO(IO_t io, ioConfig_t cfg)
 {
-	if (!io)
-		return;
-	rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
-	RCC_ClockCmd(rcc, ENABLE);
+    if (!io)
+        return;
+    rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
+    RCC_ClockCmd(rcc, ENABLE);
 
-	GPIO_InitTypeDef init = {
-		.GPIO_Pin = IO_Pin(io),
-		.GPIO_Speed = cfg & 0x03,
-		.GPIO_Mode = cfg & 0x7c,
-	};
-	GPIO_Init(IO_GPIO(io), &init);
+    GPIO_InitTypeDef init = {
+        .GPIO_Pin = IO_Pin(io),
+        .GPIO_Speed = cfg & 0x03,
+        .GPIO_Mode = cfg & 0x7c,
+    };
+    GPIO_Init(IO_GPIO(io), &init);
 }
 
 #elif defined(STM32F3) || defined(STM32F4)
 
 void IOConfigGPIO(IO_t io, ioConfig_t cfg)
 {
-	if (!io)
-		return;
-	rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
-	RCC_ClockCmd(rcc, ENABLE);
+    if (!io)
+        return;
+    rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
+    RCC_ClockCmd(rcc, ENABLE);
 
-	GPIO_InitTypeDef init = {
-		.GPIO_Pin = IO_Pin(io),
-		.GPIO_Mode = (cfg >> 0) & 0x03,
-		.GPIO_Speed = (cfg >> 2) & 0x03,
-		.GPIO_OType = (cfg >> 4) & 0x01,
-		.GPIO_PuPd = (cfg >> 5) & 0x03,
-	};
-	GPIO_Init(IO_GPIO(io), &init);
+    GPIO_InitTypeDef init = {
+        .GPIO_Pin = IO_Pin(io),
+        .GPIO_Mode = (cfg >> 0) & 0x03,
+        .GPIO_Speed = (cfg >> 2) & 0x03,
+        .GPIO_OType = (cfg >> 4) & 0x01,
+        .GPIO_PuPd = (cfg >> 5) & 0x03,
+    };
+    GPIO_Init(IO_GPIO(io), &init);
 }
 
 void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af)
 {
-	if (!io)
-		return;
+    if (!io)
+        return;
 
-	rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
-	RCC_ClockCmd(rcc, ENABLE);
-	GPIO_PinAFConfig(IO_GPIO(io), IO_GPIO_PinSource(io), af);
+    rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
+    RCC_ClockCmd(rcc, ENABLE);
+    GPIO_PinAFConfig(IO_GPIO(io), IO_GPIO_PinSource(io), af);
 
-	GPIO_InitTypeDef init = {
-		.GPIO_Pin = IO_Pin(io),
-		.GPIO_Mode = (cfg >> 0) & 0x03,
-		.GPIO_Speed = (cfg >> 2) & 0x03,
-		.GPIO_OType = (cfg >> 4) & 0x01,
-		.GPIO_PuPd = (cfg >> 5) & 0x03,
-	};
-	GPIO_Init(IO_GPIO(io), &init);
+    GPIO_InitTypeDef init = {
+        .GPIO_Pin = IO_Pin(io),
+        .GPIO_Mode = (cfg >> 0) & 0x03,
+        .GPIO_Speed = (cfg >> 2) & 0x03,
+        .GPIO_OType = (cfg >> 4) & 0x01,
+        .GPIO_PuPd = (cfg >> 5) & 0x03,
+    };
+    GPIO_Init(IO_GPIO(io), &init);
 }
 #endif
 
@@ -279,32 +279,32 @@ ioRec_t ioRecs[DEFIO_IO_USED_COUNT];
 // initialize all ioRec_t structures from ROM
 // currently only bitmask is used, this may change in future
 void IOInitGlobal(void) {
-	ioRec_t *ioRec = ioRecs;
+    ioRec_t *ioRec = ioRecs;
 
-	for (unsigned port = 0; port < ARRAYLEN(ioDefUsedMask); port++) {
-		for (unsigned pin = 0; pin < sizeof(ioDefUsedMask[0]) * 8; pin++) {
-			if (ioDefUsedMask[port] & (1 << pin)) {
-				ioRec->gpio = (GPIO_TypeDef *)(GPIOA_BASE + (port << 10));   // ports are 0x400 apart
-				ioRec->pin = 1 << pin;
-				ioRec++;
-			}
-		}
-	}
+    for (unsigned port = 0; port < ARRAYLEN(ioDefUsedMask); port++) {
+        for (unsigned pin = 0; pin < sizeof(ioDefUsedMask[0]) * 8; pin++) {
+            if (ioDefUsedMask[port] & (1 << pin)) {
+                ioRec->gpio = (GPIO_TypeDef *)(GPIOA_BASE + (port << 10));   // ports are 0x400 apart
+                ioRec->pin = 1 << pin;
+                ioRec++;
+            }
+        }
+    }
 }
 
 IO_t IOGetByTag(ioTag_t tag)
 {
-	int portIdx = DEFIO_TAG_GPIOID(tag);
-	int pinIdx = DEFIO_TAG_PIN(tag);
+    int portIdx = DEFIO_TAG_GPIOID(tag);
+    int pinIdx = DEFIO_TAG_PIN(tag);
 
-	if (portIdx >= DEFIO_PORT_USED_COUNT)
-		return NULL;
-	// check if pin exists
-	if (!(ioDefUsedMask[portIdx] & (1 << pinIdx)))
-		return NULL;
-	// count bits before this pin on single port
-	int offset = __builtin_popcount(((1 << pinIdx) - 1) & ioDefUsedMask[portIdx]);
-	// and add port offset
-	offset += ioDefUsedOffset[portIdx];
-	return ioRecs + offset;
+    if (portIdx >= DEFIO_PORT_USED_COUNT)
+        return NULL;
+    // check if pin exists
+    if (!(ioDefUsedMask[portIdx] & (1 << pinIdx)))
+        return NULL;
+    // count bits before this pin on single port
+    int offset = __builtin_popcount(((1 << pinIdx) - 1) & ioDefUsedMask[portIdx]);
+    // and add port offset
+    offset += ioDefUsedOffset[portIdx];
+    return ioRecs + offset;
 }
diff --git a/src/main/drivers/io_impl.h b/src/main/drivers/io_impl.h
index b7bbcec7f3..3a084949fe 100644
--- a/src/main/drivers/io_impl.h
+++ b/src/main/drivers/io_impl.h
@@ -5,14 +5,14 @@
 #include "platform.h"
 
 typedef struct ioDef_s {
-	ioTag_t tag;
+    ioTag_t tag;
 } ioDef_t;
 
 typedef struct ioRec_s {
-	GPIO_TypeDef *gpio;
-	uint16_t pin;
-	resourceOwner_t owner;
-	resourceType_t resourcesUsed; // TODO!
+    GPIO_TypeDef *gpio;
+    uint16_t pin;
+    resourceOwner_t owner;
+    resourceType_t resourcesUsed; // TODO!
 } ioRec_t;
 
 extern ioRec_t ioRecs[DEFIO_IO_USED_COUNT];
diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index 9b03896914..2911b68949 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -38,13 +38,13 @@
 static IO_t max7456CsPin = IO_NONE;
 
 /** PAL or NTSC, value is number of chars total */
-#define VIDEO_MODE_PIXELS_NTSC   390
-#define VIDEO_MODE_PIXELS_PAL    480
+#define VIDEO_BUFFER_CHARS_NTSC   390
+#define VIDEO_BUFFER_CHARS_PAL    480
 
 uint16_t max_screen_size;
 uint8_t  video_signal_type = 0;
 uint8_t  max7456_lock = 0;
-char screen[VIDEO_MODE_PIXELS_PAL];
+char screen[VIDEO_BUFFER_CHARS_PAL];
 
 
 uint8_t max7456_send(uint8_t add, uint8_t data) {
@@ -93,10 +93,10 @@ void max7456_init(uint8_t system) {
     }
 
     if (video_signal_type) { //PAL
-        max_screen_size = VIDEO_MODE_PIXELS_PAL;
+        max_screen_size = VIDEO_BUFFER_CHARS_PAL;
         max_screen_rows = 16;
     } else {                 // NTSC
-        max_screen_size = VIDEO_MODE_PIXELS_NTSC;
+        max_screen_size = VIDEO_BUFFER_CHARS_NTSC;
         max_screen_rows = 13;
     }
 
diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index 6e9ceb3a58..2e96f31c52 100755
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -111,7 +111,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
     int channelIndex = 0;
 
     memset(&pwmOutputConfiguration, 0, sizeof(pwmOutputConfiguration));
-
+       
     // this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ]
     if (init->airplane)
         i = 2; // switch to air hardware config
@@ -189,12 +189,11 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
 #endif
 
 #ifdef SONAR
-        if (init->sonarGPIOConfig &&
+        if (init->sonarConfig && 
             (
-                CheckGPIOPin(timerHardwarePtr->pin, init->sonarGPIOConfig->gpio, init->sonarGPIOConfig->triggerPin) ||
-                CheckGPIOPin(timerHardwarePtr->pin, init->sonarGPIOConfig->gpio, init->sonarGPIOConfig->echoPin)
-            )
-        ) {
+                timerHardwarePtr->pin == init->sonarConfig->triggerPin ||
+                timerHardwarePtr->pin == init->sonarConfig->echoPin
+            )) {
             continue;
         }
 #endif
diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h
index 0d70fafc58..4c94681bc3 100644
--- a/src/main/drivers/pwm_mapping.h
+++ b/src/main/drivers/pwm_mapping.h
@@ -16,7 +16,7 @@
  */
 
 #pragma once
-#include "gpio.h"
+
 #include "timer.h"
 
 #define MAX_PWM_MOTORS  12
@@ -42,11 +42,10 @@
 #define ONESHOT42_TIMER_MHZ 24
 #define ONESHOT125_TIMER_MHZ 8
 
-typedef struct sonarGPIOConfig_s {
-    GPIO_TypeDef *gpio;
-    uint16_t triggerPin;
-    uint16_t echoPin;
-} sonarGPIOConfig_t;
+typedef struct sonarIOConfig_s {
+    ioTag_t triggerPin;
+    ioTag_t echoPin;
+} sonarIOConfig_t;
 
 typedef struct drv_pwm_config_s {
     bool useParallelPWM;
@@ -78,7 +77,7 @@ typedef struct drv_pwm_config_s {
     uint16_t motorPwmRate;
     uint16_t idlePulse;  // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm),
                          // some higher value (used by 3d mode), or 0, for brushed pwm drivers.
-    sonarGPIOConfig_t *sonarGPIOConfig;
+    sonarIOConfig_t *sonarConfig;
 } drv_pwm_config_t;
 
 enum {
diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c
index 5c930955b3..6ab1f26bad 100644
--- a/src/main/drivers/serial_softserial.c
+++ b/src/main/drivers/serial_softserial.c
@@ -96,7 +96,7 @@ void setTxSignal(softSerial_t *softSerial, uint8_t state)
     if (state) {
         IOHi(softSerial->txIO);
     } else {
-        IOLo(softSerial->txIO);
+	    IOLo(softSerial->txIO);
     }
 }
 
@@ -220,7 +220,7 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb
 
     softSerial->txIO = IOGetByTag(softSerial->txTimerHardware->pin);
     serialOutputPortConfig(softSerial->txTimerHardware->pin);
-
+    
     softSerial->rxIO = IOGetByTag(softSerial->rxTimerHardware->pin);
     serialInputPortConfig(softSerial->rxTimerHardware->pin);
 
diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c
index a835bfa05b..dbe72acd3f 100644
--- a/src/main/drivers/sonar_hcsr04.c
+++ b/src/main/drivers/sonar_hcsr04.c
@@ -22,7 +22,6 @@
 #include "build_config.h"
 
 #include "system.h"
-#include "gpio.h"
 #include "nvic.h"
 #include "io.h"
 #include "exti.h"
@@ -43,8 +42,9 @@ static uint32_t lastMeasurementAt;
 static sonarHardware_t const *sonarHardware;
 
 extiCallbackRec_t hcsr04_extiCallbackRec;
+
 static IO_t echoIO;
-//static IO_t triggerIO;
+static IO_t triggerIO;
 
 void hcsr04_extiHandler(extiCallbackRec_t* cb)
 {
@@ -52,7 +52,7 @@ void hcsr04_extiHandler(extiCallbackRec_t* cb)
     uint32_t timing_stop;
     UNUSED(cb);
 
-    if (digitalIn(sonarHardware->echo_gpio, sonarHardware->echo_pin) != 0) {
+    if (IORead(echoIO) != 0) {
         timing_start = micros();
     }
     else {
@@ -71,32 +71,27 @@ void hcsr04_init(const sonarHardware_t *initialSonarHardware, sonarRange_t *sona
     sonarRange->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
 
 #if !defined(UNIT_TEST)
-    gpio_config_t gpio;
 
 #ifdef STM32F10X
     // enable AFIO for EXTI support
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
 #endif
 
-#ifdef STM32F303xC
-    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
-
+#if defined(STM32F3) || defined(STM32F4)
     /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
 #endif
 
     // trigger pin
-    gpio.pin = sonarHardware->trigger_pin;
-    gpio.mode = Mode_Out_PP;
-    gpio.speed = Speed_2MHz;
-    gpioInit(sonarHardware->trigger_gpio, &gpio);
-
-    // echo pin
-    gpio.pin = sonarHardware->echo_pin;
-    gpio.mode = Mode_IN_FLOATING;
-    gpioInit(sonarHardware->echo_gpio, &gpio);
+    triggerIO = IOGetByTag(sonarHardware->triggerIO);
+    IOInit(triggerIO, OWNER_SONAR, RESOURCE_INPUT);
+    IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
     
-	echoIO = IOGetByTag(sonarHardware->echoIO);
+    // echo pin
+    echoIO = IOGetByTag(sonarHardware->echoIO);
+    IOInit(echoIO, OWNER_SONAR, RESOURCE_INPUT);
+    IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
+
 #ifdef USE_EXTI
 	EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
 	EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
@@ -123,10 +118,10 @@ void hcsr04_start_reading(void)
 
     lastMeasurementAt = now;
 
-    digitalHi(sonarHardware->trigger_gpio, sonarHardware->trigger_pin);
+    IOHi(triggerIO);
     //  The width of trig signal must be greater than 10us
     delayMicroseconds(11);
-    digitalLo(sonarHardware->trigger_gpio, sonarHardware->trigger_pin);
+    IOLo(triggerIO);
 #endif
 }
 
diff --git a/src/main/drivers/sonar_hcsr04.h b/src/main/drivers/sonar_hcsr04.h
index 99c89a4c8c..311fa233f5 100644
--- a/src/main/drivers/sonar_hcsr04.h
+++ b/src/main/drivers/sonar_hcsr04.h
@@ -21,10 +21,6 @@
 #include "io.h"
 
 typedef struct sonarHardware_s {
-    uint16_t trigger_pin;
-	GPIO_TypeDef* trigger_gpio;
-    uint16_t echo_pin;
-	GPIO_TypeDef* echo_gpio;
     ioTag_t triggerIO;
     ioTag_t echoIO;
 } sonarHardware_t;
diff --git a/src/main/drivers/vtx_soft_spi_rtc6705.c b/src/main/drivers/vtx_soft_spi_rtc6705.c
index e66e5de603..b4b8ffce13 100644
--- a/src/main/drivers/vtx_soft_spi_rtc6705.c
+++ b/src/main/drivers/vtx_soft_spi_rtc6705.c
@@ -148,4 +148,9 @@ void rtc6705_soft_spi_set_channel(uint16_t channel_freq) {
     rtc6705_write_register(0, 400);
     rtc6705_write_register(1, (N << 7) | A);
 }
+
+void rtc6705_soft_spi_set_rf_power(uint8_t power) {
+    rtc6705_write_register(7, (power ? PA_CONTROL_DEFAULT : (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK))));
+}
+
 #endif
diff --git a/src/main/drivers/vtx_soft_spi_rtc6705.h b/src/main/drivers/vtx_soft_spi_rtc6705.h
index 6d761418bf..5f4314de1d 100644
--- a/src/main/drivers/vtx_soft_spi_rtc6705.h
+++ b/src/main/drivers/vtx_soft_spi_rtc6705.h
@@ -17,9 +17,21 @@
 
 #pragma once
 
+#define DP_5G_MASK      0x7000
+#define PA5G_BS_MASK    0x0E00
+#define PA5G_PW_MASK    0x0180
+#define PD_Q5G_MASK     0x0040
+#define QI_5G_MASK      0x0038
+#define PA_BS_MASK      0x0007
+
+#define PA_CONTROL_DEFAULT   0x4FBD
+
+
 extern char* vtx_bands[];
 extern uint16_t vtx_freq[];
 extern uint16_t current_vtx_channel;
 
 void rtc6705_soft_spi_init(void);
 void rtc6705_soft_spi_set_channel(uint16_t channel_freq);
+void rtc6705_soft_spi_set_rf_power(uint8_t power);
+
diff --git a/src/main/io/display.c b/src/main/io/display.c
index ed394d9c95..a61b64f276 100644
--- a/src/main/io/display.c
+++ b/src/main/io/display.c
@@ -63,9 +63,9 @@
 #include "config/runtime_config.h"
 #include "config/config_profile.h"
 
-#include "display.h"
+#include "io/display.h"
 
-#include "scheduler.h"
+#include "scheduler/scheduler.h"
 
 extern profile_t *currentProfile;
 
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 40fbcd3600..16a3ec96c1 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -22,7 +22,7 @@
 #include <math.h>
 
 #include "platform.h"
-#include "scheduler.h"
+#include "scheduler/scheduler.h"
 
 #include "common/axis.h"
 #include "common/color.h"
@@ -111,7 +111,6 @@
 #include "drivers/vtx_soft_spi_rtc6705.h"
 #endif
 
-#include "scheduler.h"
 #include "common/printf.h"
 
 #define MICROSECONDS_IN_A_SECOND (1000 * 1000)
@@ -713,6 +712,7 @@ void osdInit(void)
     rtc6705_soft_spi_init();
     current_vtx_channel = masterConfig.vtx_channel;
     rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
+    rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power);
 #endif
     max7456_init(masterConfig.osdProfile.system);
 
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 8bd73c94b0..e8d5ae9372 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -24,7 +24,6 @@
 #include <ctype.h>
 
 #include "platform.h"
-#include "scheduler.h"
 #include "version.h"
 #include "debug.h"
 
@@ -92,7 +91,8 @@
 
 #include "common/printf.h"
 
-#include "serial_cli.h"
+#include "io/serial_cli.h"
+#include "scheduler/scheduler.h"
 
 // FIXME remove this for targets that don't need a CLI.  Perhaps use a no-op macro when USE_CLI is not enabled
 // signal that we're in cli mode
@@ -302,6 +302,7 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("profile", "change profile",
         "[<index>]", cliProfile),
 	CLI_COMMAND_DEF("rateprofile", "change rate profile", "[<index>]", cliRateProfile),
+	CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource),
     CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
     CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFail),
     CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave),
@@ -805,6 +806,7 @@ const clivalue_t valueTable[] = {
 #endif
 #ifdef USE_RTC6705
     { "vtx_channel",                VAR_INT16  | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0,  39 } },
+    { "vtx_power",                  VAR_UINT8  | MASTER_VALUE, &masterConfig.vtx_power,   .config.minmax = { 0,  1 } },
 #endif
 #ifdef OSD
     { "osd_system",                 VAR_UINT8  | MASTER_VALUE, &masterConfig.osdProfile.system, .config.minmax = { 0, 2 } },
@@ -1892,7 +1894,7 @@ static void dumpValues(uint16_t valueSection)
         cliPrintf("set %s = ", valueTable[i].name);
         cliPrintVar(value, 0);
         cliPrint("\r\n");
-
+        
 #ifdef STM32F4
         delayMicroseconds(1000);
 #endif
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 0592833529..fe3d2cf8e9 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -24,7 +24,6 @@
 #include "build_config.h"
 #include "debug.h"
 #include "platform.h"
-#include "scheduler.h"
 
 #include "common/axis.h"
 #include "common/color.h"
@@ -64,6 +63,8 @@
 
 #include "telemetry/telemetry.h"
 
+#include "scheduler/scheduler.h"
+
 #include "sensors/boardalignment.h"
 #include "sensors/sensors.h"
 #include "sensors/battery.h"
diff --git a/src/main/main.c b/src/main/main.c
index 83690f2a26..e221da6dcb 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -22,8 +22,6 @@
 #include <math.h>
 
 #include "platform.h"
-#include "scheduler.h"
-
 #include "common/axis.h"
 #include "common/color.h"
 #include "common/maths.h"
@@ -78,6 +76,8 @@
 #include "io/osd.h"
 #include "io/vtx.h"
 
+#include "scheduler/scheduler.h"
+
 #include "sensors/sensors.h"
 #include "sensors/sonar.h"
 #include "sensors/barometer.h"
@@ -267,12 +267,11 @@ void init(void)
 
     if (feature(FEATURE_SONAR)) {
         sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig);
-        sonarGPIOConfig_t sonarGPIOConfig = {
-            .gpio = SONAR_GPIO,
-            .triggerPin = sonarHardware->echo_pin,
-            .echoPin = sonarHardware->trigger_pin,
+        sonarIOConfig_t sonarConfig = {
+            .triggerPin = sonarHardware->triggerIO,
+            .echoPin = sonarHardware->echoIO
         };
-        pwm_params.sonarGPIOConfig = &sonarGPIOConfig;
+        pwm_params.sonarConfig = &sonarConfig;
     }
 #endif
 
@@ -354,10 +353,6 @@ void init(void)
         .isInverted = false
 #endif
     };
-#ifdef AFROMINI
-    beeperConfig.isOD = true;
-    beeperConfig.isInverted = true;
-#endif
 #ifdef NAZE
     if (hardwareRevision >= NAZE32_REV5) {
         // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
diff --git a/src/main/mw.c b/src/main/mw.c
index a52258129e..abd0175f59 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -21,7 +21,6 @@
 #include <math.h>
 
 #include "platform.h"
-#include "scheduler.h"
 #include "debug.h"
 
 #include "common/maths.h"
@@ -88,6 +87,9 @@
 #include "config/config_profile.h"
 #include "config/config_master.h"
 
+#include "scheduler/scheduler.h"
+#include "scheduler/scheduler_tasks.h"
+
 // June 2013     V2.2-dev
 
 enum {
@@ -841,8 +843,9 @@ void taskUpdateBattery(void)
     }
 }
 
-bool taskUpdateRxCheck(void)
+bool taskUpdateRxCheck(uint32_t currentDeltaTime)
 {
+    UNUSED(currentDeltaTime);
     updateRx(currentTime);
     return shouldProcessRx(currentTime);
 }
diff --git a/src/main/scheduler.c b/src/main/scheduler/scheduler.c
similarity index 99%
rename from src/main/scheduler.c
rename to src/main/scheduler/scheduler.c
index 7e7b98469b..7fe3fea8ec 100755
--- a/src/main/scheduler.c
+++ b/src/main/scheduler/scheduler.c
@@ -23,10 +23,12 @@
 
 #include "platform.h"
 
-#include "scheduler.h"
 #include "debug.h"
 #include "build_config.h"
 
+#include "scheduler/scheduler.h"
+#include "scheduler/scheduler_tasks.h"
+
 #include "common/maths.h"
 
 #include "drivers/system.h"
diff --git a/src/main/scheduler.h b/src/main/scheduler/scheduler.h
similarity index 100%
rename from src/main/scheduler.h
rename to src/main/scheduler/scheduler.h
diff --git a/src/main/scheduler_tasks.c b/src/main/scheduler/scheduler_tasks.c
similarity index 87%
rename from src/main/scheduler_tasks.c
rename to src/main/scheduler/scheduler_tasks.c
index 393f6d2c0e..721ad27eb8 100644
--- a/src/main/scheduler_tasks.c
+++ b/src/main/scheduler/scheduler_tasks.c
@@ -20,33 +20,9 @@
 #include <stdint.h>
 
 #include <platform.h>
-#include "scheduler.h"
 
-void taskSystem(void);
-void taskMainPidLoopCheck(void);
-void taskUpdateAccelerometer(void);
-void taskUpdateAttitude(void);
-bool taskUpdateRxCheck(uint32_t currentDeltaTime);
-void taskUpdateRxMain(void);
-void taskHandleSerial(void);
-void taskUpdateBattery(void);
-void taskUpdateBeeper(void);
-void taskProcessGPS(void);
-void taskUpdateCompass(void);
-void taskUpdateBaro(void);
-void taskUpdateSonar(void);
-void taskCalculateAltitude(void);
-void taskUpdateDisplay(void);
-void taskTelemetry(void);
-void taskLedStrip(void);
-void taskTransponder(void);
-#ifdef OSD
-void taskUpdateOsd(void);
-#endif
-#ifdef USE_BST
-void taskBstReadWrite(void);
-void taskBstMasterProcess(void);
-#endif
+#include "scheduler/scheduler.h"
+#include "scheduler/scheduler_tasks.h"
 
 cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_SYSTEM] = {
diff --git a/src/main/scheduler/scheduler_tasks.h b/src/main/scheduler/scheduler_tasks.h
new file mode 100644
index 0000000000..8f3e123ad9
--- /dev/null
+++ b/src/main/scheduler/scheduler_tasks.h
@@ -0,0 +1,45 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+void taskSystem(void);
+void taskMainPidLoopCheck(void);
+void taskUpdateAccelerometer(void);
+void taskUpdateAttitude(void);
+bool taskUpdateRxCheck(uint32_t currentDeltaTime);
+void taskUpdateRxMain(void);
+void taskHandleSerial(void);
+void taskUpdateBattery(void);
+void taskUpdateBeeper(void);
+void taskProcessGPS(void);
+void taskUpdateCompass(void);
+void taskUpdateBaro(void);
+void taskUpdateSonar(void);
+void taskCalculateAltitude(void);
+void taskUpdateDisplay(void);
+void taskTelemetry(void);
+void taskLedStrip(void);
+void taskTransponder(void);
+#ifdef OSD
+void taskUpdateOsd(void);
+#endif
+#ifdef USE_BST
+void taskBstReadWrite(void);
+void taskBstMasterProcess(void);
+#endif
+
diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c
index e4ad51c3a9..2a011d8e2b 100644
--- a/src/main/sensors/barometer.c
+++ b/src/main/sensors/barometer.c
@@ -20,8 +20,6 @@
 #include <math.h>
 
 #include "platform.h"
-#include "scheduler.h"
-
 #include "common/maths.h"
 
 #include "drivers/barometer.h"
diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c
index 23bef4dc8c..73859f3917 100644
--- a/src/main/sensors/sonar.c
+++ b/src/main/sensors/sonar.c
@@ -26,7 +26,7 @@
 #include "common/axis.h"
 
 #include "drivers/sonar_hcsr04.h"
-#include "drivers/gpio.h"
+#include "drivers/io.h"
 #include "config/runtime_config.h"
 #include "config/config.h"
 
@@ -48,79 +48,21 @@ float sonarMaxTiltCos;
 
 static int32_t calculatedAltitude;
 
+#ifdef SONAR_CUSTOM_CONFIG
+const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig);
+#endif
+
 const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig)
 {
-#if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R)
-    static const sonarHardware_t const sonarPWM56 = {
-        .trigger_pin = Pin_8,   // PWM5 (PB8) - 5v tolerant
-        .trigger_gpio = GPIOB,
-        .echo_pin = Pin_9,      // PWM6 (PB9) - 5v tolerant
-        .echo_gpio = GPIOB,
-		.triggerIO = IO_TAG(PB8),
-		.echoIO = IO_TAG(PB9),
-    };
-    static const sonarHardware_t sonarRC78 = {
-        .trigger_pin = Pin_0,   // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
-        .trigger_gpio = GPIOB,
-        .echo_pin = Pin_1,      // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
-        .echo_gpio = GPIOB,
-		.triggerIO = IO_TAG(PB0),
-		.echoIO = IO_TAG(PB1),
-    };
-    // If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
-    if (feature(FEATURE_SOFTSERIAL)
-            || feature(FEATURE_RX_PARALLEL_PWM )
-            || (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC)) {
-        return &sonarPWM56;
-    } else {
-        return &sonarRC78;
-    }
-#elif defined(OLIMEXINO)
+#if defined(SONAR_TRIGGER_PIN) && defined(SONAR_ECHO_PIN)
     UNUSED(batteryConfig);
     static const sonarHardware_t const sonarHardware = {
-        .trigger_pin = Pin_0,   // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
-        .trigger_gpio = GPIOB,
-        .echo_pin = Pin_1,      // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
-        .echo_gpio = GPIOB,
-		.triggerIO = IO_TAG(PB0),
-		.echoIO = IO_TAG(PB1),
-    };
-    return &sonarHardware;
-#elif defined(CC3D)
-    UNUSED(batteryConfig);
-    static const sonarHardware_t const sonarHardware = {
-        .trigger_pin = Pin_5,   // (PB5)
-        .trigger_gpio = GPIOB,
-        .echo_pin = Pin_0,      // (PB0) - only 3.3v ( add a 1K Ohms resistor )
-        .echo_gpio = GPIOB,
-		.triggerIO = IO_TAG(PB5),
-		.echoIO = IO_TAG(PB0),
-    };
-    return &sonarHardware;
-    
-// TODO - move sonar pin selection to CLI
-#elif defined(SPRACINGF3) || defined(SPRACINGF3MINI) || defined(FURYF3) || defined(RMDO) || defined(OMNIBUS)
-    UNUSED(batteryConfig);
-    static const sonarHardware_t const sonarHardware = {
-        .trigger_pin = Pin_0,   // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
-        .trigger_gpio = GPIOB,
-        .echo_pin = Pin_1,      // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
-        .echo_gpio = GPIOB,
-		.triggerIO = IO_TAG(PB0),
-		.echoIO = IO_TAG(PB1),
-    };
-    return &sonarHardware;
-#elif defined(SPARKY)
-    UNUSED(batteryConfig);
-    static const sonarHardware_t const sonarHardware = {
-        .trigger_pin = Pin_2,   // PWM6 (PA2) - only 3.3v ( add a 1K Ohms resistor )
-        .trigger_gpio = GPIOA,
-        .echo_pin = Pin_1,      // PWM7 (PB1) - only 3.3v ( add a 1K Ohms resistor )
-        .echo_gpio = GPIOB,
-		.triggerIO = IO_TAG(PA2),
-		.echoIO = IO_TAG(PB1),
+		.triggerIO = IO_TAG(SONAR_TRIGGER_PIN),
+		.echoIO = IO_TAG(SONAR_ECHO_PIN),
     };
     return &sonarHardware;
+#elif defined(SONAR_CUSTOM_CONFIG)
+    return sonarGetTargetHardwareConfiguration(batteryConfig);
 #elif defined(UNIT_TEST)
     UNUSED(batteryConfig);
     return 0;
diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index 708d4547a0..dc39a80c52 100644
--- a/src/main/target/CC3D/target.h
+++ b/src/main/target/CC3D/target.h
@@ -118,6 +118,9 @@
 
 #define DISPLAY
 #define SONAR
+#define SONAR_ECHO_PIN      PB0
+#define SONAR_TRIGGER_PIN   PB5
+
 #undef GPS
 
 #undef BARO
diff --git a/src/main/target/EUSTM32F103RC/sonar_config.c b/src/main/target/EUSTM32F103RC/sonar_config.c
new file mode 100644
index 0000000000..e2f4fb7b19
--- /dev/null
+++ b/src/main/target/EUSTM32F103RC/sonar_config.c
@@ -0,0 +1,28 @@
+
+#include "drivers/sonar_hcsr04.h"
+#include "drivers/io.h"
+#include "config/runtime_config.h"
+#include "config/config.h"
+
+#include "sensors/battery.h"
+#include "sensors/sonar.h"
+
+const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig)
+{
+    static const sonarHardware_t const sonarPWM56 = {
+        .triggerIO = IO_TAG(PB8),
+        .echoIO = IO_TAG(PB9),
+    };
+    static const sonarHardware_t sonarRC78 = {
+        .triggerIO = IO_TAG(PB0),
+        .echoIO = IO_TAG(PB1),
+    };
+    // If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
+    if (feature(FEATURE_SOFTSERIAL)
+            || feature(FEATURE_RX_PARALLEL_PWM )
+            || (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC)) {
+        return &sonarPWM56;
+    } else {
+        return &sonarRC78;
+    }
+}
\ No newline at end of file
diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h
index ef7534c663..83fe60e92d 100644
--- a/src/main/target/EUSTM32F103RC/target.h
+++ b/src/main/target/EUSTM32F103RC/target.h
@@ -69,6 +69,8 @@
 #define MAG_AK8975_ALIGN CW180_DEG_FLIP
 
 #define SONAR
+#define SONAR_CUSTOM_CONFIG
+
 #define DISPLAY
 
 #define USE_USART1
diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h
index fb4b899ea2..c68ce2c085 100644
--- a/src/main/target/FURYF3/target.h
+++ b/src/main/target/FURYF3/target.h
@@ -184,6 +184,9 @@
 #define WS2811_DMA_HANDLER_IDENTIFER    DMA1_CH2_HANDLER
 
 #define SONAR
+#define SONAR_ECHO_PIN      PB1
+#define SONAR_TRIGGER_PIN   PB0
+
 #define DEFAULT_RX_FEATURE FEATURE_RX_PPM
 #define DEFAULT_FEATURES FEATURE_BLACKBOX
 
diff --git a/src/main/target/NAZE/AFROMINI.mk b/src/main/target/NAZE/AFROMINI.mk
new file mode 100644
index 0000000000..0f11de8fe6
--- /dev/null
+++ b/src/main/target/NAZE/AFROMINI.mk
@@ -0,0 +1 @@
+# AFROMINI is a VARIANT of NAZE being recognized as rev4, but needs to use rev5
diff --git a/src/main/target/NAZE/sonar_config.c b/src/main/target/NAZE/sonar_config.c
new file mode 100644
index 0000000000..3362146c0d
--- /dev/null
+++ b/src/main/target/NAZE/sonar_config.c
@@ -0,0 +1,28 @@
+
+#include "drivers/sonar_hcsr04.h"
+#include "drivers/io.h"
+#include "config/runtime_config.h"
+#include "config/config.h"
+
+#include "sensors/battery.h"
+#include "sensors/sonar.h"
+
+const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig)
+{
+    static const sonarHardware_t const sonarPWM56 = {
+		.triggerIO = IO_TAG(PB8),
+		.echoIO = IO_TAG(PB9),
+    };
+    static const sonarHardware_t sonarRC78 = {
+		.triggerIO = IO_TAG(PB0),
+		.echoIO = IO_TAG(PB1),
+    };
+    // If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
+    if (feature(FEATURE_SOFTSERIAL)
+            || feature(FEATURE_RX_PARALLEL_PWM )
+            || (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC)) {
+        return &sonarPWM56;
+    } else {
+        return &sonarRC78;
+    }
+}
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index ffabc5e584..557f295af0 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -26,6 +26,9 @@
 #define LED1    PB4 // PB4 (LED)
 
 #define BEEPER      PA12 // PA12 (Beeper)
+#ifdef AFROMINI
+#define BEEPER_INVERTED
+#endif
 
 #define BARO_XCLR_PIN    PC13
 #define BARO_EOC_PIN     PC14
@@ -108,6 +111,7 @@
 #define MAG_HMC5883_ALIGN CW180_DEG
 
 #define SONAR
+#define SONAR_CUSTOM_CONFIG
 #define DISPLAY
 
 #define USE_USART1
diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h
index 86a4304723..c50c1d51fc 100644
--- a/src/main/target/OLIMEXINO/target.h
+++ b/src/main/target/OLIMEXINO/target.h
@@ -59,6 +59,8 @@
 #define USE_MAG_HMC5883
 
 #define SONAR
+#define SONAR_ECHO_PIN      PB1
+#define SONAR_TRIGGER_PIN   PB0
 
 #define USE_USART1
 #define USE_USART2
diff --git a/src/main/target/PORT103R/sonar_config.c b/src/main/target/PORT103R/sonar_config.c
new file mode 100644
index 0000000000..e2f4fb7b19
--- /dev/null
+++ b/src/main/target/PORT103R/sonar_config.c
@@ -0,0 +1,28 @@
+
+#include "drivers/sonar_hcsr04.h"
+#include "drivers/io.h"
+#include "config/runtime_config.h"
+#include "config/config.h"
+
+#include "sensors/battery.h"
+#include "sensors/sonar.h"
+
+const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig)
+{
+    static const sonarHardware_t const sonarPWM56 = {
+        .triggerIO = IO_TAG(PB8),
+        .echoIO = IO_TAG(PB9),
+    };
+    static const sonarHardware_t sonarRC78 = {
+        .triggerIO = IO_TAG(PB0),
+        .echoIO = IO_TAG(PB1),
+    };
+    // If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
+    if (feature(FEATURE_SOFTSERIAL)
+            || feature(FEATURE_RX_PARALLEL_PWM )
+            || (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC)) {
+        return &sonarPWM56;
+    } else {
+        return &sonarRC78;
+    }
+}
\ No newline at end of file
diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h
index c0dfc4aa33..1cf06398cb 100644
--- a/src/main/target/PORT103R/target.h
+++ b/src/main/target/PORT103R/target.h
@@ -87,6 +87,8 @@
 #define USE_FLASH_M25P16
 
 #define SONAR
+#define SONAR_CUSTOM_CONFIG
+
 #define DISPLAY
 
 #define USE_USART1
diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h
index 09a3497eaa..24234ea27b 100644
--- a/src/main/target/RMDO/target.h
+++ b/src/main/target/RMDO/target.h
@@ -52,6 +52,8 @@
 #define USE_FLASH_M25P16
 
 #define SONAR
+#define SONAR_ECHO_PIN      PB1
+#define SONAR_TRIGGER_PIN   PB0
 
 #define USE_USART1
 #define USE_USART2
diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c
index 4ccf123ea2..4ac409d394 100644
--- a/src/main/target/SIRINFPV/target.c
+++ b/src/main/target/SIRINFPV/target.c
@@ -49,13 +49,13 @@ const uint16_t airPWM[] = {
 
 const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
 
-    { TIM4,  GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn,               1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_2, 0}, // PWM1 - PB6
-    { TIM4,  GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn,               1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_2, 0}, // PWM2 - PB6
-    { TIM4,  GPIOB, Pin_8,  TIM_Channel_3, TIM4_IRQn,               1, Mode_AF_PP, GPIO_PinSource8,  GPIO_AF_2, 0},  // PWM3 - PB8
-    { TIM4,  GPIOB, Pin_9,  TIM_Channel_4, TIM4_IRQn,               1, Mode_AF_PP, GPIO_PinSource9,  GPIO_AF_2, 0},  // PWM4 - PB9
+    { TIM4,  IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn,               1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM1 - PB6
+    { TIM4,  IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn,               1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM2 - PB6
+    { TIM4,  IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn,               1, IOCFG_AF_PP, GPIO_AF_2, 0},  // PWM3 - PB8
+    { TIM4,  IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn,               1, IOCFG_AF_PP, GPIO_AF_2, 0},  // PWM4 - PB9
 
-    { TIM3,  GPIOB, Pin_0,  TIM_Channel_3, TIM3_IRQn,               1, Mode_AF_PP, GPIO_PinSource0,  GPIO_AF_2, 0}, // PWM5 - PB0  - *TIM3_CH3
-    { TIM3,  GPIOB, Pin_1,  TIM_Channel_4, TIM3_IRQn,               1, Mode_AF_PP, GPIO_PinSource1,  GPIO_AF_2, 0}, // PWM6 - PB1  - *TIM3_CH4
+    { TIM3,  IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn,               1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB0  - *TIM3_CH3
+    { TIM3,  IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn,               1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB1  - *TIM3_CH4
 
 };
 
diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h
index 7e3e2ed9dd..32b783d64e 100644
--- a/src/main/target/SPARKY/target.h
+++ b/src/main/target/SPARKY/target.h
@@ -156,6 +156,9 @@
 // USART2, PA3
 #define BIND_PIN PA3
 
+//#define SONAR
+//#define SONAR_ECHO_PIN      PB1
+//#define SONAR_TRIGGER_PIN   PA2
 
 // available IO pins (from schematics)
 //#define TARGET_IO_PORTA (BIT(1)|BIT(2)|BIT(3)|BIT(4)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(10)|BIT(11)|BIT(12)|BIT(13)|BIT(14)|BIT(15))
diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h
index c041b54d92..52263deb8b 100644
--- a/src/main/target/SPRACINGF3/target.h
+++ b/src/main/target/SPRACINGF3/target.h
@@ -61,6 +61,8 @@
 #define USE_FLASH_M25P16
 
 #define SONAR
+#define SONAR_ECHO_PIN      PB1
+#define SONAR_TRIGGER_PIN   PB0
 
 #define USE_USART1
 #define USE_USART2
diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h
index 5272a49030..6063abd97d 100644
--- a/src/main/target/SPRACINGF3MINI/target.h
+++ b/src/main/target/SPRACINGF3MINI/target.h
@@ -24,9 +24,9 @@
 // early prototype had slightly different pin mappings.
 //#define SPRACINGF3MINI_MKII_REVA
 
-#define LED0    PB3
+#define LED0            PB3
 
-#define BEEPER    PC15
+#define BEEPER          PC15
 #define BEEPER_INVERTED
 
 #define USABLE_TIMER_CHANNEL_COUNT 12 // 8 Outputs; PPM; LED Strip; 2 additional PWM pins also on UART3 RX/TX pins.
@@ -50,7 +50,7 @@
 //#define USE_FAKE_ACC
 #define USE_ACC_MPU6500
 
-#define ACC_MPU6500_ALIGN CW180_DEG
+#define ACC_MPU6500_ALIGN  CW180_DEG
 #define GYRO_MPU6500_ALIGN CW180_DEG
 
 #define BARO
@@ -64,11 +64,13 @@
 #define MAG_AK8975_ALIGN CW90_DEG_FLIP
 
 #define SONAR
+#define SONAR_ECHO_PIN      PB1
+#define SONAR_TRIGGER_PIN   PB0
 
 #define USB_IO
 #define USB_CABLE_DETECTION
 
-#define USB_DETECT_PIN                   PB5
+#define USB_DETECT_PIN      PB5
 
 #define USE_VCP
 #define USE_USART1
diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc
index 7944809cfa..10814d1790 100644
--- a/src/test/unit/scheduler_unittest.cc
+++ b/src/test/unit/scheduler_unittest.cc
@@ -19,7 +19,7 @@
 
 extern "C" {
     #include "platform.h"
-    #include "scheduler.h"
+    #include "scheduler/scheduler.h"
 }
 
 #include "unittest_macros.h"