diff --git a/MilliOhmMeter_FW/src/board.cpp b/MilliOhmMeter_FW/src/board.cpp
index 71ad71e..4af3428 100644
--- a/MilliOhmMeter_FW/src/board.cpp
+++ b/MilliOhmMeter_FW/src/board.cpp
@@ -25,6 +25,16 @@ void initInputs(void)
   log_i("Init Inputs: OK");
 }
 
+bool getLevelIn(void)
+{
+  return digitalRead(LEVEL_IN);
+}
+
+bool getErrorIn(void)
+{
+  return digitalRead(ERROR_IN);
+}
+
 void initOutputs(void)
 {
   pinMode(UC01, OUTPUT);
diff --git a/MilliOhmMeter_FW/src/board.h b/MilliOhmMeter_FW/src/board.h
index fc23b3f..5db5ec8 100644
--- a/MilliOhmMeter_FW/src/board.h
+++ b/MilliOhmMeter_FW/src/board.h
@@ -1,7 +1,9 @@
 #pragma once
 
 void initBoard(void);
-void scanI2C(void);
+
+bool getLevelIn(void);
+bool getErrorIn(void);
 
 #define HAS_DISPLAY
 #define HAS_SSD1322
diff --git a/MilliOhmMeter_FW/src/gain_control.cpp b/MilliOhmMeter_FW/src/gain_control.cpp
index 81bb4ec..4e56492 100644
--- a/MilliOhmMeter_FW/src/gain_control.cpp
+++ b/MilliOhmMeter_FW/src/gain_control.cpp
@@ -1,13 +1,55 @@
 #include "gain_control.h"
 
+c_gainControl modeNone(e_measureMode::mNone, 0, 0, 0, 0, 0, 0);
 c_gainControl mode20mA(e_measureMode::mA20, 1, 1, 0, 0, 1, 1);
 
+
+std::vector<c_gainControl> gainList;
+
+e_measureMode lastGain = e_measureMode::mNone;
+
 void initGainControl(void)
 {
-
+  gainList.push_back(modeNone);
+  gainList.push_back(mode20mA);
 }
 
 void handleGainControl(void)
 {
+}
 
+bool setGain(e_measureMode mode)
+{
+  bool found = false;
+  if(mode == lastGain)
+  {
+    return false;
+  }
+  
+  for (auto &&gain : gainList)
+  {
+    if (gain.getMode() == mode)
+    {
+      gain.activate();
+      found = true;
+      lastGain = mode;
+    }
+  }
+  if(!found)
+  {
+    log_e("measureMode %d not found", mode);
+    resetGain();
+    return false;
+  }
+  return true;
+}
+
+void resetGain(void)
+{
+  log_d("reset gain control");
+  for (auto &&gain : gainList)
+  {
+    gain.deactivate();
+  }
+  setGain(mNone);
 }
\ No newline at end of file
diff --git a/MilliOhmMeter_FW/src/gain_control.h b/MilliOhmMeter_FW/src/gain_control.h
index 2f48ad3..4d5133a 100644
--- a/MilliOhmMeter_FW/src/gain_control.h
+++ b/MilliOhmMeter_FW/src/gain_control.h
@@ -3,6 +3,7 @@
 #include "Arduino.h"
 #include "measure_mode.h"
 #include "board.h"
+#include <vector>
 
 #define UCCONTROLIO 6
 
@@ -12,6 +13,7 @@ class c_gainControl
 
   bool _uc[UCCONTROLIO];
   uint8_t _ucPin[UCCONTROLIO] = {UC01, UC02, UC03, UC04, UC05, UC06};
+  bool state = false;
 
 public:
   c_gainControl(e_measureMode mode, bool uc01, bool uc02, bool uc03, bool uc04, bool uc05, bool uc06) : _mode(mode)
@@ -30,6 +32,12 @@ public:
     {
       digitalWrite(_ucPin[i], _uc[i]);
     }
+    state = true;
+  }
+
+  void deactivate(void)
+  {
+    state = false;
   }
 
   e_measureMode getMode(void)
@@ -39,4 +47,7 @@ public:
 };
 
 void initGainControl(void);
-void handleGainControl(void);
\ No newline at end of file
+void handleGainControl(void);
+
+void resetGain(void);
+bool setGain(e_measureMode mode);
diff --git a/MilliOhmMeter_FW/src/measure.cpp b/MilliOhmMeter_FW/src/measure.cpp
index be88dc6..8c72144 100644
--- a/MilliOhmMeter_FW/src/measure.cpp
+++ b/MilliOhmMeter_FW/src/measure.cpp
@@ -6,6 +6,8 @@ void initMeasure(void)
   log_i("init measure");
   initMeasureHall();
   initMeasureMode();
+  initMeasureState();
+  log_i("init measure: OK");
   
 }
 
@@ -14,6 +16,7 @@ void handleMeasure(void)
   log_d("handle measure");
   handleMeasureHall();
   handleMeasureMode();
+  handleMeasureState();
 }
 
 double getMin(void)
