diff --git a/docs/img/FSM.png b/docs/img/FSM.png
index 7f6db881fff5cdfb9351c0348dfec49ff082516d..37f9b8ef9cbe7779719a011c026eb0739606f4a1 100644
Binary files a/docs/img/FSM.png and b/docs/img/FSM.png differ
diff --git a/src/hdl/controlUnit.vhd b/src/hdl/controlUnit.vhd
index 21da15f3e1bdfbfa00939c8508c01ee8a1caca2f..5e6b2a9eabb6c89115e4163cca6e549af8bb4901 100644
--- a/src/hdl/controlUnit.vhd
+++ b/src/hdl/controlUnit.vhd
@@ -54,23 +54,47 @@ begin
   -- Process to describe the state register
   -- Current state is provide at the output of the register
   -- and is updated with the next state at each rising edge of clock
-  process (_BLANK_) is
+  process (I_clock, I_reset) is
   begin
     if I_reset = '1' then               -- asynchronous reset (active high)
-      SR_currentState <= _BLANK_
+      SR_currentState <= WAIT_SAMPLE ;
     elsif rising_edge(I_clock) then     -- rising clock edge
-      _BLANK_
+     SR_currentState <= SR_nextState;
     end if;
   end process;
 
   -- Combinatorial process computing the next state which depends on
   -- the current state and on the inputs
-  process (_BLANK_) is
+  process (SR_currentState, I_inputSampleValid, I_processingDone) is
   begin
     case SR_currentState is
 
       when WAIT_SAMPLE =>
-        _BLANK_
+        if I_inputSampleValid = '1' then
+          SR_nextState <= STORE;
+        else
+          SR_nextState <= WAIT_SAMPLE;
+        end if;
+
+      when STORE =>
+        SR_nextState <= PROCESSING_LOOP;
+
+      when PROCESSING_LOOP =>
+        if I_processingDone = '1' then
+          SR_nextState <= OUTPUT;
+        else
+          SR_nextState <= PROCESSING_LOOP;
+        end if;
+
+      when OUTPUT =>
+        SR_nextState <= WAIT_END_SAMPLE;
+
+      when WAIT_END_SAMPLE =>
+        if I_inputSampleValid = '0' then
+          SR_nextState <= WAIT_SAMPLE;
+        else
+          SR_nextState <= WAIT_END_SAMPLE;
+        end if;
 
       when others => null;
     end case;
@@ -78,13 +102,13 @@ begin
 
   -- Rules to compute the outputs depending on the current state
   -- (and on the inputs, if you want a Mealy machine).
-  O_loadShift           <= '1' when _BLANK_ else '0';
-  O_initAddress         <= '1' when _BLANK_ else '0';
-  O_incrAddress         <= '1' when _BLANK_ else '0';
-  O_initSum             <= '1' when _BLANK_ else '0';
-  O_loadSum             <= '1' when _BLANK_ else '0';
-  O_loadOutput          <= '1' when _BLANK_ else '0';
-  O_FilteredSampleValid <= '1' when _BLANK_ else '0';
+  O_loadShift           <= '1' when SR_currentState = STORE else '0';
+  O_initAddress         <= '1' when SR_currentState = STORE else '0';
+  O_incrAddress         <= '1' when SR_currentState = PROCESSING_LOOP else '0';
+  O_initSum             <= '1' when SR_currentState = STORE else '0';
+  O_loadSum             <= '1' when SR_currentState = PROCESSING_LOOP else '0';
+  O_loadOutput          <= '1' when SR_currentState = OUTPUT else '0';
+  O_FilteredSampleValid <= '1' when SR_currentState = OUTPUT else '0';