diff --git a/Makefile b/Makefile
index fb06439ec5..f36211887c 100644
--- a/Makefile
+++ b/Makefile
@@ -42,66 +42,75 @@ BIN_DIR		 = $(ROOT)/obj
 
 # Source files common to all targets
 COMMON_SRC	 = startup_stm32f10x_md_gcc.S \
+		   battery.c \
+		   boardalignment.c \
 		   buzzer.c \
 		   config.c \
-		   drv_system.c \
 		   main.c \
 		   mw.c \
 		   printf.c \
-		   sensors.c \
-		   utils.c \
-		   drivers/bus/drv_i2c.c \
-		   drivers/bus/drv_i2c_soft.c \
-		   drivers/gpio/drv_gpio.c \
-		   drivers/serial/drv_serial.c \
-		   drivers/serial/drv_softserial.c \
-		   drivers/serial/drv_uart.c \
-		   flight/imu.c \
-		   flight/mixer.c \
-		   gps/gps.c \
-		   rx/sbus.c \
-		   rx/sumd.c \
-		   rx/spektrum.c \
-		   telemetry/telemetry_common.c \
-		   telemetry/telemetry_frsky.c \
-		   telemetry/telemetry_hott.c \
-		   ui/cli.c \
-		   ui/serial.c \
+		   sensors_acceleration.c \
+		   sensors_altitude.c \
+		   sensors_compass.c \
+		   sensors_gyro.c \
+		   sensors_initialisation.c \
+		   sensors_sonar.c \
+		   maths.c \
+		   typeconversion.c \
+		   drivers/bus_i2c.c \
+		   drivers/bus_i2c_soft.c \
+		   drivers/gpio_common.c \
+		   drivers/serial_common.c \
+		   drivers/serial_softserial.c \
+		   drivers/serial_uart.c \
+		   drivers/system_common.c \
+		   flight_imu.c \
+		   flight_mixer.c \
+		   gps_common.c \
+		   rx_sbus.c \
+		   rx_sumd.c \
+		   rx_spektrum.c \
+		   telemetry_common.c \
+		   telemetry_frsky.c \
+		   telemetry_hott.c \
+		   typeconversion.c \
+		   serial_cli.c \
+		   serial_msp.c \
 		   $(CMSIS_SRC) \
 		   $(STDPERIPH_SRC)
 
 # Source files for the NAZE target
-NAZE_SRC	 = drv_pwm.c \
-		   drivers/accgyro/drv_adxl345.c \
-		   drivers/accgyro/drv_bma280.c \
-		   drivers/accgyro/drv_l3g4200d.c \
-		   drivers/accgyro/drv_mma845x.c \
-		   drivers/accgyro/drv_mpu3050.c \
-		   drivers/accgyro/drv_mpu6050.c \
-		   drivers/adc/drv_adc.c \
-		   drivers/altimeter/drv_bmp085.c \
-		   drivers/altimeter/drv_ms5611.c \
-		   drivers/bus/drv_spi.c \
-		   drivers/compass/drv_hmc5883l.c \
-		   drivers/light/drv_ledring.c \
-		   drivers/sonar/drv_hcsr04.c \
-		   drivers/timer/drv_timer.c \
+NAZE_SRC	 = drivers/accgyro_adxl345.c \
+		   drivers/accgyro_bma280.c \
+		   drivers/accgyro_l3g4200d.c \
+		   drivers/accgyro_mma845x.c \
+		   drivers/accgyro_mpu3050.c \
+		   drivers/accgyro_mpu6050.c \
+		   drivers/adc_common.c \
+		   drivers/altimeter_bmp085.c \
+		   drivers/altimeter_ms5611.c \
+		   drivers/bus_spi.c \
+		   drivers/compass_hmc5883l.c \
+		   drivers/light_ledring.c \
+		   drivers/sonar_hcsr04.c \
+		   drivers/pwm_common.c \
+		   drivers/timer_common.c \
 		   $(COMMON_SRC)
 
 # Source files for the FY90Q target
-FY90Q_SRC	 = drv_adc_fy90q.c \
-		   drv_pwm_fy90q.c \
+FY90Q_SRC	 = drivers/adc_fy90q.c \
+		   drivers/pwm_fy90q.c \
 		   $(COMMON_SRC)
 
 # Source files for the OLIMEXINO target
-OLIMEXINO_SRC	 = drv_pwm.c \
-		   drivers/accgyro/drv_adxl345.c \
-		   drivers/accgyro/drv_mpu3050.c \
-		   drivers/accgyro/drv_mpu6050.c \
-		   drivers/accgyro/drv_l3g4200d.c \
-		   drivers/adc/drv_adc.c \
-		   drivers/bus/drv_spi.c \
-		   drivers/timer/drv_timer.c \
+OLIMEXINO_SRC	 = drivers/accgyro_adxl345.c \
+		   drivers/accgyro_mpu3050.c \
+		   drivers/accgyro_mpu6050.c \
+		   drivers/accgyro_l3g4200d.c \
+		   drivers/adc_common.c \
+		   drivers/bus_spi.c \
+		   drivers/pwm_common.c \
+		   drivers/timer_common.c \
 		   $(COMMON_SRC)
 		   
 # In some cases, %.s regarded as intermediate file, which is actually not.
diff --git a/src/battery.c b/src/battery.c
new file mode 100644
index 0000000000..2a9b4c4733
--- /dev/null
+++ b/src/battery.c
@@ -0,0 +1,35 @@
+#include "board.h"
+#include "mw.h"
+
+// Battery monitoring stuff
+uint8_t batteryCellCount = 3;       // cell count
+uint16_t batteryWarningVoltage;     // annoying buzzer after this one, battery ready to be dead
+
+uint16_t batteryAdcToVoltage(uint16_t src)
+{
+    // calculate battery voltage based on ADC reading
+    // result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 4095 = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V
+    return (((src) * 3.3f) / 4095) * mcfg.vbatscale;
+}
+
+void batteryInit(void)
+{
+    uint32_t i;
+    uint32_t voltage = 0;
+
+    // average up some voltage readings
+    for (i = 0; i < 32; i++) {
+        voltage += adcGetChannel(ADC_BATTERY);
+        delay(10);
+    }
+
+    voltage = batteryAdcToVoltage((uint16_t)(voltage / 32));
+
+    // autodetect cell count, going from 2S..6S
+    for (i = 2; i < 6; i++) {
+        if (voltage < i * mcfg.vbatmaxcellvoltage)
+            break;
+    }
+    batteryCellCount = i;
+    batteryWarningVoltage = i * mcfg.vbatmincellvoltage; // 3.3V per cell minimum, configurable in CLI
+}
diff --git a/src/battery.h b/src/battery.h
new file mode 100644
index 0000000000..6b6041bd79
--- /dev/null
+++ b/src/battery.h
@@ -0,0 +1,5 @@
+
+extern uint16_t batteryWarningVoltage;
+
+uint16_t batteryAdcToVoltage(uint16_t src);
+void batteryInit(void);
diff --git a/src/board.h b/src/board.h
index 620a297d06..af9f67db42 100755
--- a/src/board.h
+++ b/src/board.h
@@ -17,11 +17,13 @@
 #ifndef __CC_ARM
 // only need this garbage on gcc
 #define USE_LAME_PRINTF
+#define PRINTF_LONG_SUPPORT
+
 #include "printf.h"
 #endif
 
-#include "drv_system.h"         // timers, delays, etc
-#include "drivers/gpio/drv_gpio.h"
+#include "drivers/system_common.h"         // timers, delays, etc
+#include "drivers/gpio_common.h"
 
 #ifndef M_PI
 #define M_PI       3.14159265358979323846f
@@ -274,7 +276,9 @@ typedef struct baro_t
 
 #undef SOFT_I2C                 // enable to test software i2c
 
-#include "utils.h"
+#include "boardalignment.h"
+#include "battery.h"
+#include "math.h"
 
 #ifdef FY90Q
  // FY90Q
@@ -286,40 +290,40 @@ typedef struct baro_t
 
 #ifdef OLIMEXINO
 // OLIMEXINO
