diff --git a/lib/Espfc/src/Sensor/BaroSensor.cpp b/lib/Espfc/src/Sensor/BaroSensor.cpp
new file mode 100644
index 0000000..cba071b
--- /dev/null
+++ b/lib/Espfc/src/Sensor/BaroSensor.cpp
@@ -0,0 +1,125 @@
+#include "BaroSensor.h"
+
+namespace Espfc {
+
+namespace Sensor {
+
+BaroSensor::BaroSensor(Model& model): _model(model), _state(BARO_STATE_INIT), _counter(0) {}
+
+int BaroSensor::begin()
+{
+  _model.state.baro.rate = 0;
+  if(!_model.baroActive()) return 0;
+  _baro = _model.state.baro.dev;
+  if(!_baro) return 0;
+
+  _baro->setMode(BARO_MODE_TEMP);
+  int delay = _baro->getDelay(BARO_MODE_TEMP) + _baro->getDelay(BARO_MODE_PRESS);
+  int toGyroRate = (delay / _model.state.gyro.timer.interval) + 1; // number of gyro readings per cycle
+  int interval = _model.state.gyro.timer.interval * toGyroRate;
+  _model.state.baro.rate = 1000000 / interval;
+
+  _model.state.baro.altitudeBiasSamples = 2 * _model.state.baro.rate;
+
+  // TODO: move filters to BaroState
+  _temperatureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baro.rate * 0.1f), _model.state.baro.rate);
+  _pressureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baro.rate * 0.1f), _model.state.baro.rate);
+  _altitudeFilter.begin(_model.config.baro.filter, _model.state.baro.rate);
+
+  _model.logger.info().log(F("BARO INIT")).log(FPSTR(Device::BaroDevice::getName(_baro->getType()))).log(_baro->getAddress()).log(toGyroRate).log(_model.state.baro.rate).logln(_model.config.baro.filter.freq);
+
+  return 1;
+}
+
+int BaroSensor::update()
+{
+  int status = read();
+
+  return status;
+}
+
+int BaroSensor::read()
+{
+  if(!_baro || !_model.baroActive()) return 0;
+
+  if(_wait > micros()) return 0;
+
+  Stats::Measure measure(_model.state.stats, COUNTER_BARO);
+
+  if(_model.config.debug.mode == DEBUG_BARO)
+  {
+    _model.state.debug[0] = _state;
+  }
+
+  switch(_state)
+  {
+    case BARO_STATE_INIT:
+      _baro->setMode(BARO_MODE_TEMP);
+      _state = BARO_STATE_TEMP_GET;
+      _wait = micros() + _baro->getDelay(BARO_MODE_TEMP);
+      return 0;
+    case BARO_STATE_TEMP_GET:
+      readTemperature();
+      _baro->setMode(BARO_MODE_PRESS);
+      _state = BARO_STATE_PRESS_GET;
+      _wait = micros() + _baro->getDelay(BARO_MODE_PRESS);
+      _counter = 1;
+      return 1;
+    case BARO_STATE_PRESS_GET:
+      readPressure();
+      updateAltitude();
+      if(--_counter > 0)
+      {
+        _baro->setMode(BARO_MODE_PRESS);
+        _state = BARO_STATE_PRESS_GET;
+        _wait = micros() + _baro->getDelay(BARO_MODE_PRESS);
+      }
+      else
+      {
+        _baro->setMode(BARO_MODE_TEMP);
+        _state = BARO_STATE_TEMP_GET;
+        _wait = micros() + _baro->getDelay(BARO_MODE_TEMP);
+      }
+      return 1;
+      break;
+    default:
+      _state = BARO_STATE_INIT;
+      break;
+  }
+
+  return 0;
+}
+
+void BaroSensor::readTemperature()
+{
+  _model.state.baro.temperatureRaw = _baro->readTemperature();
+  _model.state.baro.temperature = _temperatureFilter.update(_model.state.baro.temperatureRaw);
+}
+
+void BaroSensor::readPressure()
+{
+  _model.state.baro.pressureRaw = _baro->readPressure();
+  _model.state.baro.pressure = _pressureFilter.update(_model.state.baro.pressureRaw);
+}
+
+void BaroSensor::updateAltitude()
+{
+  _model.state.baro.altitudeRaw = _altitudeFilter.update(Math::toAltitude(_model.state.baro.pressure));
+  if(_model.state.baro.altitudeBiasSamples > 0)
+  {
+    _model.state.baro.altitudeBiasSamples--;
+    _model.state.baro.altitudeBias += (_model.state.baro.altitudeRaw - _model.state.baro.altitudeBias) * 0.2f;
+  }
+  _model.state.baro.altitude = _model.state.baro.altitudeRaw - _model.state.baro.altitudeBias;
+
+  if(_model.config.debug.mode == DEBUG_BARO)
+  {
+    _model.state.debug[1] = lrintf(_model.state.baro.pressureRaw * 0.1f); // hPa x 10
+    _model.state.debug[2] = lrintf(_model.state.baro.temperatureRaw * 100.f); // deg C x 100
+    _model.state.debug[3] = lrintf(_model.state.baro.altitudeRaw * 10.f);
+  }
+}
+
+}
+
+}
diff --git a/lib/Espfc/src/Sensor/BaroSensor.h b/lib/Espfc/src/Sensor/BaroSensor.h
index dbeda6d..d5f8201 100644
--- a/lib/Espfc/src/Sensor/BaroSensor.h
+++ b/lib/Espfc/src/Sensor/BaroSensor.h
@@ -2,6 +2,7 @@
 #define _ESPFC_SENSOR_BARO_SENSOR_H_
 
 #include "BaseSensor.h"
