Fix I2C for GPS, needs timeout handling

9 files changed, 396 insertions, 94 deletions

diff --git a/src/fsm/audio.c b/src/fsm/audio.c
index 02123fd..e74e93a 100644
--- a/src/fsm/audio.c
+++ b/src/fsm/audio.c
@@ -28,14 +28,11 @@ void InitializeAudio(int plln, int pllr, int i2sdiv, int i2sodd) {
     DMARunning = false;
 
     // Turn on peripherals.
-    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
-    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
-    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
-    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
+    // Assume GPIOA,B,C,D already on
     RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
     RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI3, ENABLE);
 
-    i2c_init();
+    // Assume I2C is set up
 
     // Configure reset pin.
     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;;
@@ -229,9 +226,13 @@ void DMA1_Stream7_IRQHandler() {
     }
 }
 
+// Warning: don't i2c_write call from IRQ handler !
 static void WriteRegister(uint8_t address, uint8_t value)
 {
     const uint8_t device = 0x4a;
     const uint8_t data[2] = {address, value};
+    i2c_transaction_start();
     i2c_write(device, data, 2);
+    i2c_transaction_end();
 }
+
diff --git a/src/fsm/common.c b/src/fsm/common.c
index 6e00740..ca4c009 100644
--- a/src/fsm/common.c
+++ b/src/fsm/common.c
@@ -25,6 +25,7 @@
 #include "common.h"
 #include "FreeRTOS.h"
 #include "timers.h"
+#include <stm32f4xx.h>
 
 static uint64_t common_timestamp = 0; // milliseconds since startup
 static TimerHandle_t common_timer;
@@ -71,4 +72,15 @@ int random_bool(void)
     return bit;
 }
 
+// For the debugger
+static int faultsource = 0;
+void trigger_fault(int source)
+{
+    __disable_irq();
+
+    faultsource = source;
+
+    while (1) {}
+}
+
 
diff --git a/src/fsm/common.h b/src/fsm/common.h
index 1bd3b96..53da672 100644
--- a/src/fsm/common.h
+++ b/src/fsm/common.h
@@ -40,5 +40,11 @@ uint64_t timestamp_now(void);
 // Return either 0 or 1, somewhat randomly
 int random_bool(void);
 
+// Fault handling mechanism
+#define FAULT_SOURCE_MAIN 1
+#define FAULT_SOURCE_GPS  2
+#define FAULT_SOURCE_I2C  3
+void trigger_fault(int source);
+
 #endif // _COMMON_H_
 
diff --git a/src/fsm/gps.c b/src/fsm/gps.c
index fdb93f2..fe3e98a 100644
--- a/src/fsm/gps.c
+++ b/src/fsm/gps.c
@@ -22,26 +22,60 @@
  * SOFTWARE.
 */
 
-#include "gps.h"
-#include "i2c.h"
-#include "string.h"
 #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 "i2c.h"
+#include "ubx.h"
+
 
-static ubx_nav_timeutc_t gps_timeutc;
+static struct gps_time_s gps_timeutc;
+static int gps_fix_3d;
 
 static void gps_task(void *pvParameters);
 
 // Callback functions for UBX parser
-static void gps_nav_sol(ubx_nav_sol_t *sol) {}
+static void gps_nav_sol(ubx_nav_sol_t *sol)
+{
+    gps_fix_3d = (sol->GPSfix == GPSFIX_3D) ? 1 : 0;
+}
+
 static void gps_tim_tm2(ubx_tim_tm2_t *posllh) {}
+
+SemaphoreHandle_t timeutc_semaphore;
 static void gps_nav_timeutc(ubx_nav_timeutc_t *timeutc)
 {
-    memcpy(&gps_timeutc, timeutc, sizeof(ubx_nav_timeutc_t));
+    xSemaphoreTake(timeutc_semaphore, portMAX_DELAY);
+    gps_timeutc.year  = timeutc->year;
+    gps_timeutc.month = timeutc->month;
+    gps_timeutc.day   = timeutc->day;
+    gps_timeutc.hour  = timeutc->hour;
+    gps_timeutc.min   = timeutc->min;
+    gps_timeutc.sec   = timeutc->sec;
+    gps_timeutc.valid = timeutc->valid;
+    xSemaphoreGive(timeutc_semaphore);
 }
 
