diff --git a/fpga/Makefile b/fpga/Makefile
index bb32954..0a536c0 100644
--- a/fpga/Makefile
+++ b/fpga/Makefile
@@ -175,7 +175,10 @@ build/%_tb: build/%_tb.o $(addprefix build/,$(subst .vhd,.o,$(VHDSOURCE)))
 build/%_tb.vcd: build/%_tb
 	(cd "$(shell dirname "$<")"; ghdl -r "$(basename $(notdir $<))" --vcd="../$@" )
 
-%_wave: build/%_tb.vcd
+build/%_tb.ghw: build/%_tb
+	(cd "$(shell dirname "$<")"; ghdl -r "$(basename $(notdir $<))" --wave="../$@" )
+
+%_wave: build/%_tb.ghw
 	gtkwave --save "$(notdir $(basename $<)).gtkw" "$<"
 
 ###########################################################################
diff --git a/fpga/communication.vhd b/fpga/communication.vhd
index 5810b2c..931a524 100644
--- a/fpga/communication.vhd
+++ b/fpga/communication.vhd
@@ -34,8 +34,9 @@ architecture Behavioral of communication is
     signal readOffset : integer := 0;
 
     type sendMessages is (none, A2FD_PINGs, F2AD_ERR_UNKNOWN_CODEs);
-    signal sendMessage : sendMessages := none;
-    signal sendOffset : integer := 0;
+
+    constant SENDQUEUE_SIZE : integer := 4;
+    type sendQueueMemorya is array (0 to SENDQUEUE_SIZE - 1) of sendMessages;
 
     signal frontTrigger : integer := 0;
     signal backTrigger : integer := 0;
@@ -46,13 +47,49 @@ begin
 
     txStb <= txStbs;
 
+
     readsendFA : process(clock, reset)
+
+        variable sendMessage : sendMessages := none;
+        variable sendOffset : integer := 0;
+
+        -- Send queue
+        variable sendQueueMemory : sendQueueMemorya;
+        variable sendTail : integer := 0;
+        variable sendHead : integer := 0;
+        variable sendLooped : boolean := false;
+
+        procedure pushSend
+        (message : in sendMessages) is
+        begin
+            sendQueueMemory(sendHead) := message;
+            if sendHead < SENDQUEUE_SIZE - 1 then
+                sendHead := sendHead + 1;
+            else
+                sendHead := 0;
+                sendLooped := true;
+            end if;
+        end pushSend;
+
+        procedure popSend
+        (message : out sendMessages) is
+        begin
+            if sendTail < sendHead or sendLooped then
+                message := sendQueueMemory(sendTail);
+                if sendTail < SENDQUEUE_SIZE - 1 then
+                    sendTail := sendTail + 1;
+                else
+                    sendTail := 0;
+                    sendLooped := false;
+                end if;
+            else
+                message := none;
+            end if;
+        end popSend;
+
     begin
         if reset = '1' then
             readState <= readIdle;
-            txStbs <= '0';
-            sendMessage <= none;
-            sendOffset <= 0;
         else
             if rising_edge(clock) then
                 -- If read something
@@ -60,31 +97,36 @@ begin
                     if readState = readIdle then
                         case rxData is
                             when A2FD_PING =>
-                                sendMessage <= A2FD_PINGs; -- TODO Not so brutal
+                                pushSend(A2FD_PINGs);
                             when others =>
-                                sendMessage <= F2AD_ERR_UNKNOWN_CODEs;
+                                pushSend(F2AD_ERR_UNKNOWN_CODEs);
                         end case;
                     end if;
                 end if;
 
                 -- If what was sent is acknowledged and there is still something to send
                 if txStbs = '0' or txAck = '1' then
+                    if sendMessage = none then -- Reads a message if none is currently being sent
+                        sendOffset := 0;
+                        popSend(sendMessage);
+                    else
+                        sendOffset := sendOffset + 1;
+                    end if;
+
                     txStbs <= '1';
-                    sendOffset <= sendOffset + 1;
                     case sendMessage is
-                        when none =>
+                        when none => -- If no message available: don't send anything
                             txStbs <= '0';
-                            sendOffset <= 0;
                         when A2FD_PINGs =>
                             txData <= A2FD_PING;
-                            sendMessage <= none;
+                            sendMessage := none;
                         when F2AD_ERR_UNKNOWN_CODEs =>
                             case sendOffset is
                                 when 0 =>
                                     txData <= F2AD_ERR;
                                 when others =>
                                     txData <= ERR_UNKNOWN_CODE;
-                                    sendMessage <= none;
+                                    sendMessage := none;
                             end case;
                     end case;
                 end if;
diff --git a/fpga/communication_tb.gtkw b/fpga/communication_tb.gtkw
index 4707e3b..a9c81c2 100644
--- a/fpga/communication_tb.gtkw
+++ b/fpga/communication_tb.gtkw
@@ -1,41 +1,52 @@
 [*]
 [*] GTKWave Analyzer v3.3.86 (w)1999-2017 BSI
-[*] Sun Feb 25 17:14:08 2018
+[*] Mon Feb 26 17:06:42 2018
 [*]
