diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp
index 5865599..eb6d03e 100644
--- a/lib/Espfc/src/Blackbox/Blackbox.cpp
+++ b/lib/Espfc/src/Blackbox/Blackbox.cpp
@@ -248,7 +248,7 @@ void FAST_CODE_ATTR Blackbox::updateData()
       acc.accADC[i] = _model.state.accel[i] * ACCEL_G_INV * acc.dev.acc_1G;
     }
     if(_model.magActive()) {
-      mag.magADC[i] = _model.state.mag[i] * 1090;
+      mag.magADC[i] = _model.state.mag.adc[i] * 1090;
     }
     if(_model.baroActive()) {
       baro.altitude = lrintf(_model.state.baroAltitude * 100.f); // cm
diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h
index 60163af..ba8bbcf 100644
--- a/lib/Espfc/src/Cli.h
+++ b/lib/Espfc/src/Cli.h
@@ -1058,17 +1058,17 @@ class Cli
           s.print(_model.config.mag.offset[0]); s.print(' ');
           s.print(_model.config.mag.offset[1]); s.print(' ');
           s.print(_model.config.mag.offset[2]); s.print(F(" ["));
-          s.print(_model.state.magCalibrationOffset[0]); s.print(' ');
-          s.print(_model.state.magCalibrationOffset[1]); s.print(' ');
-          s.print(_model.state.magCalibrationOffset[2]); s.println(F("]"));
+          s.print(_model.state.mag.calibrationOffset[0]); s.print(' ');
+          s.print(_model.state.mag.calibrationOffset[1]); s.print(' ');
+          s.print(_model.state.mag.calibrationOffset[2]); s.println(F("]"));
 
           s.print(F("   mag scale: "));
           s.print(_model.config.mag.scale[0]); s.print(' ');
           s.print(_model.config.mag.scale[1]); s.print(' ');
           s.print(_model.config.mag.scale[2]); s.print(F(" ["));
-          s.print(_model.state.magCalibrationScale[0]); s.print(' ');
-          s.print(_model.state.magCalibrationScale[1]); s.print(' ');
-          s.print(_model.state.magCalibrationScale[2]); s.println(F("]"));
+          s.print(_model.state.mag.calibrationScale[0]); s.print(' ');
+          s.print(_model.state.mag.calibrationScale[1]); s.print(' ');
+          s.print(_model.state.mag.calibrationScale[2]); s.println(F("]"));
         }
         else if(strcmp_P(cmd.args[1], PSTR("gyro")) == 0)
         {
@@ -1092,8 +1092,8 @@ class Cli
         }
         else if(strcmp_P(cmd.args[1], PSTR("reset_mag")) == 0 || strcmp_P(cmd.args[1], PSTR("reset_all")) == 0)
         {
-          _model.state.magCalibrationOffset = VectorFloat();
-          _model.state.magCalibrationScale = VectorFloat(1.f, 1.f, 1.f);
+          _model.state.mag.calibrationOffset = VectorFloat();
+          _model.state.mag.calibrationScale = VectorFloat(1.f, 1.f, 1.f);
           s.println(F("OK"));
         }
       }
@@ -1241,7 +1241,7 @@ class Cli
 
         Device::GyroDevice * gyro = _model.state.gyroDev;
         Device::BaroDevice * baro = _model.state.baroDev;
-        Device::MagDevice  * mag  = _model.state.magDev;
+        Device::MagDevice  * mag  = _model.state.mag.dev;
         s.print(F("     devices: "));
         if(gyro)
         {
@@ -1543,7 +1543,7 @@ class Cli
       s.println(F(" Hz"));
 
       s.print(F("    mag rate: "));
-      s.print(_model.state.magTimer.rate);
+      s.print(_model.state.mag.timer.rate);
       s.println(F(" Hz"));
     }
 
diff --git a/lib/Espfc/src/Device/MagAK8963.h b/lib/Espfc/src/Device/MagAK8963.h
index da1515b..ae8d17d 100644
--- a/lib/Espfc/src/Device/MagAK8963.h
+++ b/lib/Espfc/src/Device/MagAK8963.h
@@ -81,7 +81,7 @@ class MagAK8963: public MagDevice
 
     const VectorFloat convert(const VectorInt16& v) const override
     {
-      return (VectorFloat)v * scale;
+      return static_cast<VectorFloat>(v) * scale;
     }
 
     int getRate() const override