+// Get current time from GPS
+void gps_utctime(struct gps_time_s *timeutc)
+{
+    xSemaphoreTake(timeutc_semaphore, portMAX_DELAY);
+    timeutc->year  = gps_timeutc.year;
+    timeutc->month = gps_timeutc.month;
+    timeutc->day   = gps_timeutc.day;
+    timeutc->hour  = gps_timeutc.hour;
+    timeutc->min   = gps_timeutc.min;
+    timeutc->sec   = gps_timeutc.sec;
+    timeutc->valid = gps_timeutc.valid;
+    xSemaphoreGive(timeutc_semaphore);
+}
+
+
 const ubx_callbacks_t gps_ubx_cb = {
     gps_nav_sol,
     gps_nav_timeutc,
@@ -49,26 +83,74 @@ const ubx_callbacks_t gps_ubx_cb = {
 };
 
 
-#define RXBUF_LEN 32
+#define RXBUF_LEN 128
 static uint8_t rxbuf[RXBUF_LEN];
+static uint8_t gps_init_messages[] = {
+    UBX_ENABLE_NAV_SOL,
+    UBX_ENABLE_NAV_TIMEUTC,
+    UBX_ENABLE_TIM_TM2 };
 
 static void gps_task(void *pvParameters)
 {
+    const uint8_t address = 0xFD;
+    int must_init_gps = 1;
+
     while (1) {
-        const uint8_t address = 0xFF;
-        int bytes_read = i2c_read(GPS_I2C_ADDR, address, rxbuf, RXBUF_LEN);
+        taskYIELD();
+
+        i2c_transaction_start();
+
+        if (must_init_gps) {
+            i2c_write(GPS_I2C_ADDR,
+                    gps_init_messages,
+                    sizeof(gps_init_messages));
+
+            must_init_gps = 0;
+        }
+
+        int bytes_read = i2c_read_from(GPS_I2C_ADDR, address, rxbuf, 2);
 
-        for (int i = 0; i < bytes_read; i++) {
-            ubx_parse(rxbuf[i]);
+        if (bytes_read != 2) {
+            i2c_transaction_end();
+            continue;
+        }
+
+        uint16_t bytes_available = (rxbuf[0] << 8) | rxbuf[1];
+
+        if (bytes_available) {
+            if (bytes_available > RXBUF_LEN) {
+                bytes_available = RXBUF_LEN;
+            }
+
+            bytes_read = i2c_read(GPS_I2C_ADDR, rxbuf, bytes_available);
+            i2c_transaction_end();
+
+            for (int i = 0; i < bytes_read; i++) {
+                ubx_parse(rxbuf[i]);
+            }
+        }
+        else {
+            i2c_transaction_end();
         }
     }
 }
 
 void gps_init()
 {
-    i2c_init();
+    gps_fix_3d = 0;
+    gps_timeutc.valid = 0;
+
     ubx_register(&gps_ubx_cb);
 
+    timeutc_semaphore = xSemaphoreCreateBinary();
+
+    if( timeutc_semaphore == NULL ) {
+        trigger_fault(FAULT_SOURCE_GPS);
+    }
+    else {
+        xSemaphoreGive(timeutc_semaphore);
+    }
+
     xTaskCreate(
             gps_task,
             "TaskGPS",
@@ -81,13 +163,6 @@ void gps_init()
 // Return 1 of the GPS is receiving time
 int gps_locked()
 {
-    return 0;
-}
-
-// Get current time from GPS
-ubx_nav_timeutc_t* gps_utctime()
-{
-
-    return &gps_timeutc;
+    return gps_fix_3d;
 }
 
diff --git a/src/fsm/gps.h b/src/fsm/gps.h
index e54524b..82f8a77 100644
--- a/src/fsm/gps.h
+++ b/src/fsm/gps.h
@@ -22,6 +22,8 @@
  * SOFTWARE.
 */
 
+#include <stdint.h>
+
 #ifndef __GPS_H_
 #define __GPS_H_
 
@@ -34,7 +36,15 @@
 
 #define GPS_I2C_ADDR 0x42 // I2C address of GPS receiver
 
-#include "ubx.h"
+struct gps_time_s {
+    uint16_t  year;   // Year, range 1999..2099 (UTC)
+    uint8_t   month;  // Month, range 1..12 (UTC)
+    uint8_t   day;    // Day of Month, range 1..31 (UTC)
+    uint8_t   hour;   // Hour of Day, range 0..23 (UTC)
+    uint8_t   min;    // Minute of Hour, range 0..59 (UTC)
+    uint8_t   sec;    // Seconds of Minute, range 0..59 (UTC)
+    uint8_t   valid;  // validity flag
+};
 
 // Setup communication and GPS receiver
 void gps_init();
@@ -43,7 +53,7 @@ void gps_init();
 int gps_locked();
 
 // Get current time from GPS
-ubx_nav_timeutc_t* gps_utctime();
+void gps_utctime(struct gps_time_s *timeutc);
 
 #endif // __GPS_H_
 
diff --git a/src/fsm/i2c.c b/src/fsm/i2c.c
index da109fc..8215ec7 100644
--- a/src/fsm/i2c.c
+++ b/src/fsm/i2c.c
@@ -23,20 +23,56 @@
 */
 
 #include "i2c.h"
+#include "common.h"
 #include "stm32f4xx_conf.h"
 #include "stm32f4xx_i2c.h"
 #include "stm32f4xx.h"
+#include "FreeRTOS.h"
+#include "FreeRTOSConfig.h"
+#include "task.h"
+#include "semphr.h"
 
 static int i2c_init_done = 0;
+static int i2c_error = 0;
 
 static I2C_TypeDef* const I2Cx = I2C1;
 
+static SemaphoreHandle_t i2c_semaphore;
+
+static void i2c_device_init(void)
+{
+    // configure I2C1
+    I2C_InitTypeDef I2C_InitStruct;
+    I2C_InitStruct.I2C_ClockSpeed = 100000; //Hz
+    I2C_InitStruct.I2C_Mode = I2C_Mode_I2C;
+    I2C_InitStruct.I2C_DutyCycle = I2C_DutyCycle_2; // 50% duty cycle
+    I2C_InitStruct.I2C_OwnAddress1 = 0x00;          // not relevant in master mode
+
+    // disable acknowledge when reading (can be changed later on)
+    I2C_InitStruct.I2C_Ack = I2C_Ack_Disable;
+
+    I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
+    I2C_Init(I2C1, &I2C_InitStruct);
+
+    // enable I2C1
+    I2C_Cmd(I2C1, ENABLE);
+}
+
 void i2c_init()
 {
     if (i2c_init_done == 1) {
         return;
     }
 
+    i2c_semaphore = xSemaphoreCreateBinary();
+
+    if ( i2c_semaphore == NULL ) {
+        trigger_fault(FAULT_SOURCE_I2C);
+    }
+    else {
+        xSemaphoreGive(i2c_semaphore);
+    }
+
     RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
     RCC_APB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
 
@@ -56,32 +92,17 @@ void i2c_init()
     RCC_APB1PeriphResetCmd(RCC_APB1Periph_I2C1, ENABLE);
     RCC_APB1PeriphResetCmd(RCC_APB1Periph_I2C1, DISABLE);
 
-    // configure I2C1
-    I2C_InitTypeDef I2C_InitStruct;
-    I2C_InitStruct.I2C_ClockSpeed = 100000; //Hz
-    I2C_InitStruct.I2C_Mode = I2C_Mode_I2C;
-    I2C_InitStruct.I2C_DutyCycle = I2C_DutyCycle_2; // 50% duty cycle
-    I2C_InitStruct.I2C_OwnAddress1 = 0x00;          // not relevant in master mode
-
-    // disable acknowledge when reading (can be changed later on)
-    I2C_InitStruct.I2C_Ack = I2C_Ack_Disable;
-
-    I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
-    I2C_Init(I2C1, &I2C_InitStruct);
-
-    // enable I2C1
-    I2C_Cmd(I2C1, ENABLE);
-
+    i2c_device_init();
     i2c_init_done = 1;
 }
 
 
-static void i2c_start(uint8_t device, uint8_t direction)
+static int i2c_start(uint8_t device, uint8_t direction)
 {
-    while(I2C_GetFlagStatus(I2Cx, I2C_FLAG_BUSY));
     I2C_GenerateSTART(I2Cx, ENABLE);
+
     // wait for I2C1 EV5 --> Slave has acknowledged start condition
-    while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT));
+    while (!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT));
 
     // Send slave Address for write
     I2C_Send7bitAddress(I2Cx, device << 1, direction);
@@ -91,47 +112,137 @@ static void i2c_start(uint8_t device, uint8_t direction)
      * Master receiver mode, depending on the transmission
      * direction
      */
-    if(direction == I2C_Direction_Transmitter) {
-        while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
+    uint32_t event = 0;
+    if (direction == I2C_Direction_Transmitter) {
+        event = I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED;
     }
-    else if(direction == I2C_Direction_Receiver) {
-        while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));
+    else if (direction == I2C_Direction_Receiver) {
+        event = I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED;
     }
+    else {
+        trigger_fault(FAULT_SOURCE_I2C);
+    }
+
+    while (!I2C_CheckEvent(I2Cx, event));
+
+    return 1;
 }
 
-static void i2c_send(uint8_t data) {
+static int i2c_send(uint8_t data)
+{
     I2C_SendData(I2Cx, data);
+
     // wait for I2C1 EV8_2 --> byte has been transmitted
-    while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
+    while (!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
+
+    return 1;
 }
 
-void i2c_write(uint8_t device, const uint8_t *txbuf, int len) {
-    i2c_start(device, I2C_Direction_Transmitter);
-    for (int i = 0; i < len; i++) {
-        i2c_send(txbuf[i]);
+int i2c_write(uint8_t device, const uint8_t *txbuf, int len)
+{
+    if (i2c_init_done == 0) {
+        trigger_fault(FAULT_SOURCE_I2C);
+    }
+
+    while (I2C_GetFlagStatus(I2Cx, I2C_FLAG_BUSY));
+
+    int success = 0;
+
+    if (i2c_start(device, I2C_Direction_Transmitter)) {
+        for (int i = 0; i < len; i++) {
+            success = i2c_send(txbuf[i]);
+            if (!success) {
+                break;
+            }
+        }
     }
     I2C_GenerateSTOP(I2Cx, ENABLE);
+    while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
+
+    return success;
 }
 
-int i2c_read(uint8_t device, uint8_t *rxbuf, int len)
+static int i2c_read_nobuscheck(uint8_t device, uint8_t *rxbuf, int len)
 {
+    if (i2c_init_done == 0) {
+        trigger_fault(FAULT_SOURCE_I2C);
+    }
+
     i2c_start(device, I2C_Direction_Receiver);
     for (int i = 0; i < len; i++) {
-        if (i < len-1) {
-            I2C_AcknowledgeConfig(I2Cx, ENABLE);
-            // wait until one byte has been received
-            while( !I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_RECEIVED) );
-            rxbuf[i] = I2C_ReceiveData(I2Cx);
+        if (i == len-1) {
+            I2C_AcknowledgeConfig(I2Cx, DISABLE);
         }
         else {
-            I2C_AcknowledgeConfig(I2Cx, DISABLE);
+            I2C_AcknowledgeConfig(I2Cx, ENABLE);
+        }
+
+        // wait until one byte has been received
+        while( !I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_RECEIVED) );
+
+        if (i == len-1) {
             I2C_GenerateSTOP(I2Cx, ENABLE);
-            // wait until one byte has been received
-            while( !I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_RECEIVED) );
-            rxbuf[i] = I2C_ReceiveData(I2Cx);
         }
+
+        rxbuf[i] = I2C_ReceiveData(I2Cx);
     }
 
     return len;
 }
 
+int i2c_read(uint8_t device, uint8_t *rxbuf, int len)
+{
+    while (I2C_GetFlagStatus(I2Cx, I2C_FLAG_BUSY));
+
+    return i2c_read_nobuscheck(device, rxbuf, len);
+}
+
+int i2c_read_from(uint8_t device, uint8_t address, uint8_t *rxbuf, int len)
+{
+    if (i2c_init_done == 0) {
+        trigger_fault(FAULT_SOURCE_I2C);
+    }
+
+    while (I2C_GetFlagStatus(I2Cx, I2C_FLAG_BUSY));
+
+    int success = i2c_start(device, I2C_Direction_Transmitter);
+    if (success) {
+        success = i2c_send(address);
+    }
+    // Don't do a STOP
+
+    if (success) {
+        success = i2c_read_nobuscheck(device, rxbuf, len);
+    }
+
+    return success;
+}
+
+
+
+void i2c_transaction_start()
+{
+    if (i2c_init_done == 0) {
+        trigger_fault(FAULT_SOURCE_I2C);
+    }
+    xSemaphoreTake(i2c_semaphore, portMAX_DELAY);
+}
+
+void i2c_transaction_end()
+{
+    if (i2c_init_done == 0) {
+        trigger_fault(FAULT_SOURCE_I2C);
+    }
+
+    if ( i2c_error ) {
+        I2C_SoftwareResetCmd(I2Cx, ENABLE);
+        I2C_SoftwareResetCmd(I2Cx, DISABLE);
+
+        i2c_device_init();
+
+        i2c_error = 0;
+    }
+
+    xSemaphoreGive(i2c_semaphore);
+}
+
diff --git a/src/fsm/i2c.h b/src/fsm/i2c.h
index 2e2dbd3..109213b 100644
--- a/src/fsm/i2c.h
+++ b/src/fsm/i2c.h
@@ -30,11 +30,24 @@
 /* Initialise I2C on the board for both the audio codec and the GPS receiver */
 void i2c_init();
 
-/* Do an I2C write */
-void i2c_write(uint8_t device, const uint8_t *txbuf, int len);
+/* Do an I2C write, return 1 on success, 0 on failure */
+int i2c_write(uint8_t device, const uint8_t *txbuf, int len);
 
-/* Do an I2C write into rxbuf. Returns number of bytes received */
+/* Do an I2C read into rxbuf.
+ * Returns number of bytes received, or 0 in case of failure
+ */
 int i2c_read(uint8_t device, uint8_t *rxbuf, int len);
 
+/* Do an I2C write for the address, and then a read into rxbuf.
+ * Returns number of bytes received, or 0 in case of failure
+ */
+int i2c_read_from(uint8_t device, uint8_t address, uint8_t *rxbuf, int len);
+
+/* Start an I2C transaction, keeping exclusive access to I2C */
+void i2c_transaction_start(void);
+
+/* End an I2C transaction, unlocking the exclusive access to I2C */
+void i2c_transaction_end(void);
+
 #endif // __I2C_H_
 
diff --git a/src/fsm/main.c b/src/fsm/main.c
index 225b2f9..58e4140 100644
--- a/src/fsm/main.c
+++ b/src/fsm/main.c
@@ -36,6 +36,8 @@
 #include "semphr.h"
 #include "cw.h"
 #include "pio.h"
+#include "i2c.h"
+#include "gps.h"
 #include "fsm.h"
 #include "common.h"
 
@@ -48,7 +50,11 @@ void init();
 // Tasks
 static void detect_button_press(void *pvParameters);
 static void exercise_fsm(void *pvParameters);
-static void audio_task(void *pvParameters);
+static void gps_monit_task(void *pvParameters);
+static void launcher_task(void *pvParameters);
+
+// Audio callback function
+static void audio_callback(void* context, int select_buffer);
 
 struct cw_msg_s {
     const char* msg;
@@ -65,22 +71,48 @@ void vApplicationStackOverflowHook( TaskHandle_t xTask,
 int main(void) {
     init();
 
-    cw_init(16000);
+    TaskHandle_t task_handle;
+    xTaskCreate(
+            launcher_task,
+            "TaskLauncher",
+            configMINIMAL_STACK_SIZE,
+            (void*) NULL,
+            tskIDLE_PRIORITY + 2UL,
+            &task_handle);
 
-    pio_init();
+    if (!task_handle) {
+        trigger_fault(FAULT_SOURCE_MAIN);
+    }
 
-    common_init();
+    /* Start the RTOS Scheduler */
+    vTaskStartScheduler();
 
-    InitializeAudio(Audio16000HzSettings);
-    SetAudioVolume(164);
+    /* HALT */
+    while(1);
+}
+
+// Launcher task is here to make sure the scheduler is
+// already running when calling the init functions.
+static void launcher_task(void *pvParameters)
+{
+    cw_init(16000);
+    pio_init();
+    i2c_init();
+    common_init();
+    gps_init();
 
+    TaskHandle_t task_handle;
     xTaskCreate(
             detect_button_press,
             "TaskButton",
             4*configMINIMAL_STACK_SIZE,
             (void*) NULL,
             tskIDLE_PRIORITY + 2UL,
-            NULL);
+            &task_handle);
+
+    if (!task_handle) {
+        trigger_fault(FAULT_SOURCE_MAIN);
+    }
 
     xTaskCreate(
             exercise_fsm,
@@ -88,24 +120,40 @@ int main(void) {
             4*configMINIMAL_STACK_SIZE,
             (void*) NULL,
             tskIDLE_PRIORITY + 2UL,
-            NULL);
+            &task_handle);
+
+    if (!task_handle) {
+        trigger_fault(FAULT_SOURCE_MAIN);
+    }
 
     xTaskCreate(
-            audio_task,
-            "TaskAudio",
+            gps_monit_task,
+            "TaskGPSMonit",
             configMINIMAL_STACK_SIZE,
             (void*) NULL,
             tskIDLE_PRIORITY + 2UL,
-            NULL);
+            &task_handle);
 
-    /* Start the RTOS Scheduler */
-    vTaskStartScheduler();
+    if (!task_handle) {
+        trigger_fault(FAULT_SOURCE_MAIN);
+    }
 
-    /* HALT */
-    while(1);
+    InitializeAudio(Audio16000HzSettings);
+    SetAudioVolume(164);
+
+    PlayAudioWithCallback(audio_callback, NULL);
+
+
+    /* 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);
+    }
 }
 
-// disabled
 static void detect_button_press(void *pvParameters)
 {
     GPIO_SetBits(GPIOD, GPIO_Pin_12);
@@ -160,11 +208,24 @@ static void audio_callback(void* context, int select_buffer)
     ProvideAudioBufferWithoutBlocking(samples, samples_len);
 }
 
-static void audio_task(void *pvParameters)
+
+static struct gps_time_s gps_time;
+static void gps_monit_task(void *pvParameters)
 {
-    PlayAudioWithCallback(audio_callback, NULL);
+    GPIO_SetBits(GPIOD, GPIO_Pin_15);
 
     while (1) {
+        if (gps_locked()) {
+
+            gps_utctime(&gps_time);
+
+            if (gps_time.sec % 10 > 5) {
+                GPIO_SetBits(GPIOD, GPIO_Pin_15);
+            }
+            else {
+                GPIO_ResetBits(GPIOD, GPIO_Pin_15);
+            }
+        }
         taskYIELD();
     }
 }
@@ -185,12 +246,6 @@ static void exercise_fsm(void *pvParameters)
         fsm_input.start_tm = (tm_trigger == 1 && last_tm_trigger == 0) ? 1 : 0;
         last_tm_trigger = tm_trigger;
 
-        if (fsm_input.start_tm) {
-            GPIO_SetBits(GPIOD, GPIO_Pin_15);
-        }
-        else {
-            GPIO_ResetBits(GPIOD, GPIO_Pin_15);
-        }
         fsm_input.sq = fsm_input.carrier; // TODO clarify
         fsm_input.cw_done = !cw_busy();
 
@@ -234,6 +289,9 @@ void init() {
     //SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2));  // set CP10 and CP11 Full Access
 
     // GPIOD Periph clock enable
+    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
+    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
+    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
     RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
 
     // Configure PD12, PD13, PD14 and PD15 in output pushpull mode
@@ -257,7 +315,6 @@ void init() {
 
     // Clock
     RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
-    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
 
     // IO
     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6;
diff --git a/src/fsm/ubx.h b/src/fsm/ubx.h
index 2af5705..e80464e 100644
--- a/src/fsm/ubx.h
+++ b/src/fsm/ubx.h
@@ -80,6 +80,23 @@ typedef enum ubx_state_e
     UBXSTATE_CKB
 } ubx_state_t;
 
+#define GPSFIX_3D 0x3
+
+// UBX messages to enable these messages on the I2C port
+#define UBX_ENABLE_NAV_SOL \
+  0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, \
+  0x01, 0x06, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, \
+  0x17, 0xDB
+
+#define UBX_ENABLE_NAV_TIMEUTC \
+  0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, \
+  0x01, 0x21, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, \
+  0x32, 0x98
+
+#define UBX_ENABLE_TIM_TM2 \
+  0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, \
+  0x0D, 0x03, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, \
+  0x20, 0x26
 
 typedef struct ubx_nav_sol_s
 {