diff --git a/main.c b/main.c
--- a/main.c
+++ b/main.c
@@ -29,7 +29,7 @@ uint32_t packet_sent=1;
 uint32_t packet_receive=1;
 
 
-char* itoa(int i, char b[]){
+char* itoa(int32_t i, char b[]){
     char const digit[] = "0123456789";
     char* p = b;
     if(i<0){
@@ -49,6 +49,13 @@ char* itoa(int i, char b[]){
     return b;
 }
 
+
+int32_t round(double x) {
+    if (x >= 0)
+        return (long) (x+0.5);
+    return (long) (x-0.5);
+}
+
 static __IO uint32_t TimingDelay;
 
 // Move to header file
@@ -118,9 +125,11 @@ int main(void)
         if(!sw_btn) {
             GPIO_ToggleBits(LED_STAT);
             if(!toggle) {
+                GPIO_SetBits(GPIOB, GPIO_Pin_13);
                 toggle = ! toggle;
             }
             else  {
+                GPIO_ResetBits(GPIOB, GPIO_Pin_13);
                 toggle = ! toggle;
             }    
         }
@@ -143,22 +152,66 @@ void process()
 {
     // Assert CS
     GPIO_ResetBits(MAX_CS);
-    Delay(50);
+    Delay(1);
 
     // This may not clock at all... might need to send 16 bits first
     SPI_I2S_SendData(SPI2, 0xAA); // send dummy data
     SPI_I2S_SendData(SPI2, 0xAA); // send dummy data
-    uint16_t retval = SPI_I2S_ReceiveData(SPI2);
+    uint16_t temp1 = SPI_I2S_ReceiveData(SPI2);
+//    SPI_I2S_SendData(SPI2, 0xAA); // send dummy data
+//    SPI_I2S_SendData(SPI2, 0xAA); // send dummy data
+    uint16_t temp2 = 0;//SPI_I2S_ReceiveData(SPI2);
+
+    // Deassert CS
+    Delay(1);
+    GPIO_SetBits(MAX_CS);
+
+    int32_t temp = (temp1 << 16) | temp2;
 
+    //////////////////////////
+    // Calc internal temp   //
+    //////////////////////////
+/*    temp = temp >> 4; // Drop last 4 bits, no need for them
+    float internal_temp = temp & 0x7FF // Lower 11bits are internal temp
+    
+    // Check internal temp sign
+    if(temp & 0x800) {
+        // Convert to negative value by extending sign and casting to signed type.
+        int16_t tmp = 0xF800 | (temp & 0x7FF);
+        internal_temp = tmp;
+    }
+    internal_temp *= 0.0625; // LSB = 0.0625 degrees
+    // Now we have a good internal temp!
+*/
+    //////////////////////////
+    // Calc external temp   //
+    //////////////////////////
+    if(temp & 0x7) {
+        // Something is wrong...
+        ssd1306_DrawString("!TempCOMMS", 3, 35);
+    }
+
+    if(temp & 0x80000000) {
+        // Negative value, drop the lower 18 bits and explicitly extend sign bits
+        temp = 0xFFFFC000 | ((temp >> 18) & 0x00003FFFF);
+    }
+    else {
+        // Positive value, just drop lower 18
+        temp >>= 18;
+    }
+
+    double temp_centigrade = temp;
+
+
+    if(temp > 0) {
+        GPIO_SetBits(LED_STAT);
+    }
     char tempstr[9];
-    itoa(retval, tempstr);
+    itoa(temp, tempstr);
     ssd1306_DrawString("Temp: ", 1, 40);
     ssd1306_DrawString(tempstr, 1, 70);
 
-
-    // Deassert CS
-    GPIO_SetBits(MAX_CS);
-
+/*
     if((!retval || (temp & 0x2) != 0))
     {
         ssd1306_DrawString("!TempCOMMS", 3, 35);
@@ -172,7 +225,7 @@ void process()
     }
 
     temp = (temp & 0x7FF8) >> 5;
-
+*/
 
     // TODO: Add calibration offset (linear)
 
@@ -390,6 +443,7 @@ void init_spi(void)
     SPI_InitTypeDef  SPI_InitStructure;
 
     // OLED IC
+    SPI_Cmd(SPI1, DISABLE); 
     SPI_InitStructure.SPI_Direction = SPI_Direction_1Line_Tx;
     SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
     SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
@@ -402,14 +456,16 @@ void init_spi(void)
     SPI_Init(SPI1, &SPI_InitStructure);
     SPI_Cmd(SPI1, ENABLE);           /* Enable the SPI  */   
 
+
     // MAX IC
-    SPI_InitStructure.SPI_Direction = SPI_Direction_1Line_Rx;
+    SPI_Cmd(SPI2, DISABLE); 
+    SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
     SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
     SPI_InitStructure.SPI_DataSize = SPI_DataSize_16b; // Andysworkshop
-    SPI_InitStructure.SPI_CPOL = SPI_CPOL_High; // From andysworkshop
-    SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge; // same
+    SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low; // From andysworkshop
+    SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge; // same
     SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
-    SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_128;
+    SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256;
     SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
     SPI_InitStructure.SPI_CRCPolynomial = 7;
     SPI_Init(SPI2, &SPI_InitStructure);
@@ -492,7 +548,6 @@ void init_gpio(void) {
   /*Enable or disable the AHB peripheral clock */
   RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
 
-
 // SPI PINSSS
 
   /*Configure GPIO pin : PB, MOSI, SCK */
@@ -502,22 +557,20 @@ void init_gpio(void) {
   GPIO_InitStruct.GPIO_Speed = GPIO_Speed_10MHz;
   GPIO_Init(GPIOB, &GPIO_InitStruct);
 
+ GPIO_InitTypeDef GPIO_InitStruct2;
 // MISO
-  GPIO_InitStruct.GPIO_Pin = GPIO_Pin_14;
-  GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN;
-  GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
-  GPIO_InitStruct.GPIO_Speed = GPIO_Speed_10MHz;
-  GPIO_Init(GPIOB, &GPIO_InitStruct);
+  GPIO_InitStruct2.GPIO_Pin = GPIO_Pin_14;
+  GPIO_InitStruct2.GPIO_Mode = GPIO_Mode_AF;
+  GPIO_InitStruct2.GPIO_PuPd = GPIO_PuPd_NOPULL;
+  GPIO_InitStruct2.GPIO_Speed = GPIO_Speed_10MHz;
+  GPIO_Init(GPIOB, &GPIO_InitStruct2);
 
 
-  /*Configure GPIO pin alternate function */
+  //Configure GPIO pin alternate function 
   GPIO_PinAFConfig(GPIOB, GPIO_PinSource13, GPIO_AF_SPI2);
+  GPIO_PinAFConfig(GPIOB, GPIO_PinSource14, GPIO_AF_SPI2);
+  GPIO_PinAFConfig(GPIOB, GPIO_PinSource15, GPIO_AF_SPI2);
 
-  /*Configure GPIO pin alternate function */
-  GPIO_PinAFConfig(GPIOB, GPIO_PinSource14, GPIO_AF_SPI2);
-
-  /*Configure GPIO pin alternate function */
-  GPIO_PinAFConfig(GPIOB, GPIO_PinSource15, GPIO_AF_SPI2);
 
   /** USB GPIO Configuration  
   PA11   ------> USB_DM