+#include "Model.h"
 #include "Device/BaroDevice.h"
 #include "Filter.h"
 
@@ -19,122 +20,16 @@ class BaroSensor: public BaseSensor
       BARO_STATE_PRESS_GET,
     };
 
-    BaroSensor(Model& model): _model(model), _state(BARO_STATE_INIT), _counter(0) {}
+    BaroSensor(Model& model);
 
-    int begin()
-    {
-      _model.state.baro.rate = 0;
-      if(!_model.baroActive()) return 0;
-      _baro = _model.state.baro.dev;
-      if(!_baro) return 0;
-
-      _baro->setMode(BARO_MODE_TEMP);
-      int delay = _baro->getDelay(BARO_MODE_TEMP) + _baro->getDelay(BARO_MODE_PRESS);
-      int toGyroRate = (delay / _model.state.gyro.timer.interval) + 1; // number of gyro readings per cycle
-      int interval = _model.state.gyro.timer.interval * toGyroRate;
-      _model.state.baro.rate = 1000000 / interval;
-
-      _model.state.baro.altitudeBiasSamples = 2 * _model.state.baro.rate;
-
-      // TODO: move filters to BaroState
-      _temperatureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baro.rate * 0.1f), _model.state.baro.rate);
-      _pressureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baro.rate * 0.1f), _model.state.baro.rate);
-      _altitudeFilter.begin(_model.config.baro.filter, _model.state.baro.rate);
-
-      _model.logger.info().log(F("BARO INIT")).log(FPSTR(Device::BaroDevice::getName(_baro->getType()))).log(_baro->getAddress()).log(toGyroRate).log(_model.state.baro.rate).logln(_model.config.baro.filter.freq);
-
-      return 1;
-    }
-
-    int update()
-    {
-      int status = read();
-
-      return status;
-    }
-
-    int read()
-    {
-      if(!_baro || !_model.baroActive()) return 0;
-      
-      if(_wait > micros()) return 0;
-
-      Stats::Measure measure(_model.state.stats, COUNTER_BARO);
-
-      if(_model.config.debug.mode == DEBUG_BARO)
-      {
-        _model.state.debug[0] = _state;
-      }
-
-      switch(_state)
-      {
-        case BARO_STATE_INIT:
-          _baro->setMode(BARO_MODE_TEMP);
-          _state = BARO_STATE_TEMP_GET;
-          _wait = micros() + _baro->getDelay(BARO_MODE_TEMP);
-          return 0;
-        case BARO_STATE_TEMP_GET:
-          readTemperature();
-          _baro->setMode(BARO_MODE_PRESS);
-          _state = BARO_STATE_PRESS_GET;
-          _wait = micros() + _baro->getDelay(BARO_MODE_PRESS);
-          _counter = 1;
-          return 1;
-        case BARO_STATE_PRESS_GET:
-          readPressure();
-          updateAltitude();
-          if(--_counter > 0)
-          {
-            _baro->setMode(BARO_MODE_PRESS);
-            _state = BARO_STATE_PRESS_GET;
-            _wait = micros() + _baro->getDelay(BARO_MODE_PRESS);
-          }
-          else
-          {
-            _baro->setMode(BARO_MODE_TEMP);
-            _state = BARO_STATE_TEMP_GET;
-            _wait = micros() + _baro->getDelay(BARO_MODE_TEMP);
-          }
-          return 1;
-          break;
-        default:
-          _state = BARO_STATE_INIT;
-          break;
-      }
-
-      return 0;
-    }
+    int begin();
+    int update();
+    int read();
 
   private:
-    void readTemperature()
-    {
-      _model.state.baro.temperatureRaw = _baro->readTemperature();
-      _model.state.baro.temperature = _temperatureFilter.update(_model.state.baro.temperatureRaw);
-    }
-
-    void readPressure()
-    {
-      _model.state.baro.pressureRaw = _baro->readPressure();
-      _model.state.baro.pressure = _pressureFilter.update(_model.state.baro.pressureRaw);
-    }
-
-    void updateAltitude()
-    {
-      _model.state.baro.altitudeRaw = _altitudeFilter.update(Math::toAltitude(_model.state.baro.pressure));
-      if(_model.state.baro.altitudeBiasSamples > 0)
-      {
-        _model.state.baro.altitudeBiasSamples--;
-        _model.state.baro.altitudeBias += (_model.state.baro.altitudeRaw - _model.state.baro.altitudeBias) * 0.2f;
-      }
-      _model.state.baro.altitude = _model.state.baro.altitudeRaw - _model.state.baro.altitudeBias;
-
-      if(_model.config.debug.mode == DEBUG_BARO)
-      {
-        _model.state.debug[1] = lrintf(_model.state.baro.pressureRaw * 0.1f); // hPa x 10
-        _model.state.debug[2] = lrintf(_model.state.baro.temperatureRaw * 100.f); // deg C x 100
-        _model.state.debug[3] = lrintf(_model.state.baro.altitudeRaw * 10.f);
-      }
-    }
+    void readTemperature();
+    void readPressure();
+    void updateAltitude();
 
     Model& _model;
     Device::BaroDevice * _baro;
