Simulator: UART, Leds, begining of GPS

25 files changed, 681 insertions, 477 deletions

diff --git a/src/common/includes/GPIO/leds.h b/src/common/includes/GPIO/leds.h
new file mode 100644
index 0000000..70c57d2
--- /dev/null
+++ b/src/common/includes/GPIO/leds.h
@@ -0,0 +1,9 @@
+#pragma once
+
+#define LED_GREEN 1
+#define LED_ORANGE 2
+#define LED_RED 3
+#define LED_BLUE 4
+
+void leds_turn_off(int);
+void leds_turn_on(int);
diff --git a/src/common/includes/Core/usart.h b/src/common/includes/GPIO/usart.h
index 9d9b59c..62c86c9 100644
--- a/src/common/includes/Core/usart.h
+++ b/src/common/includes/GPIO/usart.h
@@ -1,7 +1,7 @@
 /*
  * The MIT License (MIT)
  *
- * Copyright (c) 2016 Matthias P. Braendli
+ * Copyright (c) 2016 Matthias P. Braendli, Maximilien Cuony
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -53,5 +53,12 @@ void usart_debug_puts(const char* str);
 // Return 1 on success
 int usart_get_nmea_sentence(char* nmea);
 
+void usart_debug_timestamp();
+
+void usart_gps_specific_init();
+
+void usart_process_char(char);
+void usart_gps_process_char(char);
+
 #endif //__USART_H_
 
diff --git a/src/fsm/gps.h b/src/common/includes/GPS/gps.h
index 3472237..768ade1 100644
--- a/src/fsm/gps.h
+++ b/src/common/includes/GPS/gps.h
@@ -43,4 +43,3 @@ int gps_locked();
 // Get current time from GPS
 // Returns 1 if time is valid, 0 otherwise
 int gps_utctime(struct tm *timeutc);
-
diff --git a/src/fsm/minmea.h b/src/common/includes/GPS/minmea.h
index 6d7fe18..6d7fe18 100644
--- a/src/fsm/minmea.h
+++ b/src/common/includes/GPS/minmea.h
diff --git a/src/common/src/Core/common.c b/src/common/src/Core/common.c
index 2eee88f..8c63917 100644
--- a/src/common/src/Core/common.c
+++ b/src/common/src/Core/common.c
@@ -23,7 +23,7 @@
 */
 
 #include "Core/common.h"
-#include "Core/usart.h"
+#include "GPIO/usart.h"
 #include "FreeRTOS.h"
 #include "timers.h"
 /* #include "Core/gps.h" */