-[dumpfile] "/home/geoffrey/Documents/Polytech/Robotech/2017-2018/CdF/cdf2018-principal/fpga/build/communication_tb.vcd"
-[dumpfile_mtime] "Sun Feb 25 17:12:53 2018"
-[dumpfile_size] 4479
+[dumpfile] "/home/geoffrey/Documents/Polytech/Robotech/2017-2018/CdF/cdf2018-principal/fpga/build/communication_tb.ghw"
+[dumpfile_mtime] "Mon Feb 26 17:03:51 2018"
+[dumpfile_size] 3315
 [savefile] "/home/geoffrey/Documents/Polytech/Robotech/2017-2018/CdF/cdf2018-principal/fpga/communication_tb.gtkw"
 [timestart] 0
 [size] 1680 1012
 [pos] -1 -1
-*-27.785210 1133000000 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1
+*-28.533670 586000000 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1
+[treeopen] top.
+[treeopen] top.communication_tb.
+[treeopen] top.communication_tb.dut.
 [sst_width] 213
 [signals_width] 118
 [sst_expanded] 1
-[sst_vpaned_height] 244
+[sst_vpaned_height] 200
 @28
-clock
-reset
-@820
-[color] 2
-rxdata[7:0]
-@22
-[color] 2
-rxdata[7:0]
-@28
-[color] 2
-rxstb
-@820
-[color] 5
-txdata[7:0]
+top.communication_tb.clock
+top.communication_tb.reset
 @22
 [color] 5
-txdata[7:0]
+#{top.communication_tb.rxdata[7:0]} top.communication_tb.rxdata[7] top.communication_tb.rxdata[6] top.communication_tb.rxdata[5] top.communication_tb.rxdata[4] top.communication_tb.rxdata[3] top.communication_tb.rxdata[2] top.communication_tb.rxdata[1] top.communication_tb.rxdata[0]
+@820
+[color] 5
+#{top.communication_tb.rxdata[7:0]} top.communication_tb.rxdata[7] top.communication_tb.rxdata[6] top.communication_tb.rxdata[5] top.communication_tb.rxdata[4] top.communication_tb.rxdata[3] top.communication_tb.rxdata[2] top.communication_tb.rxdata[1] top.communication_tb.rxdata[0]
 @28
 [color] 5
-txstb
-[color] 5
-txack
+top.communication_tb.dut.rxstb
+@420
+[color] 6
+top.communication_tb.dut.readstate
+[color] 6
+top.communication_tb.dut.readoffset
+@421
+[color] 2
+top.communication_tb.dut.a
+@22
+[color] 1
+#{top.communication_tb.txdata[7:0]} top.communication_tb.txdata[7] top.communication_tb.txdata[6] top.communication_tb.txdata[5] top.communication_tb.txdata[4] top.communication_tb.txdata[3] top.communication_tb.txdata[2] top.communication_tb.txdata[1] top.communication_tb.txdata[0]
+@820
+[color] 1
+#{top.communication_tb.txdata[7:0]} top.communication_tb.txdata[7] top.communication_tb.txdata[6] top.communication_tb.txdata[5] top.communication_tb.txdata[4] top.communication_tb.txdata[3] top.communication_tb.txdata[2] top.communication_tb.txdata[1] top.communication_tb.txdata[0]
+@28
+[color] 1
+top.communication_tb.dut.txstb
+[color] 1
+top.communication_tb.dut.txack
 [pattern_trace] 1
 [pattern_trace] 0
diff --git a/fpga/communication_tb.vhd b/fpga/communication_tb.vhd
index 0c25fbc..28598e6 100644
--- a/fpga/communication_tb.vhd
+++ b/fpga/communication_tb.vhd
@@ -6,7 +6,7 @@ library ieee;
 use ieee.std_logic_1164.all;
 
 entity communication_tb is
-end communication_tb;
+    end communication_tb;
 
 architecture tb of communication_tb is
 
@@ -78,7 +78,7 @@ begin
 
 
         -- Test Ping
-        report "Receiving 'P'" severity note;
+        report "TEST Receiving 'P'" severity note;
         rxData <= x"50";
         rxStb <= '1';
         wait for TbPeriod;
@@ -101,7 +101,7 @@ begin
 
 
         -- Test unknown char
-        report "Receiving '?'" severity note;
+        report "TEST Receiving '?'" severity note;
         rxData <= x"3F";
         rxStb <= '1';
         wait for TbPeriod;
@@ -131,6 +131,33 @@ begin
         assert txStb = '0' report "Not stopping send" severity error;
         wait for 100 ns; -- Margin
 
+        -- Test queue
+        report "TEST Receiving 'P' three times" severity note;
+        rxData <= x"50";
+        for I in 0 to 2 loop
+            rxStb <= '1';
+            wait for TbPeriod;
+            rxStb <= '0';
+            wait for 100 ns;
+        end loop;
+
+        for I in 0 to 2 loop
+            wait for 100 ns;
+            assert txData = x"50" report "Not sent 'P'" severity error;
+            assert txStb = '1' report "Not sending" severity error;
+
+            report "Acknowledging send" severity note;
+            wait for 100 ns;
+            txAck <= '1';
+            wait for TbPeriod;
+            txAck <= '0';
+        end loop;
+
+        wait for 100 ns;
+        assert txStb = '0' report "Not stopping send" severity error;
+
+        wait for 100 ns; -- Margin
+
 
         TbSimEnded <= '1';
         wait;