diff --git a/lib/Espfc/src/Fusion.cpp b/lib/Espfc/src/Fusion.cpp
index 8fe3b80..96da479 100644
--- a/lib/Espfc/src/Fusion.cpp
+++ b/lib/Espfc/src/Fusion.cpp
@@ -218,8 +218,8 @@ void Fusion::updatePoseFromAccelMag()
     q.y =  cosX2 * sinY2;
     q.z = -sinX2 * sinY2;
 
-    VectorFloat m = _model.state.mag.getRotated(q);
-    _model.state.magPose = m;
+    VectorFloat m = _model.state.mag.adc.getRotated(q);
+    _model.state.mag.pose = m;
     _model.state.pose.z = -atan2(m.y, m.x);
   }
   else
@@ -265,7 +265,7 @@ void Fusion::lerpFusion()
   if(_model.magActive())
   {
     // use yaw from mag
-    VectorFloat mag = _model.state.mag.getRotated(_model.state.accelPoseQ);
+    VectorFloat mag = _model.state.mag.adc.getRotated(_model.state.accelPoseQ);
     _model.state.accelPose.z = -atan2(mag.y, mag.x);
   }
   else
@@ -294,7 +294,7 @@ void Fusion::madgwickFusion()
     _madgwick.update(
       _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z,
       _model.state.accel.x, _model.state.accel.y, _model.state.accel.z,
-      _model.state.mag.x,   _model.state.mag.y,   _model.state.mag.z
+      _model.state.mag.adc.x,   _model.state.mag.adc.y,   _model.state.mag.adc.z
     );
   }
   else
@@ -315,7 +315,7 @@ void FAST_CODE_ATTR Fusion::mahonyFusion()
     _mahony.update(
       _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z,
       _model.state.accel.x, _model.state.accel.y, _model.state.accel.z,
-      _model.state.mag.x,   _model.state.mag.y,   _model.state.mag.z
+      _model.state.mag.adc.x,   _model.state.mag.adc.y,   _model.state.mag.adc.z
     );
   }
   else
diff --git a/lib/Espfc/src/Hardware.h b/lib/Espfc/src/Hardware.h
index 9aae4cd..954ae20 100644
--- a/lib/Espfc/src/Hardware.h
+++ b/lib/Espfc/src/Hardware.h
@@ -153,9 +153,9 @@ class Hardware
         if(!detectedMag && detectDevice(qmc5883l, gyroSlaveBus)) detectedMag = &qmc5883l;
         
       }
-      _model.state.magDev = detectedMag;
-      _model.state.magPresent = (bool)detectedMag;
-      _model.state.magRate = detectedMag ? detectedMag->getRate() : 0;
+      _model.state.mag.dev = detectedMag;
+      _model.state.mag.present = (bool)detectedMag;
+      _model.state.mag.rate = detectedMag ? detectedMag->getRate() : 0;
     }
 
     void detectBaro()
diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h
index 5e76661..897d700 100644
--- a/lib/Espfc/src/Model.h
+++ b/lib/Espfc/src/Model.h
@@ -119,7 +119,7 @@ class Model
 
     bool magActive() const
     {
-      return state.magPresent && config.mag.dev != MAG_NONE;
+      return state.mag.present && config.mag.dev != MAG_NONE;
     }
 
     bool baroActive() const
@@ -129,7 +129,7 @@ class Model
 
     bool calibrationActive() const
     {
-      return state.accelCalibrationState != CALIBRATION_IDLE || state.gyroCalibrationState != CALIBRATION_IDLE || state.magCalibrationState != CALIBRATION_IDLE;
+      return state.accelCalibrationState != CALIBRATION_IDLE || state.gyroCalibrationState != CALIBRATION_IDLE || state.mag.calibrationState != CALIBRATION_IDLE;
     }
 
     void calibrateGyro()
@@ -143,7 +143,7 @@ class Model
 
     void calibrateMag()
     {
-      state.magCalibrationState = CALIBRATION_START;
+      state.mag.calibrationState = CALIBRATION_START;
     }
 
     void finishCalibration()
@@ -159,11 +159,11 @@ class Model
         save();
         logger.info().log(F("ACCEL BIAS")).log(state.accelBias.x).log(state.accelBias.y).logln(state.accelBias.z);
       }