@@ -118,7 +118,7 @@ int local_time(struct tm *time)
 {
     const int local_time_offset=1; // hours
 
-    int valid = 0; // TODO gps_utctime(time);
+    int valid = gps_utctime(time);
 
     if (valid) {
         time->tm_hour += local_time_offset;
diff --git a/src/common/src/Core/main.c b/src/common/src/Core/main.c
index ac41355..feac593 100644
--- a/src/common/src/Core/main.c
+++ b/src/common/src/Core/main.c
@@ -38,12 +38,13 @@
 /* #include "cw.h" */
 /* #include "pio.h" */
 /* #include "i2c.h" */
-/* #include "gps.h" */
+#include "GPS/gps.h"
 /* #include "fsm.h" */
-/* #include "common.h" */
-#include "Core/usart.h"
+#include "Core/common.h"
+#include "GPIO/usart.h"
 /* #include "delay.h" */
 /* #include "temperature.h" */
+#include "GPIO/leds.h"
 #include "vc.h"
 
 
@@ -70,10 +71,15 @@ void vApplicationStackOverflowHook( TaskHandle_t xTask,
     while (1) {};
 }
 
+void * threadscheduler(void * arg) {
+    /* Start the RTOS Scheduler */
+    vTaskStartScheduler();
+}
+
 int main(void) {
     init();
     /* delay_init(); */
-    /* usart_init(); */
+    usart_init();
     usart_debug_puts("\r\n******* glutt-o-matique version " GIT_VERSION " *******\r\n");
     /*  */
     /* if (RCC_GetFlagStatus(RCC_FLAG_IWDGRST) != RESET) */
@@ -82,26 +88,54 @@ int main(void) {
     /* } */
     /* RCC_ClearFlag(); */
     /*  */
-    /* TaskHandle_t task_handle; */
-    /* xTaskCreate( */
-    /*         launcher_task, */
-    /*         "Launcher", */
-    /*         configMINIMAL_STACK_SIZE, */
-    /*         (void*) NULL, */
-    /*         tskIDLE_PRIORITY + 2UL, */
-    /*         &task_handle); */
-    /*  */
-    /* if (!task_handle) { */
-    /*     trigger_fault(FAULT_SOURCE_MAIN); */
-    /* } */
-    /*  */
-    /* #<{(| Start the RTOS Scheduler |)}># */
-    /* vTaskStartScheduler(); */
-    /*  */
-    /* #<{(| HALT |)}># */
+    TaskHandle_t task_handle;
+    xTaskCreate(
+            launcher_task,
+            "Launcher",
+            configMINIMAL_STACK_SIZE,
+            (void*) NULL,
+            tskIDLE_PRIORITY + 2UL,
+            &task_handle);
+
+    if (!task_handle) {
+        trigger_fault(FAULT_SOURCE_MAIN);
+    }
+
+    vTaskStartScheduler();
+    /* pthread_t pth; */
+    /* pthread_create(&pth, NULL, threadscheduler, "processing..."); */
+
+    /* HALT */
     while(1);
 }
 
+
+static void test_task(void *pvParameters) {
+
+    int i = 0;
+
+    while(1) {
+        vTaskDelay(1000 / portTICK_RATE_MS);
+
+        if (i == 0) {
+            i = 1;
+            leds_turn_on(LED_RED);
+
+        } else {
+            i = 0;
+            leds_turn_off(LED_RED);
+        }
+
+        char * poney = "$GNRMC,202140.00,A,4719.59789,N,00830.92084,E,0.012,,310516,,,A*65\n";
+        gps_usart_send(poney);
+        /* char * poney2 = "$GNRMC,202141.00,A,4719.59788,N,00830.92085,E,0.012,,310516,,,A*64"; */
+        /* gps_usart_send(poney2); */
+        /* char * poney3 = "$GNRMC,202142.00,A,4719.59785,N,00830.92087,E,0.010,,310516,,,A*6A"; */
+        /* gps_usart_send(poney3); */
+    }
+
+}
+
 // Launcher task is here to make sure the scheduler is
 // already running when calling the init functions.
 static void launcher_task(void *pvParameters)
@@ -118,15 +152,15 @@ static void launcher_task(void *pvParameters)
     /* usart_debug_puts("common init\r\n"); */
     /* common_init(); */
     /*  */
-    /* usart_debug_puts("GPS init\r\n"); */
-    /* gps_init(); */
+    usart_debug_puts("GPS init\r\n");
+    gps_init();
     /*  */
     /* usart_debug_puts("DS18B20 init\r\n"); */
     /* temperature_init(); */
     /*  */
     /* usart_debug_puts("TaskButton init\r\n"); */
     /*  */
-    /* TaskHandle_t task_handle; */
+    TaskHandle_t task_handle;
     /* xTaskCreate( */
     /*         detect_button_press, */
     /*         "TaskButton", */
@@ -153,20 +187,20 @@ static void launcher_task(void *pvParameters)
     /*     trigger_fault(FAULT_SOURCE_MAIN); */
     /* } */
     /*  */
-    /* usart_debug_puts("TaskGPS init\r\n"); */
-    /*  */
-    /* xTaskCreate( */
-    /*         gps_monit_task, */
-    /*         "TaskGPSMonit", */
-    /*         4*configMINIMAL_STACK_SIZE, */
-    /*         (void*) NULL, */
-    /*         tskIDLE_PRIORITY + 2UL, */
-    /*         &task_handle); */
-    /*  */
-    /* if (!task_handle) { */
-    /*     trigger_fault(FAULT_SOURCE_MAIN); */
-    /* } */
-    /*  */
+    usart_debug_puts("TaskGPS init\r\n");
+
+    xTaskCreate(
+            gps_monit_task,
+            "TaskGPSMonit",
+            4*configMINIMAL_STACK_SIZE,
+            (void*) NULL,
+            tskIDLE_PRIORITY + 2UL,
+            &task_handle);
+
+    if (!task_handle) {
+        trigger_fault(FAULT_SOURCE_MAIN);
+    }
+
     /* usart_debug_puts("Audio init\r\n"); */
     /*  */
     /* InitializeAudio(Audio16000HzSettings); */
@@ -179,19 +213,29 @@ static void launcher_task(void *pvParameters)
     /*  */
     /* // By default, let's the audio off to save power */
     /* AudioOff(); */
-    /*  */
-    /* usart_debug_puts("Init done.\r\n"); */
-    /*  */
-    /* #<{(| We are done now, suspend this task */
-    /*  * With FreeDOS' heap_1.c, we cannot delete it. */
-    /*  * See freertos.org -> More Advanced... -> Memory Management */
-    /*  * for more info. */
-    /*  |)}># */
-    /* while (1) { */
-    /*     vTaskSuspend(NULL); */
-    /* } */
+
+    usart_debug_puts("Init done.\r\n");
+
+    xTaskCreate(
+            test_task,
+            "TaskFSM",
+            4*configMINIMAL_STACK_SIZE,
+            (void*) NULL,
+            tskIDLE_PRIORITY + 2UL,
+            &task_handle);
+
+    /* We are done now, suspend this task
+     * With FreeDOS' heap_1.c, we cannot delete it.
+     * See freertos.org -> More Advanced... -> Memory Management
+     * for more info.
+     */
+
+    while (1) {
+        vTaskSuspend(NULL);
+    }
 }
 
+
 static void detect_button_press(void *pvParameters)
 {
     /* int pin_high_count = 0; */
@@ -264,50 +308,52 @@ static void audio_callback(void* context, int select_buffer)
     /* ProvideAudioBufferWithoutBlocking(samples, samples_len); */
 }
 
-/* static struct tm gps_time; */
+static struct tm gps_time;
 static void gps_monit_task(void *pvParameters) {
-/*     GPIO_SetBits(GPIOD, GPIOD_BOARD_LED_BLUE); */
-/*  */
-/*     int t_gps_print_latch = 0; */
-/*  */
-/*     while (1) { */
-/*         struct tm time; */
-/*         int time_valid = local_time(&time); */
-/*  */
-/*         if (time_valid) { */
-/*             if (time.tm_sec % 4 >= 2) { */
-/*                 GPIO_SetBits(GPIOD, GPIOD_BOARD_LED_BLUE); */
-/*             } */
-/*             else { */
-/*                 GPIO_ResetBits(GPIOD, GPIOD_BOARD_LED_BLUE); */
-/*             } */
-/*  */
-/*             // Even hours: tm_trigger=1, odd hours: tm_trigger=0 */
-/*             tm_trigger = (time.tm_hour + 1) % 2; */
-/*         } */
-/*  */
-/*         gps_utctime(&gps_time); */
-/*  */
-/*         if (gps_time.tm_sec % 30 == 0 && t_gps_print_latch == 0) { */
-/*             usart_debug("T_GPS %04d-%02d-%02d %02d:%02d:%02d\r\n", */
-/*                     gps_time.tm_year, gps_time.tm_mon, gps_time.tm_mday, */
-/*                     gps_time.tm_hour, gps_time.tm_min, gps_time.tm_sec); */
-/*  */
-/*             usart_debug("TIME  %04d-%02d-%02d %02d:%02d:%02d\r\n", */
-/*                     time.tm_year, time.tm_mon, time.tm_mday, */
-/*                     time.tm_hour, time.tm_min, time.tm_sec); */
-/*  */
-/*             t_gps_print_latch = 1; */
-/*         } */
-/*         if (gps_time.tm_sec % 30 > 0) { */
-/*             t_gps_print_latch = 0; */
-/*         } */
-/*  */
-/*         vTaskDelay(100 / portTICK_RATE_MS); */
-/*  */
-/*         // Reload watchdog */
-/*         IWDG_ReloadCounter(); */
-/*     } */
+
+    leds_turn_on(LED_BLUE);
+
+    int t_gps_print_latch = 0;
+
+    while (1) {
+        struct tm time;
+        int time_valid = local_time(&time);
+
+        if (time_valid) {
+            printf("Valid\n");
+            if (time.tm_sec % 4 >= 2) {
+                leds_turn_on(LED_BLUE);
+            }
+            else {
+                leds_turn_off(LED_BLUE);
+            }
+
+            // Even hours: tm_trigger=1, odd hours: tm_trigger=0
+            tm_trigger = (time.tm_hour + 1) % 2;
+        }
+
+        gps_utctime(&gps_time);
+
+        if (gps_time.tm_sec % 30 == 0 && t_gps_print_latch == 0) {
+            usart_debug("T_GPS %04d-%02d-%02d %02d:%02d:%02d\r\n",
+                    gps_time.tm_year, gps_time.tm_mon, gps_time.tm_mday,
+                    gps_time.tm_hour, gps_time.tm_min, gps_time.tm_sec);
+
+            usart_debug("TIME  %04d-%02d-%02d %02d:%02d:%02d\r\n",
+                    time.tm_year, time.tm_mon, time.tm_mday,
+                    time.tm_hour, time.tm_min, time.tm_sec);
+
+            t_gps_print_latch = 1;
+        }
+        if (gps_time.tm_sec % 30 > 0) {
+            t_gps_print_latch = 0;
+        }
+
+        vTaskDelay(100 / portTICK_RATE_MS);
+
+        // Reload watchdog //TODO
+        /* IWDG_ReloadCounter(); */
+    }
 }
 
 /* static struct fsm_input_signals_t fsm_input; */
diff --git a/src/common/src/GPIO/leds.c b/src/common/src/GPIO/leds.c
new file mode 100644
index 0000000..df2adea
--- /dev/null
+++ b/src/common/src/GPIO/leds.c
@@ -0,0 +1 @@
+#include "../../../common/includes/GPIO/leds.h"
diff --git a/src/common/src/GPIO/usart.c b/src/common/src/GPIO/usart.c
new file mode 100644
index 0000000..77272f8
--- /dev/null
+++ b/src/common/src/GPIO/usart.c
@@ -0,0 +1,136 @@
+#include <stdio.h>
+#include <stdarg.h>
+#include <string.h>
+#include <inttypes.h>
+#include "Core/common.h"
+#include "GPIO/usart.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "queue.h"
+
+// The ISR writes into this buffer
+static char nmea_sentence[MAX_NMEA_SENTENCE_LEN];
+static int  nmea_sentence_last_written = 0;
+
+
+// Once a completed NMEA sentence is received in the ISR,
+// it is appended to this queue
+static QueueHandle_t usart_nmea_queue;
+
+void usart_gps_init() {
+    usart_nmea_queue = xQueueCreate(15, MAX_NMEA_SENTENCE_LEN);
+    if (usart_nmea_queue == 0) {
+        while(1); /* fatal error */
+    }
+
+    usart_gps_specific_init();
+
+}
+
+void usart_gps_puts(const char* str) {
+    vTaskSuspendAll();
+    return usart_puts(USART3, str);
+    xTaskResumeAll();
+}
+
+#define MAX_MSG_LEN 80
+static char usart_debug_message[MAX_MSG_LEN];
+
+void usart_debug_timestamp() {
+    // Don't call printf here, to reduce stack usage
+    uint64_t now = timestamp_now();
+    if (now == 0) {
+        usart_puts(USART2, "[0] ");
+    }
+    else {
+        char ts_str[64];
+        int i = 63;
+
+        ts_str[i--] = '\0';
+        ts_str[i--] = ' ';
+        ts_str[i--] = ']';
+
+        while (now > 0 && i >= 0) {
+            ts_str[i--] = '0' + (now % 10);
+            now /= 10;
+        }
+        ts_str[i] = '[';
+
+        usart_puts(USART2, &ts_str[i]);
+    }
+}
+
+void usart_debug(const char *format, ...) {
+    va_list list;
+    va_start(list, format);
+    vsnprintf(usart_debug_message, MAX_MSG_LEN-1, format, list);
+
+    vTaskSuspendAll();
+    usart_debug_timestamp();
+    usart_puts(USART2, usart_debug_message);
+    xTaskResumeAll();
+
+    va_end(list);
+}
+
+void usart_debug_puts(const char* str) {
+    vTaskSuspendAll();
+    usart_debug_timestamp();
+    usart_puts(USART2, str);
+    xTaskResumeAll();
+}
+
+int usart_get_nmea_sentence(char* nmea) {
+    return xQueueReceive(usart_nmea_queue, nmea, portMAX_DELAY);
+}
+
+
+static void usart_clear_nmea_buffer(void) {
+    for (int i = 0; i < MAX_NMEA_SENTENCE_LEN; i++) {
+        nmea_sentence[i] = '\0';
+    }
+    nmea_sentence_last_written = 0;
+}
+
+void usart_process_char(char c) {
+
+    if (c == 'h') {
+        usart_debug_puts("help: no commands supported yet!\r\n");
+    } else {
+        usart_debug("Unknown command %c\r\n", c);
+    }
+
+}
+
+void usart_gps_process_char(char c) {
+
+    if (nmea_sentence_last_written == 0) {
+        if (c == '$') {
+            // Likely new start of sentence
+            nmea_sentence[nmea_sentence_last_written] = c;
+            nmea_sentence_last_written++;
+        }
+    }
+    else if (nmea_sentence_last_written < MAX_NMEA_SENTENCE_LEN) {
+        nmea_sentence[nmea_sentence_last_written] = c;
+        nmea_sentence_last_written++;
+
+        if (c == '\n') {
+            int success = xQueueSendToBackFromISR(
+                    usart_nmea_queue,
+                    nmea_sentence,
+                    NULL);
+
+            if (success == pdFALSE) {
+                trigger_fault(FAULT_SOURCE_USART);
+            }
+
+            usart_clear_nmea_buffer();
+        }
+    }
+    else {
+        // Buffer overrun without a meaningful NMEA message.
+        usart_clear_nmea_buffer();
+    }
+
+}
diff --git a/src/fsm/gps.c b/src/common/src/GPS/gps.c
index ea3e597..61c8c48 100644
--- a/src/fsm/gps.c
+++ b/src/common/src/GPS/gps.c
@@ -22,16 +22,14 @@
  * SOFTWARE.
 */
 
-#include "stm32f4xx_conf.h"
-#include "stm32f4xx.h"
 #include "FreeRTOS.h"
 #include "FreeRTOSConfig.h"
 #include "task.h"
 #include "semphr.h"
-#include "common.h"
-#include "gps.h"
-#include "usart.h"
-#include "minmea.h"
+#include "Core/common.h"
+#include "GPS/gps.h"
+#include "GPS/minmea.h"
+#include "GPIO/usart.h"
 
 
 TickType_t gps_timeutc_last_updated = 0;
@@ -45,8 +43,7 @@ static void gps_task(void *pvParameters);
 SemaphoreHandle_t timeutc_semaphore;
 
 // Get current time from GPS
-int gps_utctime(struct tm *timeutc)
-{
+int gps_utctime(struct tm *timeutc) {
     int valid = 0;
 
     xSemaphoreTake(timeutc_semaphore, portMAX_DELAY);
@@ -67,8 +64,7 @@ int gps_utctime(struct tm *timeutc)
 #define RXBUF_LEN MAX_NMEA_SENTENCE_LEN
 static char rxbuf[RXBUF_LEN];
 
-static void gps_task(void *pvParameters)
-{
+static void gps_task(void *pvParameters) {
     // Periodically reinit the GPS
     while (1) {
         taskYIELD();
@@ -101,18 +97,16 @@ static void gps_task(void *pvParameters)
     }
 }
 
-void gps_init()
-{
+void gps_init() {
     gps_timeutc_valid = 0;
 
     usart_gps_init();
 
     timeutc_semaphore = xSemaphoreCreateBinary();
 
-    if( timeutc_semaphore == NULL ) {
+    if (timeutc_semaphore == NULL) {
         trigger_fault(FAULT_SOURCE_GPS);
-    }
-    else {
+    } else {
         xSemaphoreGive(timeutc_semaphore);
     }
 
@@ -126,13 +120,10 @@ void gps_init()
 }
 
 // Return 1 of the GPS is receiving time
-int gps_locked()
-{
+int gps_locked() {
     if (xTaskGetTickCount() - gps_timeutc_last_updated < gps_data_validity_timeout) {
         return gps_timeutc_valid;
-    }
-    else {
+    } else {
         return 0;
     }
 }
-
diff --git a/src/fsm/minmea.c b/src/common/src/GPS/minmea.c
index fc6aa43..8700725 100644
--- a/src/fsm/minmea.c
+++ b/src/common/src/GPS/minmea.c
@@ -10,7 +10,7 @@
 // https://github.com/cloudyourcar/minmea-t/ README.md
 #define timegm mktime
 
-#include "minmea.h"
+#include "GPS/minmea.h"
 
 #include <stdlib.h>
 #include <string.h>
diff --git a/src/simulator/Makefile b/src/simulator/Makefile
index 4e74498..511b6e6 100644
--- a/src/simulator/Makefile
+++ b/src/simulator/Makefile
@@ -16,6 +16,8 @@ VPATH           += $(SRCROOT)/Source
 VPATH	        += $(SRCROOT)/Source/portable/MemMang
 VPATH	        += $(SRCROOT)/Source/portable/GCC/POSIX
 VPATH			+= $(SRCROOT)/src/Core
+VPATH			+= $(SRCROOT)/src/GPIO
+VPATH			+= $(SRCROOT)/src/GPS
 
 # FreeRTOS Objects
 C_FILES			+= croutine.c
diff --git a/src/simulator/src/Core/main.c b/src/simulator/src/Core/main.c
index 7e93176..a326dbf 100644
--- a/src/simulator/src/Core/main.c
+++ b/src/simulator/src/Core/main.c
@@ -27,11 +27,22 @@
 #include <pthread.h>
 
 
-void *threadFunc(void *arg) {
-    main2();
+static void thread_gui(void *arg) {
+    main_gui();
 }
 
 void init() {
-    pthread_t pth;
-    pthread_create(&pth, NULL, threadFunc, "processing...");
+
+    /* pthread_t pth; */
+    /* pthread_create(&pth, NULL, thread_gui, "processing..."); */
+
+    TaskHandle_t task_handle;
+    xTaskCreate(
+            thread_gui,
+            "Thread GUI",
+            configMINIMAL_STACK_SIZE,
+            (void*) NULL,
+            tskIDLE_PRIORITY + 2UL,
+            &task_handle);
+
 }
diff --git a/src/simulator/src/Core/usart.c b/src/simulator/src/Core/usart.c
deleted file mode 100644
index bd91428..0000000
--- a/src/simulator/src/Core/usart.c
+++ /dev/null
@@ -1,190 +0,0 @@
-
-/*
- * The MIT License (MIT)
- *
- * Copyright (c) 2016 Matthias P. Braendli
- *
- * 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 <stdio.h>
-#include <stdarg.h>
-#include <string.h>
-#include <inttypes.h>
-#include "Core/common.h"
-#include "Core/usart.h"
-#include "FreeRTOS.h"
-#include "task.h"
-#include "queue.h"
-
-// The ISR writes into this buffer
-static char nmea_sentence[MAX_NMEA_SENTENCE_LEN];
-static int  nmea_sentence_last_written = 0;
-
-// Once a completed NMEA sentence is received in the ISR,
-// it is appended to this queue
-static QueueHandle_t usart_nmea_queue;
-
-#define USART_TypeDef void
-
-int _USART2 = 2;
-int _USART3 = 3;
-
-#define USART2 &_USART2
-#define USART3 &_USART3
-
-void usart_init() {
-}
-
-void usart_gps_init() {
-    usart_nmea_queue = xQueueCreate(15, MAX_NMEA_SENTENCE_LEN);
-    if (usart_nmea_queue == 0) {
-        while(1); /* fatal error */
-    }
-
-}
-
-// Make sure Tasks are suspended when this is called!
-static void usart_puts(USART_TypeDef* USART, const char* str)
-{
-    while(*str) {
-        // TODO
-        putchar(*str);
-        str++;
-    }
-}
-
-void usart_gps_puts(const char* str)
-{
-    vTaskSuspendAll();
-    return usart_puts(USART3, str);
-    xTaskResumeAll();
-}
-
-#define MAX_MSG_LEN 80
-static char usart_debug_message[MAX_MSG_LEN];
-
-void usart_debug_timestamp() {
-    // Don't call printf here, to reduce stack usage
-    uint64_t now = timestamp_now();
-    if (now == 0) {
-        usart_puts(USART2, "[0] ");
-    }
-    else {
-        char ts_str[64];
-        int i = 63;
-
-        ts_str[i--] = '\0';
-        ts_str[i--] = ' ';
-        ts_str[i--] = ']';
-
-        while (now > 0 && i >= 0) {
-            ts_str[i--] = '0' + (now % 10);
-            now /= 10;
-        }
-        ts_str[i] = '[';
-
-        usart_puts(USART2, &ts_str[i]);
-    }
-}
-
-void usart_debug(const char *format, ...)
-{
-    va_list list;
-    va_start(list, format);
-    vsnprintf(usart_debug_message, MAX_MSG_LEN-1, format, list);
-
-    vTaskSuspendAll();
-    usart_debug_timestamp();
-    usart_puts(USART2, usart_debug_message);
-    xTaskResumeAll();
-
-    va_end(list);
-}
-
-void usart_debug_puts(const char* str)
-{
-    vTaskSuspendAll();
-    usart_debug_timestamp();
-    usart_puts(USART2, str);
-    xTaskResumeAll();
-}
-
-int usart_get_nmea_sentence(char* nmea)
-{
-    return xQueueReceive(usart_nmea_queue, nmea, portMAX_DELAY);
-}
-
-
-static void usart_clear_nmea_buffer(void)
-{
-    for (int i = 0; i < MAX_NMEA_SENTENCE_LEN; i++) {
-        nmea_sentence[i] = '\0';
-    }
-    nmea_sentence_last_written = 0;
-}
-
-/* void USART3_IRQHandler(void) */
-/* { */
-/*     if (USART_GetITStatus(USART3, USART_IT_RXNE)) { */
-/*         char t = USART3->DR; */
-/*  */
-/*         if (nmea_sentence_last_written == 0) { */
-/*             if (t == '$') { */
-/*                 // Likely new start of sentence */
-/*                 nmea_sentence[nmea_sentence_last_written] = t; */
-/*                 nmea_sentence_last_written++; */
-/*             } */
-/*         } */
-/*         else if (nmea_sentence_last_written < MAX_NMEA_SENTENCE_LEN) { */
-/*             nmea_sentence[nmea_sentence_last_written] = t; */
-/*             nmea_sentence_last_written++; */
-/*  */
-/*             if (t == '\n') { */
-/*                 int success = xQueueSendToBackFromISR( */
-/*                         usart_nmea_queue, */
-/*                         nmea_sentence, */
-/*                         NULL); */
-/*  */
-/*                 if (success == pdFALSE) { */
-/*                     trigger_fault(FAULT_SOURCE_USART); */
-/*                 } */
-/*  */
-/*                 usart_clear_nmea_buffer(); */
-/*             } */
-/*         } */
-/*         else { */
-/*             // Buffer overrun without a meaningful NMEA message. */
-/*             usart_clear_nmea_buffer(); */
-/*         } */
-/*     } */
-/* } */
-/*  */
-/* void USART2_IRQHandler(void) */
-/* { */
-/*     if (USART_GetITStatus(USART2, USART_IT_RXNE)) { */
-/*         char t = USART2->DR; */
-/*         if (t == 'h') { */
-/*             usart_debug_puts("help: no commands supported yet!\r\n"); */
-/*         } */
-/*         else { */
-/*             usart_debug("Unknown command %c\r\n", t); */
-/*         } */
-/*     } */
-/* } */
diff --git a/src/simulator/src/GPIO/leds.c b/src/simulator/src/GPIO/leds.c
new file mode 100644
index 0000000..874cc19
--- /dev/null
+++ b/src/simulator/src/GPIO/leds.c
@@ -0,0 +1,44 @@
+#include "../../../common/src/GPIO/leds.c"
+
+
+extern char led_blue;
+extern char led_green;
+extern char led_orange;
+extern char led_red;
+
+void leds_turn_on(int l) {
+
+    switch (l) {
+        case LED_GREEN:
+            led_green = 1;
+            break;
+        case LED_ORANGE:
+            led_orange = 1;
+            break;
+        case LED_RED:
+            led_red = 1;
+            break;
+        case LED_BLUE:
+            led_blue = 1;
+            break;
+
+    }
+}
+
+void leds_turn_off(int l) {
+
+    switch (l) {
+        case LED_GREEN:
+            led_green = 0;
+            break;
+        case LED_ORANGE:
+            led_orange = 0;
+            break;
+        case LED_RED:
+            led_red = 0;
+            break;
+        case LED_BLUE:
+            led_blue = 0;
+            break;
+    }
+}
diff --git a/src/simulator/src/GPIO/usart.c b/src/simulator/src/GPIO/usart.c
new file mode 100644
index 0000000..650602a
--- /dev/null
+++ b/src/simulator/src/GPIO/usart.c
@@ -0,0 +1,97 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2016 Maximilien Cuony
+ *
+ * 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.
+*/
+
+
+#define USART_TypeDef int
+
+int _USART2 = 2;
+int _USART3 = 3;
+
+#define USART2 &_USART2
+#define USART3 &_USART3
+
+static void usart_puts(USART_TypeDef*, const char*);
+
+#include "../../../common/includes/GPIO/usart.h"
+#include "../../../common/src/GPIO/usart.c"
+
+
+extern char uart_recv_txt[4096];
+int uart_recv_pointer = 0;
+
+
+void usart_move_buffer_up();
+
+
+void usart_init() {
+
+    // Zero buffer
+    for (int i = 0; i < 4096; i++) {
+        uart_recv_txt[i] = '\0';
+    }
+
+}
+
+void usart_gps_specific_init() {
+}
+
+void usart_move_buffer_up() {
+
+    for (int i = 0; i <= 4010; i++) {
+        uart_recv_txt[i] = uart_recv_txt[i + 10];
+        uart_recv_txt[i + 1] = '\0';
+    }
+
+    uart_recv_pointer -= 10;
+
+}
+
+// Make sure Tasks are suspended when this is called!
+static void usart_puts(USART_TypeDef* USART, const char* str) {
+
+    if (*USART == _USART2) {
+        while(*str) {
+            if (*str != '\r') {
+                uart_recv_txt[uart_recv_pointer+1] = '\0';
+                uart_recv_txt[uart_recv_pointer] = *str;
+                uart_recv_pointer++;
+
+                if (uart_recv_pointer >= 4000) {
+                    usart_move_buffer_up();
+                }
+            }
+            str++;
+        }
+    }
+}
+
+void gui_usart_send(char * string) {
+
+    while(*string) {
+        usart_process_char(*string);
+
+        string++;
+    }
+
+}
diff --git a/src/simulator/src/GPS/gps.c b/src/simulator/src/GPS/gps.c
new file mode 100644
index 0000000..0ca20f3
--- /dev/null
+++ b/src/simulator/src/GPS/gps.c
@@ -0,0 +1,11 @@
+#include "../../../common/src/GPS/gps.c"
+
+void gps_usart_send(char * string) {
+
+    while(*string) {
+        usart_gps_process_char(*string);
+
+        string++;
+    }
+
+}
diff --git a/src/simulator/src/GPS/minema.c b/src/simulator/src/GPS/minema.c
new file mode 100644
index 0000000..10df198
--- /dev/null
+++ b/src/simulator/src/GPS/minema.c
@@ -0,0 +1 @@
+#include "../../../common/src/GPS/minmea.c"
diff --git a/src/simulator/src/Gui/test.c b/src/simulator/src/Gui/gui.c
index f742fe8..81c80b6 100644
--- a/src/simulator/src/Gui/test.c
+++ b/src/simulator/src/Gui/gui.c
@@ -1,3 +1,7 @@
+#include "FreeRTOS.h"
+#include "FreeRTOSConfig.h"
+#include "task.h"
+
 #include <stdio.h>
 #include <stdlib.h>
 #include <stdint.h>
@@ -33,6 +37,22 @@
 #define LEN(a) (sizeof(a)/sizeof(a)[0])
 
 
+/**
+ * USART
+ **/
+char uart_recv_txt[4096];
+static char uart_send_txt[512];
+static int uart_send_txt_len;
+void gui_usart_send(char*);
+
+/**
+ * Leds
+ **/
+char led_blue = 0;
+char led_green = 0;
+char led_orange = 0;
+char led_red = 0;
+
 struct XWindow {
     Display *dpy;
     Window win;
@@ -81,7 +101,8 @@ static int has_extension(const char *string, const char *ext) {
     return FALSE;
 }
 
-int main2() {
+
+int main_gui() {
     /* Platform */
     int running = 1;
     struct XWindow win;
@@ -132,7 +153,9 @@ int main2() {
         {
             /* pick framebuffer with most samples per pixel */
             int i;
-            int fb_best = -1, best_num_samples = -1;
+            int fb_best = -1;
+            int best_num_samples = -1;
+
             for (i = 0; i < fb_count; ++i) {
                 XVisualInfo *vi = glXGetVisualFromFBConfig(win.dpy, fbc[i]);
                 if (vi) {
@@ -217,11 +240,12 @@ int main2() {
 
     background = nk_rgb(28,48,62);
 
-    static char box_buffer[512];
-    static int box_len;
-
     while (running)
     {
+        taskYIELD();
+
+        vTaskSuspendAll();
+
         /* Input */
         XEvent evt;
         nk_input_begin(ctx);
@@ -232,33 +256,124 @@ int main2() {
         nk_input_end(ctx);
 
         /* GUI */
-        {struct nk_panel layout;
-            if (nk_begin(ctx, &layout, "UART", nk_rect(50, 50, 400, 200), NK_WINDOW_BORDER|NK_WINDOW_MOVABLE|NK_WINDOW_SCALABLE|NK_WINDOW_MINIMIZABLE|NK_WINDOW_TITLE)) {
+        {
+
+            struct nk_panel layout;
+
+            if (nk_begin(ctx, &layout, "UART", nk_rect(50, 50, 400, 300), NK_WINDOW_BORDER|NK_WINDOW_MOVABLE|NK_WINDOW_SCALABLE|NK_WINDOW_MINIMIZABLE|NK_WINDOW_TITLE)) {
+
+
+                nk_menubar_begin(ctx);
+                nk_layout_row_begin(ctx, NK_STATIC, 25, 2);
+                nk_layout_row_push(ctx, 280);
+
+                int active = nk_edit_string(ctx, NK_EDIT_FIELD|NK_EDIT_SIG_ENTER, uart_send_txt, &uart_send_txt_len, 512,  nk_filter_default);
+
+                nk_layout_row_push(ctx, 70);
+                if (nk_button_label(ctx, "Send", NK_BUTTON_DEFAULT) || (active & NK_EDIT_COMMITED)) {
+
+                    uart_send_txt[uart_send_txt_len] = '\0';
+
+                    gui_usart_send(uart_send_txt);
+
+                    uart_send_txt[0] = '\0';
+                    uart_send_txt_len = 0;
+                }
+
+                nk_menubar_end(ctx);
 
                 nk_layout_row_dynamic(ctx, 25, 1);
 
                 nk_label(ctx, "UART Output:", NK_TEXT_LEFT);
 
-                nk_layout_row_dynamic(ctx, 75, 1);
-                nk_edit_string(ctx, NK_EDIT_BOX, box_buffer, &box_len, 512, nk_filter_default);
+                nk_layout_row_dynamic(ctx, 16, 1);
+
+                char * current_pointer = uart_recv_txt;
+                int l = 0;
 
-                /* #<{(| nk_layout_row(ctx, NK_STATIC, 25, 2, ratio); |)}># */
-                /* active = nk_edit_string(ctx, NK_EDIT_FIELD|NK_EDIT_SIG_ENTER, text[7], &text_len[7], 64,  nk_filter_ascii); */
-                /* if (nk_button_label(ctx, "Submit", NK_BUTTON_DEFAULT) || */
-                /*     (active & NK_EDIT_COMMITED)) */
-                /* { */
-                /*     text[7][text_len[7]] = '\n'; */
-                /*     text_len[7]++; */
-                /*     memcpy(&box_buffer[box_len], &text[7], (nk_size)text_len[7]); */
-                /*     box_len += text_len[7]; */
-                /*     text_len[7] = 0; */
-                /* } */
-                nk_layout_row_end(ctx);
+                while (*current_pointer != 0) {
 
-                /* nk_property_int(ctx, "Compression:", 0, &property, 100, 10, 1); */
+                    if (*current_pointer == '\n') {
+                        if (l > 1) {
+                            nk_text(ctx, current_pointer - l, l, NK_TEXT_LEFT);
+                        }
+                        current_pointer++;
+                        l = 0;
+                    }
+                    current_pointer++;
+                    l++;
+                }
+
+                if (l > 1) {
+                    nk_text(ctx, current_pointer - l, l, NK_TEXT_LEFT);
+                }
+
+                /* nk_layout_row_end(ctx); */
 
+
+                nk_end(ctx);
             }
-            nk_end(ctx);}
+
+
+            if (nk_begin(ctx, &layout, "LEDs", nk_rect(460, 50, 100, 155), NK_WINDOW_BORDER|NK_WINDOW_MOVABLE|NK_WINDOW_MINIMIZABLE|NK_WINDOW_TITLE)) {
+
+                nk_layout_row_static(ctx, 20, 20, 3);
+
+                struct nk_color color;
+
+
+                nk_text(ctx, "", 0, NK_TEXT_LEFT);
+
+                color.r = 255; color.g = 0; color.b = 0;
+
+                if (led_red == 1) {
+                    color.a = 255;
+                } else {
+                    color.a = 30;
+                }
+                nk_button_color(ctx, color, NK_BUTTON_DEFAULT);
+
+                nk_text(ctx, "", 0, NK_TEXT_LEFT);
+
+                color.r = 0; color.g = 255; color.b = 0;
+
+                if (led_green == 1) {
+                    color.a = 255;
+                } else {
+                    color.a = 30;
+                }
+                nk_button_color(ctx, color, NK_BUTTON_DEFAULT);
+
+                nk_text(ctx, "", 0, NK_TEXT_LEFT);
+
+                color.r = 255; color.g = 165; color.b = 0;
+
+                if (led_orange == 1) {
+                    color.a = 255;
+                } else {
+                    color.a = 30;
+                }
+                nk_button_color(ctx, color, NK_BUTTON_DEFAULT);
+
+                nk_text(ctx, "", 0, NK_TEXT_LEFT);
+
+                color.r = 0; color.g = 0; color.b = 255;
+
+                if (led_blue == 1) {
+                    color.a = 255;
+                } else {
+                    color.a = 30;
+                }
+                nk_button_color(ctx, color, NK_BUTTON_DEFAULT);
+
+                nk_text(ctx, "", 0, NK_TEXT_LEFT);
+
+                nk_end(ctx);
+
+            }
+
+
+        }
         /* if (nk_window_is_closed(ctx, "Demo")) break; */
 
         {
@@ -271,6 +386,8 @@ int main2() {
             nk_x11_render(NK_ANTI_ALIASING_ON, MAX_VERTEX_BUFFER, MAX_ELEMENT_BUFFER);
             glXSwapBuffers(win.dpy, win.win);
         }
+
+        xTaskResumeAll();
     }
 
     nk_x11_shutdown();
diff --git a/src/simulator/vc.h b/src/simulator/vc.h
index d8d7797..fa3f184 100644
--- a/src/simulator/vc.h
+++ b/src/simulator/vc.h
@@ -1,4 +1,4 @@
 // This file is generated by Makefile.
 // Do not edit this file!
-#define GIT_VERSION "4803231"
+#define GIT_VERSION "9069fc1"
 
diff --git a/src/stm32f/includes/GPIO/leds.h b/src/stm32f/includes/GPIO/leds.h
new file mode 100644
index 0000000..869a032
--- /dev/null
+++ b/src/stm32f/includes/GPIO/leds.h
@@ -0,0 +1,4 @@
+#define GPIOD_BOARD_LED_GREEN  GPIO_Pin_12
+#define GPIOD_BOARD_LED_ORANGE GPIO_Pin_13
+#define GPIOD_BOARD_LED_RED    GPIO_Pin_14
+#define GPIOD_BOARD_LED_BLUE   GPIO_Pin_15
diff --git a/src/stm32f/src/Core/main.c b/src/stm32f/src/Core/main.c
index 65d535e..d63f540 100644
--- a/src/stm32f/src/Core/main.c
+++ b/src/stm32f/src/Core/main.c
@@ -24,12 +24,7 @@
 
 #include "stm32f4xx_conf.h"
 
-
-#define GPIOD_BOARD_LED_GREEN  GPIO_Pin_12
-#define GPIOD_BOARD_LED_ORANGE GPIO_Pin_13
-#define GPIOD_BOARD_LED_RED    GPIO_Pin_14
-#define GPIOD_BOARD_LED_BLUE   GPIO_Pin_15
-
+#include "../../../stm32f/includes/GPIO/leds.h"
 #include "../../../common/src/Core/main.c"
 
 
diff --git a/src/stm32f/src/GPIO/leds.c b/src/stm32f/src/GPIO/leds.c
new file mode 100644
index 0000000..291b650
--- /dev/null
+++ b/src/stm32f/src/GPIO/leds.c
@@ -0,0 +1,42 @@
+#include "../../../common/src/GPIO/leds.c"
+
+#include "stm32f4xx_conf.h"
+
+#include "../../../stm32f/includes/GPIO/leds.h"
+
+void leds_turn_on(int l) {
+
+    switch (l) {
+        case LED_GREEN:
+            GPIO_SetBits(GPIOD, GPIOD_BOARD_LED_GREEN);
+            break;
+        case LED_ORANGE:
+            GPIO_SetBits(GPIOD, GPIOD_BOARD_LED_ORANGE);
+            break;
+        case LED_RED:
+            GPIO_SetBits(GPIOD, GPIOD_BOARD_LED_RED);
+            break;
+        case LED_BLUE:
+            GPIO_SetBits(GPIOD, GPIOD_BOARD_LED_BLUE);
+            break;
+
+    }
+}
+
+void leds_turn_off(int l) {
+
+    switch (l) {
+        case LED_GREEN:
+            GPIO_ResetBits(GPIOD, GPIOD_BOARD_LED_GREEN);
+            break;
+        case LED_ORANGE:
+            GPIO_ResetBits(GPIOD, GPIOD_BOARD_LED_ORANGE);
+            break;
+        case LED_RED:
+            GPIO_ResetBits(GPIOD, GPIOD_BOARD_LED_RED);
+            break;
+        case LED_BLUE:
+            GPIO_ResetBits(GPIOD, GPIOD_BOARD_LED_BLUE);
+            break;
+    }
+}
diff --git a/src/stm32f/src/Core/usart.c b/src/stm32f/src/GPIO/usart.c
index 29c8a20..d174719 100644
--- a/src/stm32f/src/Core/usart.c
+++ b/src/stm32f/src/GPIO/usart.c
@@ -1,4 +1,3 @@
-
 /*
  * The MIT License (MIT)
  *
@@ -23,18 +22,10 @@
  * SOFTWARE.
 */
 
-#include <stdio.h>
-#include <stdarg.h>
-#include <string.h>
-#include <inttypes.h>
-#include "Core/common.h"
-#include "Core/usart.h"
+
 #include <stm32f4xx.h>
 #include <stm32f4xx_usart.h>
 #include <stm32f4xx_conf.h>
-#include "FreeRTOS.h"
-#include "task.h"
-#include "queue.h"
 
 /* USART 3 on PD8 and PD9
  * See pio.txt for PIO allocation details */
@@ -45,16 +36,13 @@ const uint16_t GPIOD_PIN_USART3_RX = GPIO_Pin_9;
 const uint16_t GPIOA_PIN_USART2_RX = GPIO_Pin_3;
 const uint16_t GPIOA_PIN_USART2_TX = GPIO_Pin_2;
 
-// The ISR writes into this buffer
-static char nmea_sentence[MAX_NMEA_SENTENCE_LEN];
-static int  nmea_sentence_last_written = 0;
+static void usart_puts(USART_TypeDef*, const char*);
 
-// Once a completed NMEA sentence is received in the ISR,
-// it is appended to this queue
-static QueueHandle_t usart_nmea_queue;
+#include "../../../common/includes/GPIO/usart.h"
+#include "../../../common/src/GPIO/usart.c"
 
-void usart_init()
-{
+
+void usart_init() {
     // ============== PC DEBUG USART ===========
     RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
     RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
@@ -98,12 +86,7 @@ void usart_init()
     USART_Cmd(USART2, ENABLE);
 }
 
-void usart_gps_init()
-{
-    usart_nmea_queue = xQueueCreate(15, MAX_NMEA_SENTENCE_LEN);
-    if (usart_nmea_queue == 0) {
-        while(1); /* fatal error */
-    }
+void usart_gps_specific_init() {
 
     // ============== GPS USART ===========
     // Setup GPIO D and connect to USART 3
@@ -149,8 +132,7 @@ void usart_gps_init()
 }
 
 // Make sure Tasks are suspended when this is called!
-static void usart_puts(USART_TypeDef* USART, const char* str)
-{
+static void usart_puts(USART_TypeDef* USART, const char* str) {
     while(*str) {
         // wait until data register is empty
         USART_SendData(USART, *str);
@@ -159,122 +141,16 @@ static void usart_puts(USART_TypeDef* USART, const char* str)
     }
 }
 
-void usart_gps_puts(const char* str)
-{
-    vTaskSuspendAll();
-    return usart_puts(USART3, str);
-    xTaskResumeAll();
-}
-
-#define MAX_MSG_LEN 80
-static char usart_debug_message[MAX_MSG_LEN];
-
-void usart_debug_timestamp()
-{
-    // Don't call printf here, to reduce stack usage
-    uint64_t now = timestamp_now();
-    if (now == 0) {
-        usart_puts(USART2, "[0] ");
-    }
-    else {
-        char ts_str[64];
-        int i = 63;
-
-        ts_str[i--] = '\0';
-        ts_str[i--] = ' ';
-        ts_str[i--] = ']';
-
-        while (now > 0 && i >= 0) {
-            ts_str[i--] = '0' + (now % 10);
-            now /= 10;
-        }
-        ts_str[i] = '[';
-
-        usart_puts(USART2, &ts_str[i]);
-    }
-}
-
-void usart_debug(const char *format, ...)
-{
-    va_list list;
-    va_start(list, format);
-    vsnprintf(usart_debug_message, MAX_MSG_LEN-1, format, list);
-
-    vTaskSuspendAll();
-    usart_debug_timestamp();
-    usart_puts(USART2, usart_debug_message);
-    xTaskResumeAll();
-
-    va_end(list);
-}
-
-void usart_debug_puts(const char* str)
-{
-    vTaskSuspendAll();
-    usart_debug_timestamp();
-    usart_puts(USART2, str);
-    xTaskResumeAll();
-}
-
-int usart_get_nmea_sentence(char* nmea)
-{
-    return xQueueReceive(usart_nmea_queue, nmea, portMAX_DELAY);
-}
-
-
-static void usart_clear_nmea_buffer(void)
-{
-    for (int i = 0; i < MAX_NMEA_SENTENCE_LEN; i++) {
-        nmea_sentence[i] = '\0';
-    }
-    nmea_sentence_last_written = 0;
-}
-
-void USART3_IRQHandler(void)
-{
+void USART3_IRQHandler(void) {
     if (USART_GetITStatus(USART3, USART_IT_RXNE)) {
         char t = USART3->DR;
-
-        if (nmea_sentence_last_written == 0) {
-            if (t == '$') {
-                // Likely new start of sentence
-                nmea_sentence[nmea_sentence_last_written] = t;
-                nmea_sentence_last_written++;
-            }
-        }
-        else if (nmea_sentence_last_written < MAX_NMEA_SENTENCE_LEN) {
-            nmea_sentence[nmea_sentence_last_written] = t;
-            nmea_sentence_last_written++;
-
-            if (t == '\n') {
-                int success = xQueueSendToBackFromISR(
-                        usart_nmea_queue,
-                        nmea_sentence,
-                        NULL);
-
-                if (success == pdFALSE) {
-                    trigger_fault(FAULT_SOURCE_USART);
-                }
-
-                usart_clear_nmea_buffer();
-            }
-        }
-        else {
-            // Buffer overrun without a meaningful NMEA message.
-            usart_clear_nmea_buffer();
-        }
+        usart_gps_process_char(t);
     }
 }
 
-void USART2_IRQHandler(void)
-{
+void USART2_IRQHandler(void) {
     if (USART_GetITStatus(USART2, USART_IT_RXNE)) {
         char t = USART2->DR;
-        if (t == 'h') {
-            usart_debug_puts("help: no commands supported yet!\r\n");
-        }
-        else {
-            usart_debug("Unknown command %c\r\n", t);
-        }
+        usart_process_char(t);
     }
 }
diff --git a/src/stm32f/src/GPS/gps.c b/src/stm32f/src/GPS/gps.c
new file mode 100644
index 0000000..10afe88
--- /dev/null
+++ b/src/stm32f/src/GPS/gps.c
@@ -0,0 +1,4 @@
+#include "stm32f4xx_conf.h"
+#include "stm32f4xx.h"
+
+#include "../../../common/src/GPS/gps.c"
diff --git a/src/stm32f/src/GPS/minema.c b/src/stm32f/src/GPS/minema.c
new file mode 100644
index 0000000..10df198
--- /dev/null
+++ b/src/stm32f/src/GPS/minema.c
@@ -0,0 +1 @@
+#include "../../../common/src/GPS/minmea.c"