diff --git a/sbs.conf b/sbs.conf
index ac0a64604418413bea0d86a2a94d1923f7f34ee8..5955de38670fe0a9e01f1d4c71d9d4b97bd52833 100644
--- a/sbs.conf
+++ b/sbs.conf
@@ -97,9 +97,9 @@ Type:       srcfiles
 Files:      src/shared/boards/AnakinBoard.cpp
             src/shared/drivers/Leds.cpp
 
-[dsgamma]
+[gamma868]
 Type:       srcfiles
-Files:      src/shared/drivers/dsgamma/dsgamma-serial.cpp
+Files:      src/shared/drivers/gamma868/Gamma868.cpp
 
 
 # Boards 
@@ -203,6 +203,14 @@ Main:       test-piksi
 Type:       board
 BoardId:    stm32f429zi_stm32f4discovery
 BinName:    dsgamma-config
-Include:    %dsgamma
+Include:    %gamma868
 Defines:   
-Main:       dsgamma-config
\ No newline at end of file
+Main:       dsgamma-config
+
+[dsgamma-test]
+Type:       board
+BoardId:    stm32f429zi_stm32f4discovery
+BinName:    dsgamma-test
+Include:    %gamma868
+Defines:   
+Main:       dsgamma-test
\ No newline at end of file
diff --git a/src/entrypoints/dsgamma-config.cpp b/src/entrypoints/dsgamma-config.cpp
index 635935f47a3c2682a62732d3552c7ae647b7ed6b..beca1e74478511f5d951b6df7be743f6b38a572d 100644
--- a/src/entrypoints/dsgamma-config.cpp
+++ b/src/entrypoints/dsgamma-config.cpp
@@ -1,11 +1,23 @@
-/*
- * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
- * !! NEEDS AN OPEN auxtty@9600       !!
- * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+/* Copyright (c) 2017 Skyward Experimental Rocketry
+ * Authors: Alvise de'Faveri Tron, Nuno Barcellos
+ * 
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ * 
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
  * 
- * 1. Connect the device's RX and TX to the auxtty usart pins (USART3: PB9 tx, PB10 rx)
- * 2. Connect device's LRN SW pin to learnSwitch and LRN LED to learnAck (default PB2, PB0)
- * 3. Connect to the default serial port to read/write (PA10 tx, PA11 rx)
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
  */
 
 #include <cstdio>
@@ -18,6 +30,7 @@
 using namespace std;
 using namespace miosix;
 