-      if(state.magCalibrationState == CALIBRATION_SAVE)
+      if(state.mag.calibrationState == CALIBRATION_SAVE)
       {
         save();
-        logger.info().log(F("MAG BIAS")).log(state.magCalibrationOffset.x).log(state.magCalibrationOffset.y).logln(state.magCalibrationOffset.z);
-        logger.info().log(F("MAG SCALE")).log(state.magCalibrationScale.x).log(state.magCalibrationScale.y).logln(state.magCalibrationScale.z);
+        logger.info().log(F("MAG BIAS")).log(state.mag.calibrationOffset.x).log(state.mag.calibrationOffset.y).logln(state.mag.calibrationOffset.z);
+        logger.info().log(F("MAG SCALE")).log(state.mag.calibrationScale.x).log(state.mag.calibrationScale.y).logln(state.mag.calibrationScale.z);
       }
     }
 
@@ -441,7 +441,7 @@ class Model
       state.stats.timer.setRate(3);
       if(magActive())
       {
-        state.magTimer.setRate(state.magRate);
+        state.mag.timer.setRate(state.mag.rate);
       }
 
       state.boardAlignment.init(VectorFloat(Math::toRad(config.boardAlignment[0]), Math::toRad(config.boardAlignment[1]), Math::toRad(config.boardAlignment[2])));
@@ -486,7 +486,7 @@ class Model
         }
         if(magActive())
         {
-          state.magFilter[i].begin(config.mag.filter, state.magTimer.rate);
+          state.mag.filter[i].begin(config.mag.filter, state.mag.timer.rate);
         }
       }
 
@@ -576,8 +576,8 @@ class Model
       {
         state.gyroBias.set(i, config.gyro.bias[i] / 1000.0f);
         state.accelBias.set(i, config.accel.bias[i] / 1000.0f);
-        state.magCalibrationOffset.set(i, config.mag.offset[i] / 10.0f);
-        state.magCalibrationScale.set(i, config.mag.scale[i] / 1000.0f);
+        state.mag.calibrationOffset.set(i, config.mag.offset[i] / 10.0f);
+        state.mag.calibrationScale.set(i, config.mag.scale[i] / 1000.0f);
       }
     }
 
@@ -588,8 +588,8 @@ class Model
       {
         config.gyro.bias[i] = lrintf(state.gyroBias[i] * 1000.0f);
         config.accel.bias[i] = lrintf(state.accelBias[i] * 1000.0f);
-        config.mag.offset[i] = lrintf(state.magCalibrationOffset[i] * 10.0f);
-        config.mag.scale[i] = lrintf(state.magCalibrationScale[i] * 1000.0f);
+        config.mag.offset[i] = lrintf(state.mag.calibrationOffset[i] * 10.0f);
+        config.mag.scale[i] = lrintf(state.mag.calibrationScale[i] * 1000.0f);
       }
     }
 
diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h
index 27c440d..0d15099 100644
--- a/lib/Espfc/src/ModelConfig.h
+++ b/lib/Espfc/src/ModelConfig.h
@@ -604,8 +604,8 @@ struct MagConfig
   int8_t bus = BUS_AUTO;
   int8_t dev = MAG_NONE;
   int8_t align = ALIGN_DEFAULT;
-  int16_t scale[3] = { 0, 0, 0 };
-  int16_t offset[3] = { 1000, 1000, 1000 };
+  int16_t offset[3] = { 0, 0, 0 };
+  int16_t scale[3] = { 1000, 1000, 1000 };
   FilterConfig filter{FILTER_BIQUAD, 10};
 };
 
diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h
index 6831b56..44ceef2 100644
--- a/lib/Espfc/src/ModelState.h
+++ b/lib/Espfc/src/ModelState.h
@@ -208,11 +208,34 @@ struct MixerState
   EscDriver * escServo;
 };
 