diff --git a/lib/Espfc/src/Sensor/MagSensor.cpp b/lib/Espfc/src/Sensor/MagSensor.cpp
new file mode 100644
index 0000000..f98cf63
--- /dev/null
+++ b/lib/Espfc/src/Sensor/MagSensor.cpp
@@ -0,0 +1,163 @@
+#include "MagSensor.h"
+#include <cmath>
+
+#ifdef abs
+#undef abs
+#endif
+
+namespace Espfc {
+
+namespace Sensor {
+
+MagSensor::MagSensor(Model& model): _model(model) {}
+
+int MagSensor::begin()
+{
+  if(!_model.magActive()) return 0;
+
+  _mag = _model.state.mag.dev;
+  if(!_mag) return 0;
+
+  if(_model.state.mag.timer.rate < 5) return 0;
+
+  // by default use eeprom calibration settings
+  _model.state.mag.calibrationState = CALIBRATION_IDLE;
+  _model.state.mag.calibrationValid = true;
+
+  _model.logger.info().log(F("MAG INIT")).log(FPSTR(Device::MagDevice::getName(_mag->getType()))).log(_mag->getAddress()).logln(_model.state.mag.timer.rate);
+
+  return 1;
+}
+
+int MagSensor::update()
+{
+  int status = read();
+
+  if (status) filter();
+
+  return status;
+}
+
+int MagSensor::read()
+{
+  if(!_mag || !_model.magActive() || !_model.state.mag.timer.check()) return 0;
+
+  Stats::Measure measure(_model.state.stats, COUNTER_MAG_READ);
+  _mag->readMag(_model.state.mag.raw);
+
+  return 1;
+}
+
+int MagSensor::filter()
+{
+  if(!_mag || !_model.magActive()) return 0;
+
+  Stats::Measure measure(_model.state.stats, COUNTER_MAG_FILTER);
+
+  _model.state.mag.adc = _mag->convert(_model.state.mag.raw);
+
+  align(_model.state.mag.adc, _model.config.mag.align);
+  _model.state.mag.adc = _model.state.boardAlignment.apply(_model.state.mag.adc);
+
+  for(size_t i = 0; i < AXIS_COUNT_RPY; i++)
+  {
+    _model.state.mag.adc.set(i, _model.state.mag.filter[i].update(_model.state.mag.adc[i]));
+  }
+
+  calibrate();
+
+  return 1;
+}
+
+void MagSensor::calibrate()
+{
+  switch(_model.state.mag.calibrationState)
+  {
+    case CALIBRATION_IDLE:
+      if(_model.state.mag.calibrationValid)
+      {
+        _model.state.mag.adc -= _model.state.mag.calibrationOffset;
+        _model.state.mag.adc *= _model.state.mag.calibrationScale;
+      }
+      break;
+    case CALIBRATION_START:
+      resetCalibration();
+      _model.state.mag.calibrationSamples = 30 * _model.state.mag.timer.rate;
+      _model.state.mag.calibrationState = CALIBRATION_UPDATE;
+      break;
+    case CALIBRATION_UPDATE:
+      updateCalibration();
+      _model.state.mag.calibrationSamples--;
+      if(_model.state.mag.calibrationSamples <= 0) _model.state.mag.calibrationState = CALIBRATION_APPLY;
+      break;
+    case CALIBRATION_APPLY:
+      applyCalibration();
+      _model.state.mag.calibrationState = CALIBRATION_SAVE;
+      break;
+    case CALIBRATION_SAVE:
+      _model.finishCalibration();
+      _model.state.mag.calibrationState = CALIBRATION_IDLE;
+      break;
+    default:
+      _model.state.mag.calibrationState = CALIBRATION_IDLE;
+      break;
+  }
+}
+
+void MagSensor::resetCalibration()
+{
+  _model.state.mag.calibrationMin = VectorFloat();
+  _model.state.mag.calibrationMax = VectorFloat();
+  _model.state.mag.calibrationValid = false;
+}
+
+void MagSensor::updateCalibration()
+{
+  for(int i = 0; i < AXIS_COUNT_RPY; i++)
+  {
+    if(_model.state.mag.adc[i] < _model.state.mag.calibrationMin[i]) _model.state.mag.calibrationMin.set(i, _model.state.mag.adc[i]);
+    if(_model.state.mag.adc[i] > _model.state.mag.calibrationMax[i]) _model.state.mag.calibrationMax.set(i, _model.state.mag.adc[i]);
+  }
+}
+
+void MagSensor::applyCalibration()
+{
+  const float EPSILON = 0.001f;
+
+  // verify calibration data and find biggest range
+  float maxRange = -1;
+  for(int i = 0; i < AXIS_COUNT_RPY; i++)
+  {
+    if(_model.state.mag.calibrationMin[i] > -EPSILON) return;
+    if(_model.state.mag.calibrationMax[i] <  EPSILON) return;
+    if((_model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]) > maxRange)
+    {
+      maxRange = _model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i];
+    }
+  }
+
+  // probably incomplete data, must be positive
+  if(maxRange <= 0) return;
+
+  VectorFloat scale(1.f, 1.f, 1.f);
+  VectorFloat offset(0.f, 0.f, 0.f);
+
+  for (int i = 0; i < AXIS_COUNT_RPY; i++)
+  {
+    const float range = (_model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]);
+    const float bias  = (_model.state.mag.calibrationMax[i] + _model.state.mag.calibrationMin[i]) * 0.5f;
+
+    if(std::abs(range) < EPSILON) return;    // incomplete data
+
+    scale.set(i, maxRange / range);     // makes everything the same range
+    offset.set(i, bias);                // level bias
+  }
+
+  _model.state.mag.calibrationScale = scale;
+  _model.state.mag.calibrationOffset = offset;
+  _model.state.mag.calibrationValid = true;
+}
+
+}
+
+}
diff --git a/lib/Espfc/src/Sensor/MagSensor.h b/lib/Espfc/src/Sensor/MagSensor.h
index 01f00c7..0c42417 100644
--- a/lib/Espfc/src/Sensor/MagSensor.h
+++ b/lib/Espfc/src/Sensor/MagSensor.h
@@ -1,11 +1,9 @@
-#ifndef _ESPFC_SENSOR_MAG_SENSOR_H_
-#define _ESPFC_SENSOR_MAG_SENSOR_H_
+#pragma once
 