+//GAMMA868 configuration status
 struct config{
     int local_addr[3] = {127, 127, 127};
     int dest_addr[3] = {127, 127, 127};
@@ -27,12 +40,12 @@ struct config{
     int baudrate = 4;
 };
 
-//STM32F429 specific
+//PIN CONFIG
 typedef Gpio<GPIOG_BASE,13> greenLed;
 typedef Gpio<GPIOG_BASE,14> redLed;
+typedef Gpio<GPIOA_BASE,0> button;
 typedef Gpio<GPIOB_BASE,2> learnSwitch;
 typedef Gpio<GPIOB_BASE,0> learnAck;
-typedef Gpio<GPIOA_BASE,0> button;
 
 //Needed to check if learn mode is really active.
 FastMutex learnMutex;          
@@ -42,6 +55,7 @@ int learnMode=0;
 //Serial port
 int fd = -1;
 
+//Functions
 int initialize();
 int enterLearnMode();
 void confirmLearnMode(void *arg);
@@ -52,6 +66,18 @@ int writeConfig(config conf);
 void waitForButton();
 void waitForOk();
 
+/*
+ * GAMMA868 Configuration Software:
+ * This software is intended for stm32f429i discovery boards. 
+ * 
+ * Discovery board should be connected as follows
+ * - Default USART(default @19200 baud)     Anything with a keyboard.
+ * - Auxtty USART(must be @9600 baud)       Gamma868 SERIAL(pin 18: rx, 17:tx)
+ * - learnSwitch (see pin config)           Gamma868 LRN SW(pin 5)
+ * - learnAck (see pin config)              Gamma868 LRN LED(pin 6)
+ * 
+ * Gamma module must be in SERIAL MODEM mode (see documentation).
+ */
 int main(){
     
     printf("\n----- GAMMA868 CONFIGURATOR -----\n");
@@ -137,7 +163,6 @@ int initialize(){
         printf("Ok\n");
         return 0;
     }
-    
 }
 
 /*
@@ -153,6 +178,7 @@ int enterLearnMode(){
     Thread *checkLearnModeThread, *timeoutThread;
     checkLearnModeThread = Thread::create(confirmLearnMode,STACK_MIN);
     timeoutThread = Thread::create(timer,STACK_MIN);
+    
     if(checkLearnModeThread == NULL || timeoutThread == NULL){
         printf("Failed: learn mode control threads not created.\n");
         return -1;
@@ -194,7 +220,7 @@ void printConfig(){
 }
 
 /*
- * Sends new configuration to the gamma868 module.
+ * Sends configuration to the gamma868 module.
  */
 int writeConfig(struct config conf){
     //TODO check values before writing 
diff --git a/src/entrypoints/dsgamma-test.cpp b/src/entrypoints/dsgamma-test.cpp
index 55192919a98d6517238ed04890c50b63fbfc5cf8..05703aed9f9a2f47f4e45212decf9ac6a0aba1a4 100644
--- a/src/entrypoints/dsgamma-test.cpp
+++ b/src/entrypoints/dsgamma-test.cpp
@@ -1,19 +1,90 @@
-/*
- * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
- * !! NEEDS AN OPEN auxtty@57600      !!
- * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+/* Copyright (c) 2017 Skyward Experimental Rocketry
+ * Authors: Alvise de'Faveri Tron
+ * 
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ * 
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
  */
 
 #include <cstdio>
 #include "miosix.h"
+#include <drivers/gamma868/Gamma868.h>
 #include <string.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
 
 using namespace std;
 using namespace miosix;
 
-int main() {
+typedef Gpio<GPIOG_BASE,13> greenLed;
+typedef Gpio<GPIOG_BASE,14> redLed;
+typedef Gpio<GPIOA_BASE,0> button;
+
+int ping = 0;
+Gamma868 gamma("/dev/auxtty"); 
 
+void sender(void *arg);
+void receiver();
+int main() {
+    
+    //Discovery gpio setup
+    {
+        FastInterruptDisableLock dLock;
+        greenLed::mode(Mode::OUTPUT);
+        redLed::mode(Mode::OUTPUT);
+        button::mode(Mode::INPUT);
+    }
+    
+    Thread::create(sender, STACK_MIN);
+    
+    receiver();
+    
     
     return 0;
 }
 
+void sender(void *arg){
+    while(1){
+        while(1){
+            if(button::value()==1) break;       //Wait for button
+        }
+        
+        printf("Writing... \n");
+        char msg[2];
+        msg[0] = '#';
+        msg[1] = '?';
+        gamma.send(msg);
+        Thread::sleep(200);
+    }
+}
+
+void receiver(){
+    while(1){
+        //read 
+        int conflen = 5;
+        char config[conflen];
+        printf("Reading: \n");
+        gamma.waitFor(conflen, config);
+        
+        for(int i = 0; i < conflen ; i++){
+            printf("Received: %02X\n", config[i]);
+        }
+        
+        //Thread::sleep(200);
+    }
+}
\ No newline at end of file
diff --git a/src/entrypoints/test-dsgamma-serial.cpp b/src/entrypoints/test-dsgamma-serial.cpp
deleted file mode 100644
index 05585f766512d1a7257c220fa69e2d62cd2d8c69..0000000000000000000000000000000000000000
--- a/src/entrypoints/test-dsgamma-serial.cpp
+++ /dev/null
@@ -1,57 +0,0 @@
-
-#include <cstdio>
-#include "miosix.h"
-//#include <pthread.h> 
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-
-
-using namespace std;
-using namespace miosix;
-
-typedef Gpio<GPIOD_BASE,12> greenLed;
-typedef Gpio<GPIOD_BASE,14> redLed;
-
-void thread1(void *arg);
-
-int main(){
-     
-    {
-        FastInterruptDisableLock dLock;
-        greenLed::mode(Mode::OUTPUT);
-        redLed::mode(Mode::OUTPUT);
-    }
-    
-    int fd = open("/dev/auxtty",O_RDWR);
-    if(fd<0) printf("Error opening serial test port\n");
-    
-    Thread *t;
-    t=Thread::create(thread1,STACK_MIN);
-    if(t==NULL) printf("Error: thread not created\n");
-    
-    for(;;){
-        greenLed::high();
-        Thread::sleep(200);
-        greenLed::low();
-        Thread::sleep(2000);
-        printf("Hey!\n");
-        
-        write(fd,"24",2);
-    }
-}
-
-void thread1(void *arg){
-    int i = 0;
-    for(;;){
-        redLed::high();
-        Thread::sleep(50);
-        redLed::low();
-        Thread::sleep(50);
-        i++;
-        if(i==4){
-            Thread::sleep(2000);
-            i = 0;
-        }
-    }
-}
\ No newline at end of file
diff --git a/src/shared/drivers/gamma868/Gamma868.cpp b/src/shared/drivers/gamma868/Gamma868.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..816a8a4fcf4fbf91fa0811c662d0e504eb2f5eda
--- /dev/null
+++ b/src/shared/drivers/gamma868/Gamma868.cpp
@@ -0,0 +1,72 @@
+/* Copyright (c) 2017 Skyward Experimental Rocketry
+ * Authors: Alvise de'Faveri Tron
+ * 
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ * 
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "Gamma868.h"
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <string.h>
+
+using namespace std;
+
+#ifdef _MIOSIX
+
+#include <miosix.h>
+
+using namespace miosix;
+
+#endif //_MIOSIX
+
+#define MAX_IN_BUFFER 100
+#define MAX_OUT_BUFFER 100
+#define CMD_LEN 3
+#define DATA_LEN 10
+
+Gamma868::Gamma868(const char *serialPath)
+{
+    fd=open(serialPath,O_RDWR);
+    if(fd<0) printf("Cannot open %s\n", serialPath);
+}
+
+bool Gamma868::send(const char *msg)
+{
+    //TODO output buffer and synchronize
+    int length = strlen(msg);
+    write(fd, msg, length);
+    return true;
+}
+
+bool Gamma868::waitFor(int bufLen, char *buf)
+{
+    //TODO synchronize
+    char received[bufLen];
+    read(fd, &received, bufLen);
+    
+    for(int i = 0; i < bufLen; i++){
+            printf("Received obj: %02X\n", received[i]);
+        }
+    for(int i = 0; i < bufLen; i++){
+        buf[i] = received[i];
+    }
+    return true;
+}
\ No newline at end of file
diff --git a/src/shared/drivers/gamma868/Gamma868.h b/src/shared/drivers/gamma868/Gamma868.h
new file mode 100644
index 0000000000000000000000000000000000000000..7e2514ac6e925c295171b30220a1fa4146ed14ad
--- /dev/null
+++ b/src/shared/drivers/gamma868/Gamma868.h
@@ -0,0 +1,62 @@
+/* Copyright (c) 2017 Skyward Experimental Rocketry
+ * Authors: Alvise de'Faveri Tron
+ * 
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ * 
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifndef GAMMA868_H
+#define GAMMA868_H
+
+#include <pthread.h>
+
+struct Configuration{
+    int local_addr[3] = {127, 127, 127};
+    int dest_addr[3] = {127, 127, 127};
+    int lora_mode = 6;
+    int lora_pow = 15;
+    int handshake = 0;
+    int baudrate = 4;
+};
+
+class Gamma868 {
+    public:
+        Gamma868(const char *serialPath);
+        bool send(const char *msg);
+        bool waitFor(int bufLen, char *buf);
+        //~Gamma868();
+        
+        //bool isConnected() checks if learn switch is pulled up
+        bool enterLearnMode();
+        char *readConfiguration();
+        bool configure(Configuration newConf);
+        void exitLearnMode();
+       
+    private:
+        int checkConfiguration(Configuration config);
+        int timeout(int millis);
+        
+        int fd;
+        //pthread_t thread;
+        pthread_mutex_t readingMutex;
+        pthread_cond_t readingCond;
+        pthread_mutex_t writingMutex;
+        pthread_cond_t writingCond;
+};
+
+#endif /* GAMMA868_H */
\ No newline at end of file
diff --git a/src/shared/drivers/gamma868/dsgamma-serial.cpp b/src/shared/drivers/gamma868/dsgamma-serial.cpp
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/src/shared/drivers/gamma868/dsgamma-serial.h b/src/shared/drivers/gamma868/dsgamma-serial.h
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000