+struct MagState
+{
+  Device::MagDevice* dev;
+  bool present;
+  int rate;
+
+  VectorInt16 raw;
+  VectorFloat adc;
+  Filter filter[3];
+  Timer timer;
+
+  int calibrationSamples;
+  int calibrationState;
+  bool calibrationValid;
+  VectorFloat calibrationMin;
+  VectorFloat calibrationMax;
+  VectorFloat calibrationScale;
+  VectorFloat calibrationOffset;
+
+  VectorFloat pose;
+};
+
 // working data
 struct ModelState
 {
+  MagState mag;
+
   Device::GyroDevice* gyroDev;
-  Device::MagDevice* magDev;
   Device::BaroDevice* baroDev;
 
   VectorInt16 gyroRaw;
@@ -222,18 +245,15 @@ struct ModelState
   VectorFloat gyroImu;
 
   VectorInt16 accelRaw;
-  VectorInt16 magRaw;
 
   VectorFloat gyro;
   VectorFloat accel;
-  VectorFloat mag;
 
   VectorFloat gyroPose;
   Quaternion gyroPoseQ;
   VectorFloat accelPose;
   VectorFloat accelPose2;
   Quaternion accelPoseQ;
-  VectorFloat magPose;
 
   bool imuUpdate;
   bool loopUpdate;
@@ -254,7 +274,6 @@ struct ModelState
   Filter gyroImuFilter[3];
 
   Filter accelFilter[3];
-  Filter magFilter[3];
   Filter rpmFreqFilter[RPM_FILTER_MOTOR_MAX];
   Filter rpmFilter[RPM_FILTER_MOTOR_MAX][RPM_FILTER_HARMONICS_MAX][3];
 
@@ -304,19 +323,6 @@ struct ModelState
   Timer loopTimer;
 
   Timer actuatorTimer;
-
-  Timer magTimer;
-  int magRate;
-
-  int magCalibrationSamples;
-  int magCalibrationState;
-  bool magCalibrationValid;
-
-  VectorFloat magCalibrationMin;
-  VectorFloat magCalibrationMax;
-  VectorFloat magCalibrationScale;
-  VectorFloat magCalibrationOffset;
-  
   Timer telemetryTimer;
 
   Stats stats;
@@ -343,7 +349,6 @@ struct ModelState
 
   bool gyroPresent;
   bool accelPresent;
-  bool magPresent;
   bool baroPresent;
   
   float baroTemperatureRaw;
diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h
index 880846d..4abdf30 100644
--- a/lib/Espfc/src/Msp/MspProcessor.h
+++ b/lib/Espfc/src/Msp/MspProcessor.h
@@ -1287,7 +1287,7 @@ class MspProcessor
           }
           for (int i = 0; i < 3; i++)
           {
-            r.writeU16(lrintf(_model.state.mag[i] * 1090));
+            r.writeU16(lrintf(_model.state.mag.adc[i] * 1090));
           }
           break;
 
diff --git a/lib/Espfc/src/Sensor/MagSensor.h b/lib/Espfc/src/Sensor/MagSensor.h
index f228c69..ab1208f 100644
--- a/lib/Espfc/src/Sensor/MagSensor.h
+++ b/lib/Espfc/src/Sensor/MagSensor.h
@@ -19,16 +19,16 @@ class MagSensor: public BaseSensor
     {
       if(!_model.magActive()) return 0;
 
-      _mag = _model.state.magDev;
+      _mag = _model.state.mag.dev;
       if(!_mag) return 0;
 
-      if(_model.state.magTimer.rate < 5) return 0;
+      if(_model.state.mag.timer.rate < 5) return 0;
 
       // by default use eeprom calibration settings
-      _model.state.magCalibrationState = CALIBRATION_IDLE;
-      _model.state.magCalibrationValid = true;
+      _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.magTimer.rate);
+      _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;
     }
@@ -44,10 +44,10 @@ class MagSensor: public BaseSensor
 
     int read()
     {
-      if(!_mag || !_model.magActive() || !_model.state.magTimer.check()) return 0;
+      if(!_mag || !_model.magActive() || !_model.state.mag.timer.check()) return 0;
 
       Stats::Measure measure(_model.state.stats, COUNTER_MAG_READ);
-      _mag->readMag(_model.state.magRaw);
+      _mag->readMag(_model.state.mag.raw);
 
       return 1;
     }
@@ -58,70 +58,78 @@ class MagSensor: public BaseSensor
 
       Stats::Measure measure(_model.state.stats, COUNTER_MAG_FILTER);
 
-      _model.state.mag = _mag->convert(_model.state.magRaw);
+      _model.state.mag.adc = _mag->convert(_model.state.mag.raw);
 
-      align(_model.state.mag, _model.config.mag.align);
-      _model.state.mag = _model.state.boardAlignment.apply(_model.state.mag);
 
-      for(size_t i = 0; i < 3; i++)
+      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.set(i, _model.state.magFilter[i].update(_model.state.mag[i]));
+        _model.state.mag.adc.set(i, _model.state.mag.filter[i].update(_model.state.mag.adc[i]));
       }
 
       calibrate();
 