+#include "Model.h"
 #include "BaseSensor.h"
 #include "Device/MagDevice.h"
 
-#include <math.h>
-
 namespace Espfc {
 
 namespace Sensor {
@@ -13,156 +11,18 @@ namespace Sensor {
 class MagSensor: public BaseSensor
 {
   public:
-    MagSensor(Model& model): _model(model) {}
-
-    int begin()
-    {
-      if(!_model.magActive()) return 0;
-
-      _mag = _model.state.mag.dev;
-      if(!_mag) return 0;
-
-      if(_model.state.mag.timer.rate < 5) return 0;
-
-      // by default use eeprom calibration settings
-      _model.state.mag.calibrationState = CALIBRATION_IDLE;
-      _model.state.mag.calibrationValid = true;
-
-      _model.logger.info().log(F("MAG INIT")).log(FPSTR(Device::MagDevice::getName(_mag->getType()))).log(_mag->getAddress()).logln(_model.state.mag.timer.rate);
-      
-      return 1;
-    }
-
-    int update()
-    {
-      int status = read();
-
-      if (status) filter();
-
-      return status;
-    }
-
-    int read()
-    {
-      if(!_mag || !_model.magActive() || !_model.state.mag.timer.check()) return 0;
-
-      Stats::Measure measure(_model.state.stats, COUNTER_MAG_READ);
-      _mag->readMag(_model.state.mag.raw);
-
-      return 1;
-    }
-
-    int filter()
-    {
-      if(!_mag || !_model.magActive()) return 0;
-
-      Stats::Measure measure(_model.state.stats, COUNTER_MAG_FILTER);
+    MagSensor(Model& model);
 
-      _model.state.mag.adc = _mag->convert(_model.state.mag.raw);
-
-
-      align(_model.state.mag.adc, _model.config.mag.align);
-      _model.state.mag.adc = _model.state.boardAlignment.apply(_model.state.mag.adc);
-
-      for(size_t i = 0; i < AXIS_COUNT_RPY; i++)
-      {
-        _model.state.mag.adc.set(i, _model.state.mag.filter[i].update(_model.state.mag.adc[i]));
-      }
-
-      calibrate();
-
-      return 1;
-    }
+    int begin();
+    int update();
+    int read();
+    int filter();
 
   private:
-    void calibrate()
-    {
-      switch(_model.state.mag.calibrationState)
-      {
-        case CALIBRATION_IDLE:
-          if(_model.state.mag.calibrationValid)
-          {
-            _model.state.mag.adc -= _model.state.mag.calibrationOffset;
-            _model.state.mag.adc *= _model.state.mag.calibrationScale;
-          }
-          break;
-        case CALIBRATION_START:
-          resetCalibration();
-          _model.state.mag.calibrationSamples = 30 * _model.state.mag.timer.rate;
-          _model.state.mag.calibrationState = CALIBRATION_UPDATE;
-          break;
-        case CALIBRATION_UPDATE:
-          updateCalibration();
-          _model.state.mag.calibrationSamples--;
-          if(_model.state.mag.calibrationSamples <= 0) _model.state.mag.calibrationState = CALIBRATION_APPLY;
-          break;
-        case CALIBRATION_APPLY:
-          applyCalibration();
-          _model.state.mag.calibrationState = CALIBRATION_SAVE;
-          break;
-        case CALIBRATION_SAVE:
-          _model.finishCalibration();
-          _model.state.mag.calibrationState = CALIBRATION_IDLE;
-          break;
-        default:
-          _model.state.mag.calibrationState = CALIBRATION_IDLE;
-          break;
-      }
-    }
-
-    void resetCalibration()
-    {
-      _model.state.mag.calibrationMin = VectorFloat();
-      _model.state.mag.calibrationMax = VectorFloat();
-      _model.state.mag.calibrationValid = false;
-    }
-
-    void updateCalibration()
-    {
-      for(int i = 0; i < AXIS_COUNT_RPY; i++)
-      {
-        if(_model.state.mag.adc[i] < _model.state.mag.calibrationMin[i]) _model.state.mag.calibrationMin.set(i, _model.state.mag.adc[i]);
-        if(_model.state.mag.adc[i] > _model.state.mag.calibrationMax[i]) _model.state.mag.calibrationMax.set(i, _model.state.mag.adc[i]);
-      }
-    }
-
-    void applyCalibration()
-    {
-      const float EPSILON = 0.001f;
-
-      // verify calibration data and find biggest range
-      float maxRange = -1;
-      for(int i = 0; i < AXIS_COUNT_RPY; i++)
-      {
-        if(_model.state.mag.calibrationMin[i] > -EPSILON) return;
-        if(_model.state.mag.calibrationMax[i] <  EPSILON) return;
-        if((_model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]) > maxRange)
-        {
-          maxRange = _model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i];
-        }
-      }
-
-      // probably incomplete data, must be positive
-      if(maxRange <= 0) return;
-
-      VectorFloat scale(1.f, 1.f, 1.f);
-      VectorFloat offset(0.f, 0.f, 0.f);
-
-      for (int i = 0; i < AXIS_COUNT_RPY; i++)
-      {
-        const float range = (_model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]);
-        const float bias  = (_model.state.mag.calibrationMax[i] + _model.state.mag.calibrationMin[i]) * 0.5f;
-
-        if(abs(range) < EPSILON) return;    // incomplete data
-        
-        scale.set(i, maxRange / range);     // makes everything the same range
-        offset.set(i, bias);                // level bias
-      }
-
-      _model.state.mag.calibrationScale = scale;
-      _model.state.mag.calibrationOffset = offset;
-      _model.state.mag.calibrationValid = true;
-    }
+    void calibrate();
+    void resetCalibration();
+    void updateCalibration();
+    void applyCalibration();
 
     Model& _model;
     Device::MagDevice * _mag;
@@ -171,4 +31,3 @@ class MagSensor: public BaseSensor
 }
 
 }