diff --git a/MilliOhmMeter_FW/src/measure.h b/MilliOhmMeter_FW/src/measure.h
index 0c3a004..b6f4fb9 100644
--- a/MilliOhmMeter_FW/src/measure.h
+++ b/MilliOhmMeter_FW/src/measure.h
@@ -4,6 +4,7 @@
 #include "board.h"
 #include "measure_hall.h"
 #include "measure_mode.h"
+#include "measure_state.h"
 
 #define DACINTERVAL         10 //ms
 #define DACSTEP             8 //ticks
diff --git a/MilliOhmMeter_FW/src/measure_mode.cpp b/MilliOhmMeter_FW/src/measure_mode.cpp
index 0c16b56..a43efa0 100644
--- a/MilliOhmMeter_FW/src/measure_mode.cpp
+++ b/MilliOhmMeter_FW/src/measure_mode.cpp
@@ -18,7 +18,7 @@ e_measureMode getMeasureMode(void)
 
 bool getErrorState(void)
 {
-  return measureError;
+  return getErrorIn();
 }
 
 bool getOkState(void)
@@ -31,7 +31,6 @@ bool getOpenState(void)
   return measureOpen;
 }
 
-
 void initMeasureMode(void)
 {
   log_i("init measure mode");
@@ -39,5 +38,5 @@ void initMeasureMode(void)
 
 void handleMeasureMode(void)
 {
-
+  
 }
\ No newline at end of file
diff --git a/MilliOhmMeter_FW/src/measure_mode.h b/MilliOhmMeter_FW/src/measure_mode.h
index 6c8445a..d6e7e02 100644
--- a/MilliOhmMeter_FW/src/measure_mode.h
+++ b/MilliOhmMeter_FW/src/measure_mode.h
@@ -1,6 +1,8 @@
 #pragma once
 
 #include "Arduino.h"
+#include "board.h"
+
 
 enum e_measureMode
 {
@@ -13,6 +15,7 @@ enum e_measureMode
   mNone
 };
 
+
 e_measureMode getMeasureMode( void );
 void setMeasureMode(e_measureMode newMode);
 bool getErrorState(void);
diff --git a/MilliOhmMeter_FW/src/measure_state.cpp b/MilliOhmMeter_FW/src/measure_state.cpp
new file mode 100644
index 0000000..039aca3
--- /dev/null
+++ b/MilliOhmMeter_FW/src/measure_state.cpp
@@ -0,0 +1,61 @@
+#include "measure_state.h"
+
+e_measureStates currentMeasureState = e_measureStates::stateInit;
+e_measureStates lastMeasureState = e_measureStates::stateInit;
+
+uint32_t lastMeasureTime = 0;
+
+void setMeasureState(e_measureStates newState)
+{
+  if (currentMeasureState != e_measureStates::stateError && currentMeasureState != e_measureStates::stateFatalError )
+  {
+    currentMeasureState = newState;
+  }
+  else
+  {
+    log_e("system in error state, not changing states");
+  }
+}
+
+void initMeasureState(void)
+{
+  currentMeasureState = e_measureStates::stateInit;
+  lastMeasureTime = millis();
+}
+
+void handleMeasureState(void)
+{
+  uint32_t currentTime = millis();
+  bool newState = false;
+  if (currentTime - lastMeasureTime > MEASURESTATEINTERVAL)
+  {
+    if (currentMeasureState != lastMeasureState)
+    {
+      log_i("measureState: %s", s_measureState[currentMeasureState]);
+      newState = true;
+    }
+
+    switch (currentMeasureState)
+    {
+    case e_measureStates::stateInit:
+    {
+      setGain(mNone);
+      currentMeasureState = stateIdle;
+    }
+    break;
+    case e_measureStates::stateError:
+    {
+      setGain(mNone);
+      if(!getErrorIn())
+      {
+        currentMeasureState = e_measureStates::stateIdle;
+      }
+    }
+    break;
+
+    default:
+      break;
+    }
+    lastMeasureTime = currentTime;
+  }
+}
diff --git a/MilliOhmMeter_FW/src/measure_state.h b/MilliOhmMeter_FW/src/measure_state.h
new file mode 100644
index 0000000..8bc8b68
--- /dev/null
+++ b/MilliOhmMeter_FW/src/measure_state.h
@@ -0,0 +1,35 @@
+#pragma once
+
+#include "Arduino.h"
+#include "gain_control.h"
+#include "measure_mode.h"
+
+#define  MEASURESTATEINTERVAL 200
+
+typedef enum 
+{
+  stateInit,
+  stateIdle,
+  stateConnected,
+  stateAllGood,
+  stateOpen,
+  stateError,
+  stateFatalError,
+  lastState
+}e_measureStates;
+
+String s_measureState[lastState+1] = 
+{
+  "init",
+  "idle",
+  "connected",
+  "allGood",
+  "Open",
+  "Error",
+  "FatalError",
+  "invalidstate"
+};
+
+void initMeasureState(void);
+void handleMeasureState(void);
+void setMeasureState(e_measureStates newState);