-#include "drivers/adc/drv_adc.h"
-#include "drivers/bus/drv_i2c.h"
-#include "drivers/bus/drv_spi.h"
-#include "drivers/accgyro/drv_adxl345.h"
-#include "drivers/accgyro/drv_mpu3050.h"
-#include "drivers/accgyro/drv_mpu6050.h"
-#include "drivers/accgyro/drv_l3g4200d.h"
-#include "drv_pwm.h"
-#include "drivers/timer/drv_timer.h"
-#include "drivers/serial/drv_serial.h"
-#include "drivers/serial/drv_uart.h"
-#include "drivers/serial/drv_softserial.h"
+#include "drivers/adc_common.h"
+#include "drivers/bus_i2c.h"
+#include "drivers/bus_spi.h"
+#include "drivers/accgyro_adxl345.h"
+#include "drivers/accgyro_mpu3050.h"
+#include "drivers/accgyro_mpu6050.h"
+#include "drivers/accgyro_l3g4200d.h"
+#include "drivers/pwm_common.h"
+#include "drivers/timer_common.h"
+#include "drivers/serial_common.h"
+#include "drivers/serial_uart.h"
+#include "drivers/serial_softserial.h"
 #else
 
  // AfroFlight32
-#include "drivers/adc/drv_adc.h"
-#include "drivers/accgyro/drv_adxl345.h"
-#include "drivers/accgyro/drv_bma280.h"
-#include "drivers/altimeter/drv_bmp085.h"
-#include "drivers/altimeter/drv_ms5611.h"
-#include "drivers/compass/drv_hmc5883l.h"
-#include "drivers/bus/drv_i2c.h"
-#include "drivers/bus/drv_spi.h"
-#include "drivers/light/drv_ledring.h"
-#include "drivers/accgyro/drv_mma845x.h"
-#include "drivers/accgyro/drv_mpu3050.h"
-#include "drivers/accgyro/drv_mpu6050.h"
-#include "drivers/accgyro/drv_l3g4200d.h"
-#include "drv_pwm.h"
-#include "drivers/timer/drv_timer.h"
-#include "drivers/serial/drv_serial.h"
-#include "drivers/serial/drv_uart.h"
-#include "drivers/serial/drv_softserial.h"
-#include "drivers/sonar/drv_hcsr04.h"
+#include "drivers/adc_common.h"
+#include "drivers/accgyro_adxl345.h"
+#include "drivers/accgyro_bma280.h"
+#include "drivers/altimeter_bmp085.h"
+#include "drivers/altimeter_ms5611.h"
+#include "drivers/compass_hmc5883l.h"
+#include "drivers/bus_i2c.h"
+#include "drivers/bus_spi.h"
+#include "drivers/light_ledring.h"
+#include "drivers/accgyro_mma845x.h"
+#include "drivers/accgyro_mpu3050.h"
+#include "drivers/accgyro_mpu6050.h"
+#include "drivers/accgyro_l3g4200d.h"
+#include "drivers/pwm_common.h"
+#include "drivers/timer_common.h"
+#include "drivers/serial_common.h"
+#include "drivers/serial_uart.h"
+#include "drivers/serial_softserial.h"
+#include "drivers/sonar_hcsr04.h"
 
 #endif
 #endif
diff --git a/src/utils.c b/src/boardalignment.c
similarity index 91%
rename from src/utils.c
rename to src/boardalignment.c
index db0b2697d9..bf85ce63a3 100644
--- a/src/utils.c
+++ b/src/boardalignment.c
@@ -4,16 +4,6 @@
 static bool standardBoardAlignment = true;     // board orientation correction
 static float boardRotation[3][3];              // matrix
 
-int constrain(int amt, int low, int high)
-{
-    if (amt < low)
-        return low;
-    else if (amt > high)
-        return high;
-    else
-        return amt;
-}
-
 void initBoardAlignment(void)
 {
     float roll, pitch, yaw;
diff --git a/src/utils.h b/src/boardalignment.h
similarity index 73%
rename from src/utils.h
rename to src/boardalignment.h
index c6ef6b8cbb..e995adfd21 100644
--- a/src/utils.h
+++ b/src/boardalignment.h
@@ -1,6 +1,5 @@
 #pragma once
 
-int constrain(int amt, int low, int high);
 // sensor orientation
 void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation);
 void initBoardAlignment(void);
diff --git a/src/drivers/accgyro/drv_adxl345.c b/src/drivers/accgyro_adxl345.c
similarity index 100%
rename from src/drivers/accgyro/drv_adxl345.c
rename to src/drivers/accgyro_adxl345.c
diff --git a/src/drivers/accgyro/drv_adxl345.h b/src/drivers/accgyro_adxl345.h
similarity index 100%
rename from src/drivers/accgyro/drv_adxl345.h
rename to src/drivers/accgyro_adxl345.h
diff --git a/src/drivers/accgyro/drv_bma280.c b/src/drivers/accgyro_bma280.c
similarity index 100%
rename from src/drivers/accgyro/drv_bma280.c
rename to src/drivers/accgyro_bma280.c
diff --git a/src/drivers/accgyro/drv_bma280.h b/src/drivers/accgyro_bma280.h
similarity index 100%
rename from src/drivers/accgyro/drv_bma280.h
rename to src/drivers/accgyro_bma280.h
diff --git a/src/drivers/accgyro/drv_l3g4200d.c b/src/drivers/accgyro_l3g4200d.c
similarity index 100%
rename from src/drivers/accgyro/drv_l3g4200d.c
rename to src/drivers/accgyro_l3g4200d.c
diff --git a/src/drivers/accgyro/drv_l3g4200d.h b/src/drivers/accgyro_l3g4200d.h
similarity index 100%
rename from src/drivers/accgyro/drv_l3g4200d.h
rename to src/drivers/accgyro_l3g4200d.h
diff --git a/src/drivers/accgyro/drv_mma845x.c b/src/drivers/accgyro_mma845x.c
similarity index 100%
rename from src/drivers/accgyro/drv_mma845x.c
rename to src/drivers/accgyro_mma845x.c
diff --git a/src/drivers/accgyro/drv_mma845x.h b/src/drivers/accgyro_mma845x.h
similarity index 100%
rename from src/drivers/accgyro/drv_mma845x.h
rename to src/drivers/accgyro_mma845x.h
diff --git a/src/drivers/accgyro/drv_mpu3050.c b/src/drivers/accgyro_mpu3050.c
similarity index 100%
rename from src/drivers/accgyro/drv_mpu3050.c
rename to src/drivers/accgyro_mpu3050.c
diff --git a/src/drivers/accgyro/drv_mpu3050.h b/src/drivers/accgyro_mpu3050.h
similarity index 100%
rename from src/drivers/accgyro/drv_mpu3050.h
rename to src/drivers/accgyro_mpu3050.h
diff --git a/src/drivers/accgyro/drv_mpu6050.c b/src/drivers/accgyro_mpu6050.c
similarity index 100%
rename from src/drivers/accgyro/drv_mpu6050.c
rename to src/drivers/accgyro_mpu6050.c
diff --git a/src/drivers/accgyro/drv_mpu6050.h b/src/drivers/accgyro_mpu6050.h
similarity index 100%
rename from src/drivers/accgyro/drv_mpu6050.h
rename to src/drivers/accgyro_mpu6050.h
diff --git a/src/drivers/adc/drv_adc.c b/src/drivers/adc_common.c
similarity index 100%
rename from src/drivers/adc/drv_adc.c
rename to src/drivers/adc_common.c
diff --git a/src/drivers/adc/drv_adc.h b/src/drivers/adc_common.h
similarity index 100%
rename from src/drivers/adc/drv_adc.h
rename to src/drivers/adc_common.h
diff --git a/src/drivers/adc/drv_adc_fy90q.c b/src/drivers/adc_fy90q.c
similarity index 100%
rename from src/drivers/adc/drv_adc_fy90q.c
rename to src/drivers/adc_fy90q.c
diff --git a/src/drivers/altimeter/drv_bmp085.c b/src/drivers/altimeter_bmp085.c
similarity index 100%
rename from src/drivers/altimeter/drv_bmp085.c
rename to src/drivers/altimeter_bmp085.c
diff --git a/src/drivers/altimeter/drv_bmp085.h b/src/drivers/altimeter_bmp085.h
similarity index 100%
rename from src/drivers/altimeter/drv_bmp085.h
rename to src/drivers/altimeter_bmp085.h
diff --git a/src/drivers/altimeter/drv_ms5611.c b/src/drivers/altimeter_ms5611.c
similarity index 100%
rename from src/drivers/altimeter/drv_ms5611.c
rename to src/drivers/altimeter_ms5611.c
diff --git a/src/drivers/altimeter/drv_ms5611.h b/src/drivers/altimeter_ms5611.h
similarity index 100%
rename from src/drivers/altimeter/drv_ms5611.h
rename to src/drivers/altimeter_ms5611.h
diff --git a/src/drivers/bus/drv_i2c.c b/src/drivers/bus_i2c.c
similarity index 100%
rename from src/drivers/bus/drv_i2c.c
rename to src/drivers/bus_i2c.c
diff --git a/src/drivers/bus/drv_i2c.h b/src/drivers/bus_i2c.h
similarity index 100%
rename from src/drivers/bus/drv_i2c.h
rename to src/drivers/bus_i2c.h
diff --git a/src/drivers/bus/drv_i2c_soft.c b/src/drivers/bus_i2c_soft.c
similarity index 100%
rename from src/drivers/bus/drv_i2c_soft.c
rename to src/drivers/bus_i2c_soft.c
diff --git a/src/drivers/bus/drv_spi.c b/src/drivers/bus_spi.c
similarity index 100%
rename from src/drivers/bus/drv_spi.c
rename to src/drivers/bus_spi.c
diff --git a/src/drivers/bus/drv_spi.h b/src/drivers/bus_spi.h
similarity index 100%
rename from src/drivers/bus/drv_spi.h
rename to src/drivers/bus_spi.h
diff --git a/src/drivers/compass/drv_hmc5883l.c b/src/drivers/compass_hmc5883l.c
similarity index 100%
rename from src/drivers/compass/drv_hmc5883l.c
rename to src/drivers/compass_hmc5883l.c
diff --git a/src/drivers/compass/drv_hmc5883l.h b/src/drivers/compass_hmc5883l.h
similarity index 100%
rename from src/drivers/compass/drv_hmc5883l.h
rename to src/drivers/compass_hmc5883l.h
diff --git a/src/drivers/gpio/drv_gpio.c b/src/drivers/gpio_common.c
similarity index 100%
rename from src/drivers/gpio/drv_gpio.c
rename to src/drivers/gpio_common.c
diff --git a/src/drivers/gpio/drv_gpio.h b/src/drivers/gpio_common.h
similarity index 100%
rename from src/drivers/gpio/drv_gpio.h
rename to src/drivers/gpio_common.h
diff --git a/src/drivers/light/drv_ledring.c b/src/drivers/light_ledring.c
similarity index 93%
rename from src/drivers/light/drv_ledring.c
rename to src/drivers/light_ledring.c
index 3e94383891..dd44d7d8b1 100644
--- a/src/drivers/light/drv_ledring.c
+++ b/src/drivers/light_ledring.c
@@ -1,6 +1,8 @@
 #include "board.h"
 #include "mw.h"
 