-#endif
\ No newline at end of file
diff --git a/lib/Espfc/src/Sensor/VoltageSensor.cpp b/lib/Espfc/src/Sensor/VoltageSensor.cpp
new file mode 100644
index 0000000..6718ba0
--- /dev/null
+++ b/lib/Espfc/src/Sensor/VoltageSensor.cpp
@@ -0,0 +1,115 @@
+#include "VoltageSensor.h"
+
+#include <algorithm>
+
+namespace Espfc {
+
+namespace Sensor {
+
+VoltageSensor::VoltageSensor(Model &model) : _model(model) {}
+
+int VoltageSensor::begin()
+{
+  _model.state.battery.timer.setRate(100);
+  _model.state.battery.samples = 50;
+
+  _vFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate);
+  _vFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate);
+
+  _iFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate);
+  _iFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate);
+
+  _state = VBAT;
+
+  return 1;
+}
+
+int VoltageSensor::update()
+{
+  if (!_model.state.battery.timer.check()) return 0;
+
+  Stats::Measure measure(_model.state.stats, COUNTER_BATTERY);
+
+  switch (_state)
+  {
+  case VBAT:
+    _state = IBAT;
+    return readVbat();
+  case IBAT:
+    _state = VBAT;
+    return readIbat();
+  }
+
+  return 0;
+}
+
+int VoltageSensor::readVbat()
+{
+#ifdef ESPFC_ADC_0
+  if (_model.config.vbat.source != 1 || _model.config.pin[PIN_INPUT_ADC_0] == -1) return 0;
+  // wemos d1 mini has divider 3.2:1 (220k:100k)
+  // additionaly I've used divider 5.7:1 (4k7:1k)
+  // total should equals ~18.24:1, 73:4 resDiv:resMult should be ideal,
+  // but ~52:1 is real, did I miss something?
+  _model.state.battery.rawVoltage = analogRead(_model.config.pin[PIN_INPUT_ADC_0]);
+  float volts = _vFilterFast.update(_model.state.battery.rawVoltage * ESPFC_ADC_SCALE);
+
+  volts *= _model.config.vbat.scale * 0.1f;
+  volts *= _model.config.vbat.resMult;
+  volts /= _model.config.vbat.resDiv;
+
+  _model.state.battery.voltageUnfiltered = volts;
+  _model.state.battery.voltage = _vFilter.update(_model.state.battery.voltageUnfiltered);
+
+  // cell count detection
+  if (_model.state.battery.samples > 0)
+  {
+    _model.state.battery.cells = std::ceil(_model.state.battery.voltage / 4.2f);
+    _model.state.battery.samples--;
+  }
+
+  _model.state.battery.cellVoltage = _model.state.battery.voltage / constrain(_model.state.battery.cells, 1, 6);
+  _model.state.battery.percentage = Math::clamp(Math::map(_model.state.battery.cellVoltage, 3.4f, 4.2f, 0.0f, 100.0f), 0.0f, 100.0f);
+
+  if (_model.config.debug.mode == DEBUG_BATTERY)
+  {
+    _model.state.debug[0] = constrain(lrintf(_model.state.battery.voltageUnfiltered * 100.0f), 0, 32000);
+    _model.state.debug[1] = constrain(lrintf(_model.state.battery.voltage * 100.0f), 0, 32000);
+  }
+  return 1;
+#else
+  return 0;
+#endif
+}
+
+int VoltageSensor::readIbat()
+{
+#ifdef ESPFC_ADC_1
+  if (_model.config.ibat.source != 1 && _model.config.pin[PIN_INPUT_ADC_1] == -1) return 0;
+
+  _model.state.battery.rawCurrent = analogRead(_model.config.pin[PIN_INPUT_ADC_1]);
+  float volts = _iFilterFast.update(_model.state.battery.rawCurrent * ESPFC_ADC_SCALE);
+  float milivolts = volts * 1000.0f;
+
+  volts += _model.config.ibat.offset * 0.001f;
+  volts *= _model.config.ibat.scale * 0.1f;
+
+  _model.state.battery.currentUnfiltered = volts;
+  _model.state.battery.current = _iFilter.update(_model.state.battery.currentUnfiltered);
+
+  if (_model.config.debug.mode == DEBUG_CURRENT_SENSOR)
+  {
+    _model.state.debug[0] = lrintf(milivolts);
+    _model.state.debug[1] = constrain(lrintf(_model.state.battery.currentUnfiltered * 100.0f), 0, 32000);
+    _model.state.debug[2] = _model.state.battery.rawCurrent;
+  }
+
+  return 1;
+#else
+  return 0;
+#endif
+}
+
+}
+
+}
diff --git a/lib/Espfc/src/Sensor/VoltageSensor.h b/lib/Espfc/src/Sensor/VoltageSensor.h
index 3e4af7a..8c8688b 100644
--- a/lib/Espfc/src/Sensor/VoltageSensor.h
+++ b/lib/Espfc/src/Sensor/VoltageSensor.h
@@ -1,9 +1,8 @@
-#ifndef _ESPFC_SENSOR_VOLTAGE_SENSOR_H_
-#define _ESPFC_SENSOR_VOLTAGE_SENSOR_H_
+#pragma once
 
 #include "Model.h"
 #include "BaseSensor.h"