+      /*Serial.print(_model.state.mag.adc.x);
+      Serial.print(' ');
+      Serial.print(_model.state.mag.adc.y);
+      Serial.print(' ');
+      Serial.print(_model.state.mag.adc.z);
+      Serial.print('\n');*/
+
       return 1;
     }
 
   private:
     void calibrate()
     {
-      switch(_model.state.magCalibrationState)
+      switch(_model.state.mag.calibrationState)
       {
         case CALIBRATION_IDLE:
-          if(_model.state.magCalibrationValid)
+          if(_model.state.mag.calibrationValid)
           {
-            _model.state.mag -= _model.state.magCalibrationOffset;
-            _model.state.mag *= _model.state.magCalibrationScale;
+            _model.state.mag.adc -= _model.state.mag.calibrationOffset;
+            _model.state.mag.adc *= _model.state.mag.calibrationScale;
           }
           break;
         case CALIBRATION_START:
           resetCalibration();
-          _model.state.magCalibrationSamples = 30 * _model.state.magTimer.rate;
-          _model.state.magCalibrationState = CALIBRATION_UPDATE;
+          _model.state.mag.calibrationSamples = 30 * _model.state.mag.timer.rate;
+          _model.state.mag.calibrationState = CALIBRATION_UPDATE;
           break;
         case CALIBRATION_UPDATE:
           updateCalibration();
-          _model.state.magCalibrationSamples--;
-          if(_model.state.magCalibrationSamples <= 0) _model.state.magCalibrationState = CALIBRATION_APPLY;
+          _model.state.mag.calibrationSamples--;
+          if(_model.state.mag.calibrationSamples <= 0) _model.state.mag.calibrationState = CALIBRATION_APPLY;
           break;
         case CALIBRATION_APPLY:
           applyCalibration();
-          _model.state.magCalibrationState = CALIBRATION_SAVE;
+          _model.state.mag.calibrationState = CALIBRATION_SAVE;
           break;
         case CALIBRATION_SAVE:
           _model.finishCalibration();
-          _model.state.magCalibrationState = CALIBRATION_IDLE;
+          _model.state.mag.calibrationState = CALIBRATION_IDLE;
           break;
         default:
-          _model.state.magCalibrationState = CALIBRATION_IDLE;
+          _model.state.mag.calibrationState = CALIBRATION_IDLE;
           break;
       }
     }
 
     void resetCalibration()
     {
-      _model.state.magCalibrationMin = VectorFloat();
-      _model.state.magCalibrationMax = VectorFloat();
-      _model.state.magCalibrationValid = false;
+      _model.state.mag.calibrationMin = VectorFloat();
+      _model.state.mag.calibrationMax = VectorFloat();
+      _model.state.mag.calibrationValid = false;
     }
 
     void updateCalibration()
     {
       for(int i = 0; i < 3; i++)
       {
-        if(_model.state.mag[i] < _model.state.magCalibrationMin[i]) _model.state.magCalibrationMin.set(i, _model.state.mag[i]);
-        if(_model.state.mag[i] > _model.state.magCalibrationMax[i]) _model.state.magCalibrationMax.set(i, _model.state.mag[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]);
       }
     }
 
@@ -133,11 +141,11 @@ class MagSensor: public BaseSensor
       float maxRange = -1;
       for(int i = 0; i < 3; i++)
       {
-        if(_model.state.magCalibrationMin[i] > -EPSILON) return;
-        if(_model.state.magCalibrationMax[i] <  EPSILON) return;
-        if((_model.state.magCalibrationMax[i] - _model.state.magCalibrationMin[i]) > maxRange)
+        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.magCalibrationMax[i] - _model.state.magCalibrationMin[i];
+          maxRange = _model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i];
         }
       }
 
@@ -149,8 +157,8 @@ class MagSensor: public BaseSensor
 
       for (int i = 0; i < 3; i++)
       {
-        const float range = (_model.state.magCalibrationMax[i] - _model.state.magCalibrationMin[i]);
-        const float bias  = (_model.state.magCalibrationMax[i] + _model.state.magCalibrationMin[i]) * 0.5f;
+        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
         
@@ -158,9 +166,9 @@ class MagSensor: public BaseSensor
         offset.set(i, bias);                // level bias
       }
 
-      _model.state.magCalibrationScale = scale;
-      _model.state.magCalibrationOffset = offset;
-      _model.state.magCalibrationValid = true;
+      _model.state.mag.calibrationScale = scale;
+      _model.state.mag.calibrationOffset = offset;
+      _model.state.mag.calibrationValid = true;
     }
 
     Model& _model;