+#include "maths.h"
+
 // Driver for DFRobot I2C Led Ring
 #define LED_RING_ADDRESS    0x6D
 
diff --git a/src/drivers/light/drv_ledring.h b/src/drivers/light_ledring.h
similarity index 100%
rename from src/drivers/light/drv_ledring.h
rename to src/drivers/light_ledring.h
diff --git a/src/drv_pwm.c b/src/drivers/pwm_common.c
similarity index 100%
rename from src/drv_pwm.c
rename to src/drivers/pwm_common.c
diff --git a/src/drv_pwm.h b/src/drivers/pwm_common.h
similarity index 100%
rename from src/drv_pwm.h
rename to src/drivers/pwm_common.h
diff --git a/src/drv_pwm_fy90q.c b/src/drivers/pwm_fy90q.c
similarity index 100%
rename from src/drv_pwm_fy90q.c
rename to src/drivers/pwm_fy90q.c
diff --git a/src/drivers/serial/drv_serial.c b/src/drivers/serial_common.c
similarity index 100%
rename from src/drivers/serial/drv_serial.c
rename to src/drivers/serial_common.c
diff --git a/src/drivers/serial/drv_serial.h b/src/drivers/serial_common.h
similarity index 100%
rename from src/drivers/serial/drv_serial.h
rename to src/drivers/serial_common.h
diff --git a/src/drivers/serial/drv_softserial.c b/src/drivers/serial_softserial.c
similarity index 100%
rename from src/drivers/serial/drv_softserial.c
rename to src/drivers/serial_softserial.c
diff --git a/src/drivers/serial/drv_softserial.h b/src/drivers/serial_softserial.h
similarity index 100%
rename from src/drivers/serial/drv_softserial.h
rename to src/drivers/serial_softserial.h
diff --git a/src/drivers/serial/drv_uart.c b/src/drivers/serial_uart.c
similarity index 100%
rename from src/drivers/serial/drv_uart.c
rename to src/drivers/serial_uart.c
diff --git a/src/drivers/serial/drv_uart.h b/src/drivers/serial_uart.h
similarity index 100%
rename from src/drivers/serial/drv_uart.h
rename to src/drivers/serial_uart.h
diff --git a/src/drivers/sonar/drv_hcsr04.c b/src/drivers/sonar_hcsr04.c
similarity index 100%
rename from src/drivers/sonar/drv_hcsr04.c
rename to src/drivers/sonar_hcsr04.c
diff --git a/src/drivers/sonar/drv_hcsr04.h b/src/drivers/sonar_hcsr04.h
similarity index 100%
rename from src/drivers/sonar/drv_hcsr04.h
rename to src/drivers/sonar_hcsr04.h
diff --git a/src/drv_system.c b/src/drivers/system_common.c
similarity index 100%
rename from src/drv_system.c
rename to src/drivers/system_common.c
diff --git a/src/drv_system.h b/src/drivers/system_common.h
similarity index 100%
rename from src/drv_system.h
rename to src/drivers/system_common.h
diff --git a/src/drivers/timer/drv_timer.c b/src/drivers/timer_common.c
similarity index 100%
rename from src/drivers/timer/drv_timer.c
rename to src/drivers/timer_common.c
diff --git a/src/drivers/timer/drv_timer.h b/src/drivers/timer_common.h
similarity index 100%
rename from src/drivers/timer/drv_timer.h
rename to src/drivers/timer_common.h
diff --git a/src/flight/imu.c b/src/flight_imu.c
similarity index 96%
rename from src/flight/imu.c
rename to src/flight_imu.c
index 91637cffc7..c4874255b9 100644
--- a/src/flight/imu.c
+++ b/src/flight_imu.c
@@ -1,6 +1,8 @@
 #include "board.h"
 #include "mw.h"
 
+#include "maths.h"
+
 int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
 int32_t accSum[3];
 uint32_t accTimeSum = 0;        // keep track for integration of acc
diff --git a/src/flight/mixer.c b/src/flight_mixer.c
similarity index 97%
rename from src/flight/mixer.c
rename to src/flight_mixer.c
index d982c8e4dd..2464c82a0e 100755
--- a/src/flight/mixer.c
+++ b/src/flight_mixer.c
@@ -1,6 +1,8 @@
 #include "board.h"
 #include "mw.h"
 
+#include "maths.h"
+
 static uint8_t numberMotor = 0;
 int16_t motor[MAX_MOTORS];
 int16_t motor_disarmed[MAX_MOTORS];
diff --git a/src/gps/gps.c b/src/gps_common.c
similarity index 97%
rename from src/gps/gps.c
rename to src/gps_common.c
index 98b34d89e7..885510e270 100644
--- a/src/gps/gps.c
+++ b/src/gps_common.c
@@ -1,9 +1,7 @@
 #include "board.h"
 #include "mw.h"
 
-#ifndef sq
-#define sq(x) ((x)*(x))
-#endif
+#include "maths.h"
 
 // GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2.5second)
 #define GPS_TIMEOUT (2500)
diff --git a/src/main.c b/src/main.c
index 866d57de52..bd0877ecab 100755
--- a/src/main.c
+++ b/src/main.c
@@ -1,7 +1,7 @@
 #include "board.h"
 #include "mw.h"
 
-#include "telemetry/telemetry_common.h"
+#include "telemetry_common.h"
 
 core_t core;
 
diff --git a/src/maths.c b/src/maths.c
new file mode 100644
index 0000000000..b2aa1e251a
--- /dev/null
+++ b/src/maths.c
@@ -0,0 +1,42 @@
+#include <math.h>
+
+#include "maths.h"
+
+int constrain(int amt, int low, int high)
+{
+    if (amt < low)
+        return low;
+    else if (amt > high)
+        return high;
+    else
+        return amt;
+}
+
+void devClear(stdev_t *dev)
+{
+    dev->m_n = 0;
+}
+
+void devPush(stdev_t *dev, float x)
+{
+    dev->m_n++;
+    if (dev->m_n == 1) {
+        dev->m_oldM = dev->m_newM = x;
+        dev->m_oldS = 0.0f;
+    } else {
+        dev->m_newM = dev->m_oldM + (x - dev->m_oldM) / dev->m_n;
+        dev->m_newS = dev->m_oldS + (x - dev->m_oldM) * (x - dev->m_newM);
+        dev->m_oldM = dev->m_newM;
+        dev->m_oldS = dev->m_newS;
+    }
+}
+
+float devVariance(stdev_t *dev)
+{
+    return ((dev->m_n > 1) ? dev->m_newS / (dev->m_n - 1) : 0.0f);
+}
+
+float devStandardDeviation(stdev_t *dev)
+{
+    return sqrtf(devVariance(dev));
+}
diff --git a/src/maths.h b/src/maths.h
new file mode 100644
index 0000000000..5beb013739
--- /dev/null
+++ b/src/maths.h
@@ -0,0 +1,19 @@
+
+#pragma once
+
+#ifndef sq
+#define sq(x) ((x)*(x))
+#endif
+
+typedef struct stdev_t
+{
+    float m_oldM, m_newM, m_oldS, m_newS;
+    int m_n;
+} stdev_t;
+
+int constrain(int amt, int low, int high);
+
+void devClear(stdev_t *dev);
+void devPush(stdev_t *dev, float x);
+float devVariance(stdev_t *dev);
+float devStandardDeviation(stdev_t *dev);
diff --git a/src/mw.c b/src/mw.c
index 731de735cd..f9f08a4f91 100755
--- a/src/mw.c
+++ b/src/mw.c
@@ -1,8 +1,10 @@
 #include "board.h"
 #include "mw.h"
 