-#include <algorithm>
+#include "Filter.h"
 
 namespace Espfc {
 
@@ -16,108 +15,11 @@ class VoltageSensor: public BaseSensor
       VBAT,
       IBAT
     };
-    VoltageSensor(Model& model): _model(model) {}
-
-    int begin()
-    {
-      _model.state.battery.timer.setRate(100);
-      _model.state.battery.samples = 50;
-
-      _vFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate);
-      _vFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate);
-
-      _iFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate);
-      _iFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate);
-
-      _state = VBAT;
-
-      return 1;
-    }
-
-    int update()
-    {
-      if(!_model.state.battery.timer.check()) return 0;
-
-      Stats::Measure measure(_model.state.stats, COUNTER_BATTERY);
-
-      switch(_state) {
-        case VBAT:
-          _state = IBAT;
-          return readVbat();
-        case IBAT:
-          _state = VBAT;
-          return readIbat();
-      }
-
-      return 0;
-    }
-
-    int readVbat()
-    {
-#ifdef ESPFC_ADC_0
-      if(_model.config.vbat.source != 1 || _model.config.pin[PIN_INPUT_ADC_0] == -1) return 0;
-      // wemos d1 mini has divider 3.2:1 (220k:100k)
-      // additionaly I've used divider 5.7:1 (4k7:1k)
-      // total should equals ~18.24:1, 73:4 resDiv:resMult should be ideal,
-      // but ~52:1 is real, did I miss something?
-      _model.state.battery.rawVoltage = analogRead(_model.config.pin[PIN_INPUT_ADC_0]);
-      float volts = _vFilterFast.update(_model.state.battery.rawVoltage * ESPFC_ADC_SCALE);
-
-      volts *= _model.config.vbat.scale * 0.1f;
-      volts *= _model.config.vbat.resMult;
-      volts /= _model.config.vbat.resDiv;
-
-      _model.state.battery.voltageUnfiltered = volts;
-      _model.state.battery.voltage = _vFilter.update(_model.state.battery.voltageUnfiltered);
-
-      // cell count detection
-      if(_model.state.battery.samples > 0)
-      {
-        _model.state.battery.cells = std::ceil(_model.state.battery.voltage / 4.2f);
-        _model.state.battery.samples--;
-      }
-
-      _model.state.battery.cellVoltage = _model.state.battery.voltage / constrain(_model.state.battery.cells, 1, 6);
-      _model.state.battery.percentage = Math::clamp(Math::map(_model.state.battery.cellVoltage, 3.4f, 4.2f, 0.0f, 100.0f), 0.0f, 100.0f);
-
-      if(_model.config.debug.mode == DEBUG_BATTERY)
-      {
-        _model.state.debug[0] = constrain(lrintf(_model.state.battery.voltageUnfiltered * 100.0f), 0, 32000);
-        _model.state.debug[1] = constrain(lrintf(_model.state.battery.voltage * 100.0f), 0, 32000);
-      }
-      return 1;
-#else
-      return 0;
-#endif
-    }
-
-    int readIbat()
-    {
-#ifdef ESPFC_ADC_1
-      if(_model.config.ibat.source != 1 && _model.config.pin[PIN_INPUT_ADC_1] == -1) return 0;
-
-      _model.state.battery.rawCurrent = analogRead(_model.config.pin[PIN_INPUT_ADC_1]);
-      float volts = _iFilterFast.update(_model.state.battery.rawCurrent * ESPFC_ADC_SCALE);
-      float milivolts = volts * 1000.0f;
-
-      volts += _model.config.ibat.offset * 0.001f;
-      volts *= _model.config.ibat.scale * 0.1f;
-
-      _model.state.battery.currentUnfiltered = volts;
-      _model.state.battery.current = _iFilter.update(_model.state.battery.currentUnfiltered);
-
-      if(_model.config.debug.mode == DEBUG_CURRENT_SENSOR)
-      {
-        _model.state.debug[0] = lrintf(milivolts);
-        _model.state.debug[1] = constrain(lrintf(_model.state.battery.currentUnfiltered * 100.0f), 0, 32000);
-        _model.state.debug[2] = _model.state.battery.rawCurrent;
-      }
-
-      return 1;
-#else
-      return 0;
-#endif
-    }
+    VoltageSensor(Model& model);
+    int begin();
+    int update();
+    int readVbat();
+    int readIbat();
 
   private:
     Model& _model;
@@ -131,5 +33,3 @@ class VoltageSensor: public BaseSensor
 }
 
 }
-
-#endif
\ No newline at end of file