-#include "ui/cli.h"
-#include "telemetry/telemetry_common.h"
+#include "serial_cli.h"
+#include "telemetry_common.h"
+#include "typeconversion.h"
+#include "maths.h"
 
 // June 2013     V2.2-dev
 
@@ -64,10 +66,6 @@ uint16_t AccInflightCalibrationMeasurementDone = 0;
 uint16_t AccInflightCalibrationSavetoEEProm = 0;
 uint16_t AccInflightCalibrationActive = 0;
 
-// Battery monitoring stuff
-uint8_t batteryCellCount = 3;       // cell count
-uint16_t batteryWarningVoltage;     // annoying buzzer after this one, battery ready to be dead
-
 void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
 {
     uint8_t i, r;
diff --git a/src/printf.c b/src/printf.c
index d4fa8102b1..0d2e2e637e 100644
--- a/src/printf.c
+++ b/src/printf.c
@@ -31,99 +31,15 @@
 
 #include "board.h"
 #include "mw.h"
+
+#include "typeconversion.h"
+
 #ifdef USE_LAME_PRINTF
-#define PRINTF_LONG_SUPPORT
 
 typedef void (*putcf) (void *, char);
 static putcf stdout_putf;
 static void *stdout_putp;
 
-#ifdef PRINTF_LONG_SUPPORT
-
-static void uli2a(unsigned long int num, unsigned int base, int uc, char *bf)
-{
-    int n = 0;
-    unsigned int d = 1;
-    while (num / d >= base)
-        d *= base;
-    while (d != 0) {
-        int dgt = num / d;
-        num %= d;
-        d /= base;
-        if (n || dgt > 0 || d == 0) {
-            *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10);
-            ++n;
-        }
-    }
-    *bf = 0;
-}
-
-static void li2a(long num, char *bf)
-{
-    if (num < 0) {
-        num = -num;
-        *bf++ = '-';
-    }
-    uli2a(num, 10, 0, bf);
-}
-
-#endif
-
-static void ui2a(unsigned int num, unsigned int base, int uc, char *bf)
-{
-    int n = 0;
-    unsigned int d = 1;
-    while (num / d >= base)
-        d *= base;
-    while (d != 0) {
-        int dgt = num / d;
-        num %= d;
-        d /= base;
-        if (n || dgt > 0 || d == 0) {
-            *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10);
-            ++n;
-        }
-    }
-    *bf = 0;
-}
-
-static void i2a(int num, char *bf)
-{
-    if (num < 0) {
-        num = -num;
-        *bf++ = '-';
-    }
-    ui2a(num, 10, 0, bf);
-}
-
-static int a2d(char ch)
-{
-    if (ch >= '0' && ch <= '9')
-        return ch - '0';
-    else if (ch >= 'a' && ch <= 'f')
-        return ch - 'a' + 10;
-    else if (ch >= 'A' && ch <= 'F')
-        return ch - 'A' + 10;
-    else
-        return -1;
-}
-
-static char a2i(char ch, char **src, int base, int *nump)
-{
-    char *p = *src;
-    int num = 0;
-    int digit;
-    while ((digit = a2d(ch)) >= 0) {
-        if (digit > base)
-            break;
-        num = num * base + digit;
-        ch = *p++;
-    }
-    *src = p;
-    *nump = num;
-    return ch;
-}
-
 static void putchw(void *putp, putcf putf, int n, char z, char *bf)
 {
     char fc = z ? '0' : ' ';
@@ -215,7 +131,6 @@ void tfp_format(void *putp, putcf putf, char *fmt, va_list va)
   abort:;
 }
 
-
 void init_printf(void *putp, void (*putf) (void *, char))
 {
     stdout_putf = putf;
diff --git a/src/rx/sbus.c b/src/rx_sbus.c
similarity index 100%
rename from src/rx/sbus.c
rename to src/rx_sbus.c
diff --git a/src/rx/spektrum.c b/src/rx_spektrum.c
similarity index 100%
rename from src/rx/spektrum.c
rename to src/rx_spektrum.c
diff --git a/src/rx/sumd.c b/src/rx_sumd.c
similarity index 100%
rename from src/rx/sumd.c
rename to src/rx_sumd.c
diff --git a/src/sensors.c b/src/sensors.c
deleted file mode 100755
index ed3bd94c35..0000000000
--- a/src/sensors.c
+++ /dev/null
@@ -1,455 +0,0 @@
-#include "board.h"
-#include "mw.h"
-
-uint16_t calibratingA = 0;      // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
-uint16_t calibratingB = 0;      // baro calibration = get new ground pressure value
-uint16_t calibratingG = 0;
-uint16_t acc_1G = 256;          // this is the 1G measured acceleration.
-int16_t heading, magHold;
-
-extern uint16_t InflightcalibratingA;
-extern int16_t AccInflightCalibrationArmed;
-extern uint16_t AccInflightCalibrationMeasurementDone;
-extern uint16_t AccInflightCalibrationSavetoEEProm;
-extern uint16_t AccInflightCalibrationActive;
-extern uint16_t batteryWarningVoltage;
-extern uint8_t batteryCellCount;
-extern float magneticDeclination;
-
-sensor_t acc;                       // acc access functions
-sensor_t gyro;                      // gyro access functions
-baro_t baro;                        // barometer access functions
-uint8_t accHardware = ACC_DEFAULT;  // which accel chip is used/detected
-
-#ifdef FY90Q
-// FY90Q analog gyro/acc
-void sensorsAutodetect(void)
-{
-    adcSensorInit(&acc, &gyro);
-}
-#else
-// AfroFlight32 i2c sensors
-void sensorsAutodetect(void)
-{
-    int16_t deg, min;
-    drv_adxl345_config_t acc_params;
-    bool haveMpu6k = false;
-
-    // Autodetect gyro hardware. We have MPU3050 or MPU6050.
-    if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale)) {
-        // this filled up  acc.* struct with init values
-        haveMpu6k = true;
-    } else if (l3g4200dDetect(&gyro, mcfg.gyro_lpf)) {
-        // well, we found our gyro
-        ;
-    } else if (!mpu3050Detect(&gyro, mcfg.gyro_lpf)) {
-        // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
-        failureMode(3);
-    }
-
-    // Accelerometer. Fuck it. Let user break shit.
-retry:
-    switch (mcfg.acc_hardware) {
-        case ACC_NONE: // disable ACC
-            sensorsClear(SENSOR_ACC);
-            break;
-        case ACC_DEFAULT: // autodetect
-        case ACC_ADXL345: // ADXL345
-            acc_params.useFifo = false;
-            acc_params.dataRate = 800; // unused currently
-            if (adxl345Detect(&acc_params, &acc))
-                accHardware = ACC_ADXL345;
-            if (mcfg.acc_hardware == ACC_ADXL345)
-                break;
-            ; // fallthrough
-        case ACC_MPU6050: // MPU6050
-            if (haveMpu6k) {
-                mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale); // yes, i'm rerunning it again.  re-fill acc struct
-                accHardware = ACC_MPU6050;
-                if (mcfg.acc_hardware == ACC_MPU6050)
-                    break;
-            }
-            ; // fallthrough
-#ifndef OLIMEXINO
-        case ACC_MMA8452: // MMA8452
-            if (mma8452Detect(&acc)) {
-                accHardware = ACC_MMA8452;
-                if (mcfg.acc_hardware == ACC_MMA8452)
-                    break;
-            }
-            ; // fallthrough
-        case ACC_BMA280: // BMA280
-            if (bma280Detect(&acc)) {
-                accHardware = ACC_BMA280;
-                if (mcfg.acc_hardware == ACC_BMA280)
-                    break;
-            }
-#endif
-    }
-
-    // Found anything? Check if user fucked up or ACC is really missing.
-    if (accHardware == ACC_DEFAULT) {
-        if (mcfg.acc_hardware > ACC_DEFAULT) {
-            // Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present.
-            mcfg.acc_hardware = ACC_DEFAULT;
-            goto retry;
-        } else {
-            // We're really screwed
-            sensorsClear(SENSOR_ACC);
-        }
-    }
-
-#ifdef BARO
-    // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
-    if (!ms5611Detect(&baro)) {
-        // ms5611 disables BMP085, and tries to initialize + check PROM crc. if this works, we have a baro
-        if (!bmp085Detect(&baro)) {
-            // if both failed, we don't have anything
-            sensorsClear(SENSOR_BARO);
-        }
-    }
-#endif
-
-    // Now time to init things, acc first
-    if (sensors(SENSOR_ACC))
-        acc.init(mcfg.acc_align);
-    // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
-    gyro.init(mcfg.gyro_align);
-
-#ifdef MAG
-    if (!hmc5883lDetect(mcfg.mag_align))
-        sensorsClear(SENSOR_MAG);
-#endif
-
-    // calculate magnetic declination
-    deg = cfg.mag_declination / 100;
-    min = cfg.mag_declination % 100;
-    if (sensors(SENSOR_MAG))
-        magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
-    else
-        magneticDeclination = 0.0f;
-}
-#endif
-
-uint16_t batteryAdcToVoltage(uint16_t src)
-{
-    // calculate battery voltage based on ADC reading
-    // result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 4095 = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V
-    return (((src) * 3.3f) / 4095) * mcfg.vbatscale;
-}
-
-void batteryInit(void)
-{
-    uint32_t i;
-    uint32_t voltage = 0;
-
-    // average up some voltage readings
-    for (i = 0; i < 32; i++) {
-        voltage += adcGetChannel(ADC_BATTERY);
-        delay(10);
-    }
-
-    voltage = batteryAdcToVoltage((uint16_t)(voltage / 32));
-
-    // autodetect cell count, going from 2S..6S
-    for (i = 2; i < 6; i++) {
-        if (voltage < i * mcfg.vbatmaxcellvoltage)
-            break;
-    }
-    batteryCellCount = i;
-    batteryWarningVoltage = i * mcfg.vbatmincellvoltage; // 3.3V per cell minimum, configurable in CLI
-}
-
-static void ACC_Common(void)
-{
-    static int32_t a[3];
-    int axis;
-
-    if (calibratingA > 0) {
-        for (axis = 0; axis < 3; axis++) {
-            // Reset a[axis] at start of calibration
-            if (calibratingA == CALIBRATING_ACC_CYCLES)
-                a[axis] = 0;
-            // Sum up CALIBRATING_ACC_CYCLES readings
-            a[axis] += accADC[axis];
-            // Clear global variables for next reading
-            accADC[axis] = 0;
-            mcfg.accZero[axis] = 0;
-        }
-        // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
-        if (calibratingA == 1) {
-            mcfg.accZero[ROLL] = (a[ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
-            mcfg.accZero[PITCH] = (a[PITCH] + (CALIBRATING_ACC_CYCLES / 2))  / CALIBRATING_ACC_CYCLES;
-            mcfg.accZero[YAW] = (a[YAW] + (CALIBRATING_ACC_CYCLES / 2))  / CALIBRATING_ACC_CYCLES - acc_1G;
-            cfg.angleTrim[ROLL] = 0;
-            cfg.angleTrim[PITCH] = 0;
-            writeEEPROM(1, true);      // write accZero in EEPROM
-        }
-        calibratingA--;
-    }
-
-    if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
-        static int32_t b[3];
-        static int16_t accZero_saved[3] = { 0, 0, 0 };
-        static int16_t angleTrim_saved[2] = { 0, 0 };
-        // Saving old zeropoints before measurement
-        if (InflightcalibratingA == 50) {
-            accZero_saved[ROLL] = mcfg.accZero[ROLL];
-            accZero_saved[PITCH] = mcfg.accZero[PITCH];
-            accZero_saved[YAW] = mcfg.accZero[YAW];
-            angleTrim_saved[ROLL] = cfg.angleTrim[ROLL];
-            angleTrim_saved[PITCH] = cfg.angleTrim[PITCH];
-        }
-        if (InflightcalibratingA > 0) {
-            for (axis = 0; axis < 3; axis++) {
-                // Reset a[axis] at start of calibration
-                if (InflightcalibratingA == 50)
-                    b[axis] = 0;
-                // Sum up 50 readings
-                b[axis] += accADC[axis];
-                // Clear global variables for next reading
-                accADC[axis] = 0;
-                mcfg.accZero[axis] = 0;
-            }
-            // all values are measured
-            if (InflightcalibratingA == 1) {
-                AccInflightCalibrationActive = 0;
-                AccInflightCalibrationMeasurementDone = 1;
-                toggleBeep = 2;      // buzzer for indicatiing the end of calibration
-                // recover saved values to maintain current flight behavior until new values are transferred
-                mcfg.accZero[ROLL] = accZero_saved[ROLL];
-                mcfg.accZero[PITCH] = accZero_saved[PITCH];
-                mcfg.accZero[YAW] = accZero_saved[YAW];
-                cfg.angleTrim[ROLL] = angleTrim_saved[ROLL];
-                cfg.angleTrim[PITCH] = angleTrim_saved[PITCH];
-            }
-            InflightcalibratingA--;
-        }
-        // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
-        if (AccInflightCalibrationSavetoEEProm == 1) {      // the copter is landed, disarmed and the combo has been done again
-            AccInflightCalibrationSavetoEEProm = 0;
-            mcfg.accZero[ROLL] = b[ROLL] / 50;
-            mcfg.accZero[PITCH] = b[PITCH] / 50;
-            mcfg.accZero[YAW] = b[YAW] / 50 - acc_1G;    // for nunchuk 200=1G
-            cfg.angleTrim[ROLL] = 0;
-            cfg.angleTrim[PITCH] = 0;
-            writeEEPROM(1, true);          // write accZero in EEPROM
-        }
-    }
-
-    accADC[ROLL] -= mcfg.accZero[ROLL];
-    accADC[PITCH] -= mcfg.accZero[PITCH];
-    accADC[YAW] -= mcfg.accZero[YAW];
-}
-
-void ACC_getADC(void)
-{
-    acc.read(accADC);
-    ACC_Common();
-}
-
-#ifdef BARO
-void Baro_Common(void)
-{
-    static int32_t baroHistTab[BARO_TAB_SIZE_MAX];
-    static int baroHistIdx;
-    int indexplus1;
-
-    indexplus1 = (baroHistIdx + 1);
-    if (indexplus1 == cfg.baro_tab_size)
-        indexplus1 = 0;
-    baroHistTab[baroHistIdx] = baroPressure;
-    baroPressureSum += baroHistTab[baroHistIdx];
-    baroPressureSum -= baroHistTab[indexplus1];
-    baroHistIdx = indexplus1;
-}
-
-
-int Baro_update(void)
-{
-    static uint32_t baroDeadline = 0;
-    static int state = 0;
-
-    if ((int32_t)(currentTime - baroDeadline) < 0)
-        return 0;
-
-    baroDeadline = currentTime;
-
-    if (state) {
-        baro.get_up();
-        baro.start_ut();
-        baroDeadline += baro.ut_delay;
-        baro.calculate(&baroPressure, &baroTemperature);
-        state = 0;
-        return 2;
-    } else {
-        baro.get_ut();
-        baro.start_up();
-        Baro_Common();
-        state = 1;
-        baroDeadline += baro.up_delay;
-        return 1;
-    }
-}
-#endif /* BARO */
-
-typedef struct stdev_t
-{
-    float m_oldM, m_newM, m_oldS, m_newS;
-    int m_n;
-} stdev_t;
-
-static void devClear(stdev_t *dev)
-{
-    dev->m_n = 0;
-}
-
-static void devPush(stdev_t *dev, float x)
-{
-    dev->m_n++;
-    if (dev->m_n == 1) {
-        dev->m_oldM = dev->m_newM = x;
-        dev->m_oldS = 0.0f;
-    } else {
-        dev->m_newM = dev->m_oldM + (x - dev->m_oldM) / dev->m_n;
-        dev->m_newS = dev->m_oldS + (x - dev->m_oldM) * (x - dev->m_newM);
-        dev->m_oldM = dev->m_newM;
-        dev->m_oldS = dev->m_newS;
-    }
-}
-
-static float devVariance(stdev_t *dev)
-{
-    return ((dev->m_n > 1) ? dev->m_newS / (dev->m_n - 1) : 0.0f);
-}
-
-static float devStandardDeviation(stdev_t *dev)
-{
-    return sqrtf(devVariance(dev));
-}
-
-static void GYRO_Common(void)
-{
-    int axis;
-    static int32_t g[3];
-    static stdev_t var[3];
-
-    if (calibratingG > 0) {
-        for (axis = 0; axis < 3; axis++) {
-            // Reset g[axis] at start of calibration
-            if (calibratingG == CALIBRATING_GYRO_CYCLES) {
-                g[axis] = 0;
-                devClear(&var[axis]);
-            }
-            // Sum up 1000 readings
-            g[axis] += gyroADC[axis];
-            devPush(&var[axis], gyroADC[axis]);
-            // Clear global variables for next reading
-            gyroADC[axis] = 0;
-            gyroZero[axis] = 0;
-            if (calibratingG == 1) {
-                float dev = devStandardDeviation(&var[axis]);
-                // check deviation and startover if idiot was moving the model
-                if (mcfg.moron_threshold && dev > mcfg.moron_threshold) {
-                    calibratingG = CALIBRATING_GYRO_CYCLES;
-                    devClear(&var[0]);
-                    devClear(&var[1]);
-                    devClear(&var[2]);
-                    g[0] = g[1] = g[2] = 0;
-                    continue;
-                }
-                gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
-                blinkLED(10, 15, 1);
-            }
-        }
-        calibratingG--;
-    }
-    for (axis = 0; axis < 3; axis++)
-        gyroADC[axis] -= gyroZero[axis];
-}
-
-void Gyro_getADC(void)
-{
-    // range: +/- 8192; +/- 2000 deg/sec
-    gyro.read(gyroADC);
-    GYRO_Common();
-}
-
-#ifdef MAG
-static uint8_t magInit = 0;
-
-void Mag_init(void)
-{
-    // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
-    LED1_ON;
-    hmc5883lInit();
-    LED1_OFF;
-    magInit = 1;
-}
-
-int Mag_getADC(void)
-{
-    static uint32_t t, tCal = 0;
-    static int16_t magZeroTempMin[3];
-    static int16_t magZeroTempMax[3];
-    uint32_t axis;
-
-    if ((int32_t)(currentTime - t) < 0)
-        return 0;                 //each read is spaced by 100ms
-    t = currentTime + 100000;
-
-    // Read mag sensor
-    hmc5883lRead(magADC);
-
-    if (f.CALIBRATE_MAG) {
-        tCal = t;
-        for (axis = 0; axis < 3; axis++) {
-            mcfg.magZero[axis] = 0;
-            magZeroTempMin[axis] = magADC[axis];
-            magZeroTempMax[axis] = magADC[axis];
-        }
-        f.CALIBRATE_MAG = 0;
-    }
-
-    if (magInit) {              // we apply offset only once mag calibration is done
-        magADC[X] -= mcfg.magZero[X];
-        magADC[Y] -= mcfg.magZero[Y];
-        magADC[Z] -= mcfg.magZero[Z];
-    }
-
-    if (tCal != 0) {
-        if ((t - tCal) < 30000000) {    // 30s: you have 30s to turn the multi in all directions
-            LED0_TOGGLE;
-            for (axis = 0; axis < 3; axis++) {
-                if (magADC[axis] < magZeroTempMin[axis])
-                    magZeroTempMin[axis] = magADC[axis];
-                if (magADC[axis] > magZeroTempMax[axis])
-                    magZeroTempMax[axis] = magADC[axis];
-            }
-        } else {
-            tCal = 0;
-            for (axis = 0; axis < 3; axis++)
-                mcfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets
-            writeEEPROM(1, true);
-        }
-    }
-
-    return 1;
-}
-#endif
-
-#ifdef SONAR
-
-void Sonar_init(void)
-{
-    hcsr04_init(sonar_rc78);
-    sensorsSet(SENSOR_SONAR);
-    sonarAlt = 0;
-}
-
-void Sonar_update(void)
-{
-    hcsr04_get_distance(&sonarAlt);
-}
-
-#endif
diff --git a/src/sensors_acceleration.c b/src/sensors_acceleration.c
new file mode 100644
index 0000000000..7f7352d53f
--- /dev/null
+++ b/src/sensors_acceleration.c
@@ -0,0 +1,99 @@
+#include "board.h"
+#include "mw.h"
+
+uint16_t calibratingA = 0;      // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
+
+extern uint16_t InflightcalibratingA;
+extern int16_t AccInflightCalibrationArmed;
+extern uint16_t AccInflightCalibrationMeasurementDone;
+extern uint16_t AccInflightCalibrationSavetoEEProm;
+extern uint16_t AccInflightCalibrationActive;
+
+void ACC_Common(void)
+{
+    static int32_t a[3];
+    int axis;
+
+    if (calibratingA > 0) {
+        for (axis = 0; axis < 3; axis++) {
+            // Reset a[axis] at start of calibration
+            if (calibratingA == CALIBRATING_ACC_CYCLES)
+                a[axis] = 0;
+            // Sum up CALIBRATING_ACC_CYCLES readings
+            a[axis] += accADC[axis];
+            // Clear global variables for next reading
+            accADC[axis] = 0;
+            mcfg.accZero[axis] = 0;
+        }
+        // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
+        if (calibratingA == 1) {
+            mcfg.accZero[ROLL] = (a[ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
+            mcfg.accZero[PITCH] = (a[PITCH] + (CALIBRATING_ACC_CYCLES / 2))  / CALIBRATING_ACC_CYCLES;
+            mcfg.accZero[YAW] = (a[YAW] + (CALIBRATING_ACC_CYCLES / 2))  / CALIBRATING_ACC_CYCLES - acc_1G;
+            cfg.angleTrim[ROLL] = 0;
+            cfg.angleTrim[PITCH] = 0;
+            writeEEPROM(1, true);      // write accZero in EEPROM
+        }
+        calibratingA--;
+    }
+
+    if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
+        static int32_t b[3];
+        static int16_t accZero_saved[3] = { 0, 0, 0 };
+        static int16_t angleTrim_saved[2] = { 0, 0 };
+        // Saving old zeropoints before measurement
+        if (InflightcalibratingA == 50) {
+            accZero_saved[ROLL] = mcfg.accZero[ROLL];
+            accZero_saved[PITCH] = mcfg.accZero[PITCH];
+            accZero_saved[YAW] = mcfg.accZero[YAW];
+            angleTrim_saved[ROLL] = cfg.angleTrim[ROLL];
+            angleTrim_saved[PITCH] = cfg.angleTrim[PITCH];
+        }
+        if (InflightcalibratingA > 0) {
+            for (axis = 0; axis < 3; axis++) {
+                // Reset a[axis] at start of calibration
+                if (InflightcalibratingA == 50)
+                    b[axis] = 0;
+                // Sum up 50 readings
+                b[axis] += accADC[axis];
+                // Clear global variables for next reading
+                accADC[axis] = 0;
+                mcfg.accZero[axis] = 0;
+            }
+            // all values are measured
+            if (InflightcalibratingA == 1) {
+                AccInflightCalibrationActive = 0;
+                AccInflightCalibrationMeasurementDone = 1;
+                toggleBeep = 2;      // buzzer for indicatiing the end of calibration
+                // recover saved values to maintain current flight behavior until new values are transferred
+                mcfg.accZero[ROLL] = accZero_saved[ROLL];
+                mcfg.accZero[PITCH] = accZero_saved[PITCH];
+                mcfg.accZero[YAW] = accZero_saved[YAW];
+                cfg.angleTrim[ROLL] = angleTrim_saved[ROLL];
+                cfg.angleTrim[PITCH] = angleTrim_saved[PITCH];
+            }
+            InflightcalibratingA--;
+        }
+        // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
+        if (AccInflightCalibrationSavetoEEProm == 1) {      // the copter is landed, disarmed and the combo has been done again
+            AccInflightCalibrationSavetoEEProm = 0;
+            mcfg.accZero[ROLL] = b[ROLL] / 50;
+            mcfg.accZero[PITCH] = b[PITCH] / 50;
+            mcfg.accZero[YAW] = b[YAW] / 50 - acc_1G;    // for nunchuk 200=1G
+            cfg.angleTrim[ROLL] = 0;
+            cfg.angleTrim[PITCH] = 0;
+            writeEEPROM(1, true);          // write accZero in EEPROM
+        }
+    }
+
+    accADC[ROLL] -= mcfg.accZero[ROLL];
+    accADC[PITCH] -= mcfg.accZero[PITCH];
+    accADC[YAW] -= mcfg.accZero[YAW];
+}
+
+void ACC_getADC(void)
+{
+    acc.read(accADC);
+    ACC_Common();
+}
+
diff --git a/src/sensors_accelleration.h b/src/sensors_accelleration.h
new file mode 100644
index 0000000000..572158dbfe
--- /dev/null
+++ b/src/sensors_accelleration.h
@@ -0,0 +1,4 @@
+#pragma once
+
+void ACC_Common(void);
+void ACC_getADC(void);
diff --git a/src/sensors_altitude.c b/src/sensors_altitude.c
new file mode 100644
index 0000000000..54a5d61e63
--- /dev/null
+++ b/src/sensors_altitude.c
@@ -0,0 +1,47 @@
+#include "board.h"
+#include "mw.h"
+
+#ifdef BARO
+void Baro_Common(void)
+{
+    static int32_t baroHistTab[BARO_TAB_SIZE_MAX];
+    static int baroHistIdx;
+    int indexplus1;
+
+    indexplus1 = (baroHistIdx + 1);
+    if (indexplus1 == cfg.baro_tab_size)
+        indexplus1 = 0;
+    baroHistTab[baroHistIdx] = baroPressure;
+    baroPressureSum += baroHistTab[baroHistIdx];
+    baroPressureSum -= baroHistTab[indexplus1];
+    baroHistIdx = indexplus1;
+}
+
+
+int Baro_update(void)
+{
+    static uint32_t baroDeadline = 0;
+    static int state = 0;
+
+    if ((int32_t)(currentTime - baroDeadline) < 0)
+        return 0;
+
+    baroDeadline = currentTime;
+
+    if (state) {
+        baro.get_up();
+        baro.start_ut();
+        baroDeadline += baro.ut_delay;
+        baro.calculate(&baroPressure, &baroTemperature);
+        state = 0;
+        return 2;
+    } else {
+        baro.get_ut();
+        baro.start_up();
+        Baro_Common();
+        state = 1;
+        baroDeadline += baro.up_delay;
+        return 1;
+    }
+}
+#endif /* BARO */
diff --git a/src/sensors_altitude.h b/src/sensors_altitude.h
new file mode 100644
index 0000000000..45ae752547
--- /dev/null
+++ b/src/sensors_altitude.h
@@ -0,0 +1,6 @@
+#pragma once
+
+#ifdef BARO
+void Baro_Common(void);
+int Baro_update(void);
+#endif
diff --git a/src/sensors_compass.c b/src/sensors_compass.c
new file mode 100644
index 0000000000..9c39ec81bb
--- /dev/null
+++ b/src/sensors_compass.c
@@ -0,0 +1,65 @@
+#include "board.h"
+#include "mw.h"
+
+#ifdef MAG
+static uint8_t magInit = 0;
+
+void Mag_init(void)
+{
+    // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
+    LED1_ON;
+    hmc5883lInit();
+    LED1_OFF;
+    magInit = 1;
+}
+
+int Mag_getADC(void)
+{
+    static uint32_t t, tCal = 0;
+    static int16_t magZeroTempMin[3];
+    static int16_t magZeroTempMax[3];
+    uint32_t axis;
+
+    if ((int32_t)(currentTime - t) < 0)
+        return 0;                 //each read is spaced by 100ms
+    t = currentTime + 100000;
+
+    // Read mag sensor
+    hmc5883lRead(magADC);
+
+    if (f.CALIBRATE_MAG) {
+        tCal = t;
+        for (axis = 0; axis < 3; axis++) {
+            mcfg.magZero[axis] = 0;
+            magZeroTempMin[axis] = magADC[axis];
+            magZeroTempMax[axis] = magADC[axis];
+        }
+        f.CALIBRATE_MAG = 0;
+    }
+
+    if (magInit) {              // we apply offset only once mag calibration is done
+        magADC[X] -= mcfg.magZero[X];
+        magADC[Y] -= mcfg.magZero[Y];
+        magADC[Z] -= mcfg.magZero[Z];
+    }
+
+    if (tCal != 0) {
+        if ((t - tCal) < 30000000) {    // 30s: you have 30s to turn the multi in all directions
+            LED0_TOGGLE;
+            for (axis = 0; axis < 3; axis++) {
+                if (magADC[axis] < magZeroTempMin[axis])
+                    magZeroTempMin[axis] = magADC[axis];
+                if (magADC[axis] > magZeroTempMax[axis])
+                    magZeroTempMax[axis] = magADC[axis];
+            }
+        } else {
+            tCal = 0;
+            for (axis = 0; axis < 3; axis++)
+                mcfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets
+            writeEEPROM(1, true);
+        }
+    }
+
+    return 1;
+}
+#endif
diff --git a/src/sensors_compass.h b/src/sensors_compass.h
new file mode 100644
index 0000000000..a673bc7798
--- /dev/null
+++ b/src/sensors_compass.h
@@ -0,0 +1,6 @@
+#pragma once
+
+#ifdef MAG
+void Mag_init(void);
+int Mag_getADC(void);
+#endif
diff --git a/src/sensors_gyro.c b/src/sensors_gyro.c
new file mode 100644
index 0000000000..fecc24b80c
--- /dev/null
+++ b/src/sensors_gyro.c
@@ -0,0 +1,51 @@
+#include "board.h"
+#include "mw.h"
+
+#include "maths.h"
+
+void GYRO_Common(void)
+{
+    int axis;
+    static int32_t g[3];
+    static stdev_t var[3];
+
+    if (calibratingG > 0) {
+        for (axis = 0; axis < 3; axis++) {
+            // Reset g[axis] at start of calibration
+            if (calibratingG == CALIBRATING_GYRO_CYCLES) {
+                g[axis] = 0;
+                devClear(&var[axis]);
+            }
+            // Sum up 1000 readings
+            g[axis] += gyroADC[axis];
+            devPush(&var[axis], gyroADC[axis]);
+            // Clear global variables for next reading
+            gyroADC[axis] = 0;
+            gyroZero[axis] = 0;
+            if (calibratingG == 1) {
+                float dev = devStandardDeviation(&var[axis]);
+                // check deviation and startover if idiot was moving the model
+                if (mcfg.moron_threshold && dev > mcfg.moron_threshold) {
+                    calibratingG = CALIBRATING_GYRO_CYCLES;
+                    devClear(&var[0]);
+                    devClear(&var[1]);
+                    devClear(&var[2]);
+                    g[0] = g[1] = g[2] = 0;
+                    continue;
+                }
+                gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
+                blinkLED(10, 15, 1);
+            }
+        }
+        calibratingG--;
+    }
+    for (axis = 0; axis < 3; axis++)
+        gyroADC[axis] -= gyroZero[axis];
+}
+
+void Gyro_getADC(void)
+{
+    // range: +/- 8192; +/- 2000 deg/sec
+    gyro.read(gyroADC);
+    GYRO_Common();
+}
diff --git a/src/sensors_gyro.h b/src/sensors_gyro.h
new file mode 100644
index 0000000000..0aa443d2f1
--- /dev/null
+++ b/src/sensors_gyro.h
@@ -0,0 +1,4 @@
+#pragma once
+
+void GYRO_Common(void);
+void Gyro_getADC(void);
diff --git a/src/sensors_initialisation.c b/src/sensors_initialisation.c
new file mode 100755
index 0000000000..eec0232aa2
--- /dev/null
+++ b/src/sensors_initialisation.c
@@ -0,0 +1,127 @@
+#include "board.h"
+#include "mw.h"
+
+uint16_t calibratingB = 0;      // baro calibration = get new ground pressure value
+uint16_t calibratingG = 0;
+uint16_t acc_1G = 256;          // this is the 1G measured acceleration.
+int16_t heading, magHold;
+
+extern uint16_t batteryWarningVoltage;
+extern uint8_t batteryCellCount;
+extern float magneticDeclination;
+
+sensor_t acc;                       // acc access functions
+sensor_t gyro;                      // gyro access functions
+baro_t baro;                        // barometer access functions
+uint8_t accHardware = ACC_DEFAULT;  // which accel chip is used/detected
+
+#ifdef FY90Q
+// FY90Q analog gyro/acc
+void sensorsAutodetect(void)
+{
+    adcSensorInit(&acc, &gyro);
+}
+#else
+// AfroFlight32 i2c sensors
+void sensorsAutodetect(void)
+{
+    int16_t deg, min;
+    drv_adxl345_config_t acc_params;
+    bool haveMpu6k = false;
+
+    // Autodetect gyro hardware. We have MPU3050 or MPU6050.
+    if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale)) {
+        // this filled up  acc.* struct with init values
+        haveMpu6k = true;
+    } else if (l3g4200dDetect(&gyro, mcfg.gyro_lpf)) {
+        // well, we found our gyro
+        ;
+    } else if (!mpu3050Detect(&gyro, mcfg.gyro_lpf)) {
+        // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
+        failureMode(3);
+    }
+
+    // Accelerometer. Fuck it. Let user break shit.
+retry:
+    switch (mcfg.acc_hardware) {
+        case ACC_NONE: // disable ACC
+            sensorsClear(SENSOR_ACC);
+            break;
+        case ACC_DEFAULT: // autodetect
+        case ACC_ADXL345: // ADXL345
+            acc_params.useFifo = false;
+            acc_params.dataRate = 800; // unused currently
+            if (adxl345Detect(&acc_params, &acc))
+                accHardware = ACC_ADXL345;
+            if (mcfg.acc_hardware == ACC_ADXL345)
+                break;
+            ; // fallthrough
+        case ACC_MPU6050: // MPU6050
+            if (haveMpu6k) {
+                mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale); // yes, i'm rerunning it again.  re-fill acc struct
+                accHardware = ACC_MPU6050;
+                if (mcfg.acc_hardware == ACC_MPU6050)
+                    break;
+            }
+            ; // fallthrough
+#ifndef OLIMEXINO
+        case ACC_MMA8452: // MMA8452
+            if (mma8452Detect(&acc)) {
+                accHardware = ACC_MMA8452;
+                if (mcfg.acc_hardware == ACC_MMA8452)
+                    break;
+            }
+            ; // fallthrough
+        case ACC_BMA280: // BMA280
+            if (bma280Detect(&acc)) {
+                accHardware = ACC_BMA280;
+                if (mcfg.acc_hardware == ACC_BMA280)
+                    break;
+            }
+#endif
+    }
+
+    // Found anything? Check if user fucked up or ACC is really missing.
+    if (accHardware == ACC_DEFAULT) {
+        if (mcfg.acc_hardware > ACC_DEFAULT) {
+            // Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present.
+            mcfg.acc_hardware = ACC_DEFAULT;
+            goto retry;
+        } else {
+            // We're really screwed
+            sensorsClear(SENSOR_ACC);
+        }
+    }
+
+#ifdef BARO
+    // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
+    if (!ms5611Detect(&baro)) {
+        // ms5611 disables BMP085, and tries to initialize + check PROM crc. if this works, we have a baro
+        if (!bmp085Detect(&baro)) {
+            // if both failed, we don't have anything
+            sensorsClear(SENSOR_BARO);
+        }
+    }
+#endif
+
+    // Now time to init things, acc first
+    if (sensors(SENSOR_ACC))
+        acc.init(mcfg.acc_align);
+    // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
+    gyro.init(mcfg.gyro_align);
+
+#ifdef MAG
+    if (!hmc5883lDetect(mcfg.mag_align))
+        sensorsClear(SENSOR_MAG);
+#endif
+
+    // calculate magnetic declination
+    deg = cfg.mag_declination / 100;
+    min = cfg.mag_declination % 100;
+    if (sensors(SENSOR_MAG))
+        magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
+    else
+        magneticDeclination = 0.0f;
+}
+#endif
+
diff --git a/src/sensors_sonar.c b/src/sensors_sonar.c
new file mode 100644
index 0000000000..fbee59e1f1
--- /dev/null
+++ b/src/sensors_sonar.c
@@ -0,0 +1,18 @@
+#include "board.h"
+#include "mw.h"
+
+#ifdef SONAR
+
+void Sonar_init(void)
+{
+    hcsr04_init(sonar_rc78);
+    sensorsSet(SENSOR_SONAR);
+    sonarAlt = 0;
+}
+
+void Sonar_update(void)
+{
+    hcsr04_get_distance(&sonarAlt);
+}
+
+#endif
diff --git a/src/ui/cli.c b/src/serial_cli.c
similarity index 100%
rename from src/ui/cli.c
rename to src/serial_cli.c
diff --git a/src/ui/cli.h b/src/serial_cli.h
similarity index 100%
rename from src/ui/cli.h
rename to src/serial_cli.h
diff --git a/src/ui/serial.c b/src/serial_msp.c
similarity index 97%
rename from src/ui/serial.c
rename to src/serial_msp.c
index 41ae2a5f4b..d9b090f0ce 100755
--- a/src/ui/serial.c
+++ b/src/serial_msp.c
@@ -1,8 +1,8 @@
 #include "board.h"
 #include "mw.h"
 
-#include "ui/cli.h"
-#include "telemetry/telemetry_common.h"
+#include "serial_cli.h"
+#include "telemetry_common.h"
 
 // Multiwii Serial Protocol 0
 #define MSP_VERSION              0
diff --git a/src/telemetry/telemetry_common.c b/src/telemetry_common.c
similarity index 100%
rename from src/telemetry/telemetry_common.c
rename to src/telemetry_common.c
diff --git a/src/telemetry/telemetry_common.h b/src/telemetry_common.h
similarity index 100%
rename from src/telemetry/telemetry_common.h
rename to src/telemetry_common.h
diff --git a/src/telemetry/telemetry_frsky.c b/src/telemetry_frsky.c
similarity index 100%
rename from src/telemetry/telemetry_frsky.c
rename to src/telemetry_frsky.c
diff --git a/src/telemetry/telemetry_frsky.h b/src/telemetry_frsky.h
similarity index 100%
rename from src/telemetry/telemetry_frsky.h
rename to src/telemetry_frsky.h
diff --git a/src/telemetry/telemetry_hott.c b/src/telemetry_hott.c
similarity index 100%
rename from src/telemetry/telemetry_hott.c
rename to src/telemetry_hott.c
diff --git a/src/telemetry/telemetry_hott.h b/src/telemetry_hott.h
similarity index 100%
rename from src/telemetry/telemetry_hott.h
rename to src/telemetry_hott.h
diff --git a/src/typeconversion.c b/src/typeconversion.c
new file mode 100644
index 0000000000..26ebfb84a7
--- /dev/null
+++ b/src/typeconversion.c
@@ -0,0 +1,88 @@
+#include "board.h"
+#include "mw.h"
+
+#ifdef PRINTF_LONG_SUPPORT
+
+void uli2a(unsigned long int num, unsigned int base, int uc, char *bf)
+{
+    int n = 0;
+    unsigned int d = 1;
+    while (num / d >= base)
+        d *= base;
+    while (d != 0) {
+        int dgt = num / d;
+        num %= d;
+        d /= base;
+        if (n || dgt > 0 || d == 0) {
+            *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10);
+            ++n;
+        }
+    }
+    *bf = 0;
+}
+
+void li2a(long num, char *bf)
+{
+    if (num < 0) {
+        num = -num;
+        *bf++ = '-';
+    }
+    uli2a(num, 10, 0, bf);
+}
+
+#endif
+
+void ui2a(unsigned int num, unsigned int base, int uc, char *bf)
+{
+    int n = 0;
+    unsigned int d = 1;
+    while (num / d >= base)
+        d *= base;
+    while (d != 0) {
+        int dgt = num / d;
+        num %= d;
+        d /= base;
+        if (n || dgt > 0 || d == 0) {
+            *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10);
+            ++n;
+        }
+    }
+    *bf = 0;
+}
+
+void i2a(int num, char *bf)
+{
+    if (num < 0) {
+        num = -num;
+        *bf++ = '-';
+    }
+    ui2a(num, 10, 0, bf);
+}
+
+int a2d(char ch)
+{
+    if (ch >= '0' && ch <= '9')
+        return ch - '0';
+    else if (ch >= 'a' && ch <= 'f')
+        return ch - 'a' + 10;
+    else if (ch >= 'A' && ch <= 'F')
+        return ch - 'A' + 10;
+    else
+        return -1;
+}
+
+char a2i(char ch, char **src, int base, int *nump)
+{
+    char *p = *src;
+    int num = 0;
+    int digit;
+    while ((digit = a2d(ch)) >= 0) {
+        if (digit > base)
+            break;
+        num = num * base + digit;
+        ch = *p++;
+    }
+    *src = p;
+    *nump = num;
+    return ch;
+}
diff --git a/src/typeconversion.h b/src/typeconversion.h
new file mode 100644
index 0000000000..2fff2d89c5
--- /dev/null
+++ b/src/typeconversion.h
@@ -0,0 +1,7 @@
+#pragma once
+
+void uli2a(unsigned long int num, unsigned int base, int uc, char *bf);
+void li2a(long num, char *bf);
+void ui2a(unsigned int num, unsigned int base, int uc, char *bf);
+void i2a(int num, char *bf);
+char a2i(char ch, char **src, int base, int *nump);