diff --git a/firmware/inc/gps.h b/firmware/inc/gps.h
index 1440b4f..1ade0d0 100644
--- a/firmware/inc/gps.h
+++ b/firmware/inc/gps.h
@@ -33,12 +33,17 @@ enum gps_error_t {
   GPS_ERROR_INVALID_FRAME,
 };
 
-void gps_update();
+void gps_update_time(void);
+void gps_update_position(void);
+int gps_update_time_pending(void);
+int gps_update_position_pending(void);
 
 struct ubx_nav_posllh gps_get_nav_posllh();
 struct ubx_nav_sol gps_get_nav_sol();
 struct ubx_nav_timeutc gps_get_nav_timeutc();
 
+uint8_t gps_is_locked(void);
+
 void gps_init(void);
 void usart_loopback_test(void);
 
diff --git a/firmware/inc/hw_config.h b/firmware/inc/hw_config.h
index 86850b0..82726c4 100644
--- a/firmware/inc/hw_config.h
+++ b/firmware/inc/hw_config.h
@@ -120,6 +120,11 @@
 #define SI406X_TCXO_GCLK     	GCLK_GENERATOR_3
 #define SI406X_TCXO_FREQUENCY	16369000
 
+/**
+ * Watchdog Timer
+ */
+#define WDT_GCLK		GCLK_GENERATOR_4
+
 /**
  * External Watchdog Timer
  */
diff --git a/firmware/inc/rtty.h b/firmware/inc/rtty.h
index 600e323..5088927 100644
--- a/firmware/inc/rtty.h
+++ b/firmware/inc/rtty.h
@@ -25,8 +25,21 @@
 #ifndef RTTY_H
 #define RTTY_H
 
+#include "util/dbuffer.h"
+
+/**
+ * Output String
+ */
+#define RTTY_STRING_MAX	0x200
+/**
+ * It's actually a double buffer which we swap for mid-string updates
+ */
+ARRAY_DBUFFER_T(char, RTTY_STRING_MAX) rtty_dbuffer_string;
+
 int rtty_active(void);
-int rtty_set_string(char* string, uint32_t length);
+int rtty_start(void);
+int32_t rtty_get_index(void);
+void rtty_set_length(int32_t length);
 void rtty_tick(void);
 
 #endif /* RTTY_H */
diff --git a/firmware/inc/telemetry.h b/firmware/inc/telemetry.h
index 743c57d..c0288c5 100644
--- a/firmware/inc/telemetry.h
+++ b/firmware/inc/telemetry.h
@@ -25,6 +25,7 @@
 #ifndef TELEMETRY_H
 #define TELEMETRY_H
 
-void set_telemetry_string(void);
+uint16_t crc_checksum(char *string);
+void timer0_tick_init(uint32_t frequency);
 
 #endif /* TELEMETRY_H */
diff --git a/firmware/inc/ubx_messages.h b/firmware/inc/ubx_messages.h
index 94c1645..dd942f1 100644
--- a/firmware/inc/ubx_messages.h
+++ b/firmware/inc/ubx_messages.h
@@ -36,6 +36,7 @@ enum ubx_packet_state {
   UBX_PACKET_WAITING,
   UBX_PACKET_ACK,
   UBX_PACKET_NACK,
+  UBX_PACKET_UPDATED,
 };
 /** Generic UBX Message Type. Each message type extended is from this */
 typedef struct {
@@ -73,6 +74,26 @@ __PACKED__ struct ubx_cfg_ant {
     uint16_t pins;
   } payload;
 };
+/**
+ * UBX CFG GNSS
+ */
+__PACKED__ struct ubx_cfg_gnss {
+  ubx_message_id_t id;
+  enum ubx_packet_state state;
+  struct {
+    uint8_t msgVer;
+    uint8_t numTrkChHw;
+    uint8_t numTrkChUse;
+    uint8_t numConfigBlocks;
+    struct {
+      uint8_t gnssID;
+      uint8_t resTrkCh;
+      uint8_t maxTrkCh;
+      uint8_t reserved1;
+      int32_t flags;
+    } block[8];
+  } payload;
+};
 /**
  * UBX CFG NAV5 Navigation Engine Settings
  */
@@ -150,6 +171,17 @@ enum {
   UBX_PLATFORM_MODEL_AIRBORNE_2G	= 7,
   UBX_PLATFORM_MODEL_AIRBORNE_4G	= 8,
 };
+/**
+ * UBX GNSS Systems
+ */
+enum {
+  UBX_GNSS_GPS		= 0,
+  UBX_GNSS_SBAS		= 1,
+  UBX_GNSS_GALILEO	= 2,
+  UBX_GNSS_BEIDOU	= 3,
+  UBX_GNSS_QZSS		= 5,
+  UBX_GNSS_GLONASS	= 6,
+};
 
 /**
  * =============================================================================
diff --git a/firmware/src/gps.c b/firmware/src/gps.c
index cb909f3..6f62760 100644
--- a/firmware/src/gps.c
+++ b/firmware/src/gps.c
@@ -1,5 +1,5 @@
 /*
- * Functions for the UBLOX 6 GPS
+ * Functions for the UBLOX 8 GPS
  * Copyright (C) 2014  Richard Meadows <richardeoin>
  *
  * Permission is hereby granted, free of charge, to any person obtaining
@@ -69,6 +69,7 @@ uint8_t ubx_irq_buffer[UBX_BUFFER_LEN];
  * UBX Messages
  */
 volatile struct ubx_cfg_ant ubx_cfg_ant		= { .id = (UBX_CFG | (0x13 << 8)) };
+volatile struct ubx_cfg_gnss ubx_cfg_gnss       = { .id = (UBX_CFG | (0x3E << 8)) };
 volatile struct ubx_cfg_nav5 ubx_cfg_nav5	= { .id = (UBX_CFG | (0x24 << 8)) };
 volatile struct ubx_cfg_tp5 ubx_cfg_tp5		= { .id = (UBX_CFG | (0x31 << 8)) };
 volatile struct ubx_cfg_prt ubx_cfg_prt		= { .id = (UBX_CFG | (0x00 << 8)) };
@@ -81,6 +82,7 @@ volatile struct ubx_nav_status ubx_nav_status	= { .id = (UBX_NAV | (0x03 << 8))
  */
 volatile ubx_message_t* const ubx_messages[] = {
   (ubx_message_t*)&ubx_cfg_ant,
+  (ubx_message_t*)&ubx_cfg_gnss,
   (ubx_message_t*)&ubx_cfg_nav5,
   (ubx_message_t*)&ubx_cfg_tp5,
   (ubx_message_t*)&ubx_cfg_prt,
@@ -163,6 +165,9 @@ void ubx_process_frame(uint8_t* frame)
 	/* Populate struct */
 	memcpy((void*)(ubx_messages[i]+1), frame + 4, payload_length);
 
+	/* Set the message state */
+	ubx_messages[i]->state = UBX_PACKET_UPDATED;
+
 	return;
       }
     }
@@ -230,6 +235,9 @@ void _ubx_send_message(ubx_message_t* message, uint8_t* payload, uint16_t length
   uint8_t ubx[UBX_BUFFER_LEN];
   uint8_t* ubx_buffer = ubx;
 
+  /* Clear the message state */
+  message->state = UBX_PACKET_WAITING;
+
   /* Copy little endian */
   memcpy(ubx_buffer, &ubx_header, 2); ubx_buffer += 2; 	/* Header	*/
   memcpy(ubx_buffer, &message->id, 2); ubx_buffer += 2;	/* Message Type	*/
@@ -244,9 +252,7 @@ void _ubx_send_message(ubx_message_t* message, uint8_t* payload, uint16_t length
  * Polls the GPS for packets
  */
 void _ubx_poll(ubx_message_t* message) {
-  /* Clear the packet state */
-  message->state = UBX_PACKET_WAITING;
-
+  /* Send the message */
   _ubx_send_message(message, NULL, 0);
 
   /* Wait for acknoledge */
@@ -275,14 +281,37 @@ void gps_disable_nmea(void)
 }
 
 /**
- * Sends messages to the GPS to update the messages we want
+ * Sends messages to the GPS to update the time
  */
-void gps_update()
+void gps_update_time(void)
+{
+  _ubx_send_message((ubx_message_t*)&ubx_nav_timeutc, NULL, 0);
+};
+/**
+ * Sends messages to the GPS to update the position
+ */
+void gps_update_position(void)
 {
   _ubx_send_message((ubx_message_t*)&ubx_nav_posllh, NULL, 0);
   _ubx_send_message((ubx_message_t*)&ubx_nav_sol, NULL, 0);
-  _ubx_send_message((ubx_message_t*)&ubx_nav_timeutc, NULL, 0);
-  _ubx_send_message((ubx_message_t*)&ubx_nav_status, NULL, 0);
+
+  // _ubx_send_message((ubx_message_t*)&ubx_nav_status, NULL, 0);
+}
+/**
+ * Indicates a pending time update from the GPS
+ */
+int gps_update_time_pending(void)
+{
+  return (ubx_nav_timeutc.state == UBX_PACKET_WAITING);
+}
+/**
+ * Indicates a pending position update from the GPS
+ */
+int gps_update_position_pending(void)
+{
+  return (ubx_nav_posllh.state == UBX_PACKET_WAITING) ||
+    (ubx_nav_sol.state == UBX_PACKET_WAITING);
+  //(ubx_nav_status.state == UBX_PACKET_WAITING);
 }
 /**
  * Return the latest received messages
@@ -299,6 +328,15 @@ struct ubx_nav_timeutc gps_get_nav_timeutc()
 {
   return ubx_nav_timeutc;
 }
+/**
+ * Returns if the GPS has a position lock
+ */
+uint8_t gps_is_locked(void)
+{
+  return (ubx_nav_sol.payload.gpsFix == 0x2) ||
+    (ubx_nav_sol.payload.gpsFix == 0x3) ||
+    (ubx_nav_sol.payload.gpsFix == 0x4);
+}
 
 /**
  * Verify that the uBlox 6 GPS receiver is set to the <1g airborne
@@ -341,6 +379,36 @@ void gps_set_timepulse_five(uint32_t frequency)
 		    (uint8_t*)&ubx_cfg_tp5.payload,
 		    sizeof(ubx_cfg_tp5.payload));
 }
+/**
+ * Set which GNSS constellations to use
+ */
+void gps_set_gnss(void)
+{
+  /* Read the current settings */
+  _ubx_poll((ubx_message_t*)&ubx_cfg_gnss);
+
+  switch (ubx_cfg_gnss.payload.msgVer) {
+    case 0:
+      /* For each configuration block */
+      for (uint8_t i = 0; i < ubx_cfg_gnss.payload.numConfigBlocks; i++) {
+
+	/* If it's the configuration for something other than GPS */
+	if (ubx_cfg_gnss.payload.block[i].gnssID != UBX_GNSS_GPS) {
+
+	  /* Disable this GNSS system */
+	  ubx_cfg_gnss.payload.block[i].flags &= ~0x1;
+	}
+      }
+      break;
+    default:
+      break;
+  }
+
+  /* Write the new settings */
+  _ubx_send_message((ubx_message_t*)&ubx_cfg_gnss,
+		    (uint8_t*)&ubx_cfg_gnss.payload,
+		    4 + (8 * ubx_cfg_gnss.payload.numConfigBlocks));
+}
 
 /**
  * Init
@@ -386,6 +454,9 @@ void gps_init(void)
   /* Set the platform model */
   gps_set_platform_model();
 
+  /* Set which GNSS constellation we'd like to use */
+  gps_set_gnss();
+
   /* Set the timepulse */
   gps_set_timepulse_five(GPS_TIMEPULSE_FREQ);
 }
diff --git a/firmware/src/main.c b/firmware/src/main.c
index 42c0b74..b5405ac 100644
--- a/firmware/src/main.c
+++ b/firmware/src/main.c
@@ -34,15 +34,21 @@
 #include "system/port.h"
 #include "tc/tc_driver.h"
 #include "gps.h"
+#include "ubx_messages.h"
 #include "system/wdt.h"
 #include "timepulse.h"
 #include "telemetry.h"
+
 #include "si_trx.h"
+
+#include "analogue.h"
 #include "si4060.h"
 #include "spi_bitbang.h"
 #include "rtty.h"
 #include "system/interrupt.h"
 
+#define CALLSIGN	"UBSEDSx"
+
 void si4060_hw_init(void)
 {
   /* Configure the SDN pin */
@@ -148,25 +154,158 @@ void set_timer(uint32_t time)
   tc_start_counter(TC2);
 }
 
-/* void wdt_init() { */
-/*   /\* 64 seconds timeout. So 2^(15+6) cycles of the wdt clock *\/ */
-/*   system_gclk_gen_set_config(WDT_GCLK, */
-/* 			     GCLK_SOURCE_OSCULP32K, /\* Source 		*\/ */
-/* 			     false,		/\* High When Disabled	*\/ */
-/* 			     128,		/\* Division Factor	*\/ */
-/* 			     false,		/\* Run in standby	*\/ */
-/* 			     true);		/\* Output Pin Enable	*\/ */
-/*   system_gclk_gen_enable(WDT_GCLK); */
+void wdt_init() {
+  /* 64 seconds timeout. So 2^(15+6) cycles of the wdt clock */
+  system_gclk_gen_set_config(WDT_GCLK,
+			     GCLK_SOURCE_OSCULP32K, /* Source 		*/
+			     false,		/* High When Disabled	*/
+			     128,		/* Division Factor	*/
+			     false,		/* Run in standby	*/
+			     true);		/* Output Pin Enable	*/
+  system_gclk_gen_enable(WDT_GCLK);
 
-/*   /\* Set the watchdog timer. On 256Hz gclk 4  *\/ */
-/*   wdt_set_config(true,			/\* Lock WDT		*\/ */
-/*   		 true,			/\* Enable WDT		*\/ */
-/*   		 GCLK_GENERATOR_4,	/\* Clock Source		*\/ */
-/*   		 WDT_PERIOD_16384CLK,	/\* Timeout Period	*\/ */
-/*   		 WDT_PERIOD_NONE,	/\* Window Period	*\/ */
-/*   		 WDT_PERIOD_NONE);	/\* Early Warning Period	*\/ */
-/* } */
+  /* Set the watchdog timer. On 256Hz gclk 4  */
+  wdt_set_config(true,			/* Lock WDT		*/
+  		 true,			/* Enable WDT		*/
+  		 GCLK_GENERATOR_4,	/* Clock Source		*/
+  		 WDT_PERIOD_16384CLK,	/* Timeout Period	*/
+  		 WDT_PERIOD_NONE,	/* Window Period	*/
+  		 WDT_PERIOD_NONE);	/* Early Warning Period	*/
+}
 
+/**
+ * Power Management
+ */
+void powermananger_init(void)
+{
+  system_apb_clock_clear_mask(SYSTEM_CLOCK_APB_APBA,
+			      PM_APBAMASK_EIC | /* EIC is unused */
+			      PM_APBAMASK_RTC); /* RTC is unused */
+}
+
+
+/**
+ * Telemetry String
+ * =============================================================================
+ */
+void output_telemetry_string(void)
+{
+  double lat_fmt = 0.0;
+  double lon_fmt = 0.0;
+  uint32_t altitude = 0;
+
+  /**
+   * Callsign, Time
+   * ---------------------------------------------------------------------------
+   */
+
+  /* GPS Time */
+  gps_update_time();
+
+  /* Sleep Wait */
+  while (gps_update_time_pending()) {
+    system_sleep();
+  }
+  for (int i = 0; i < 100*1000; i++);
+
+  /* Time */
+  struct ubx_nav_timeutc time = gps_get_nav_timeutc();
+  uint8_t hours = time.payload.hour;
+  uint8_t minutes = time.payload.min;
+  uint8_t seconds = time.payload.sec;
+
+  /* init double buffers */
+  ARRAY_DBUFFER_INIT(&rtty_dbuffer_string);
+
+  /* sprintf - initial string */
+  uint16_t len = sprintf(ARRAY_DBUFFER_WRITE_PTR(&rtty_dbuffer_string),
+			 "$$%s,%02u:%02u:%02u,",
+			 CALLSIGN, hours, minutes, seconds);
+
+  /* swap buffers */
+  ARRAY_DBUFFER_SWAP(&rtty_dbuffer_string);
+
+  /* start */
+  rtty_start();
+
+  /**
+   * Position, Status, Analogue, Checksum
+   * ---------------------------------------------------------------------------
+   */
+
+  /* Analogue */
+  float battery = get_battery();
+  float temperature = si4060_get_temperature();
+
+  /* Sleep Wait */
+  while (rtty_get_index() < (len - 4)) {
+    system_sleep();
+  }
+
+  /* Request updates from the gps */
+  gps_update_position();
+  if (gps_is_locked()) {
+    led_on();
+  } else {
+    led_off();
+  }
+
+  /* Wait for the gps update. Move on if it's urgent */
+  while (gps_update_position_pending() && rtty_get_index() < (len - 1)) {
+    system_sleep();
+  }
+
+  if (gps_is_locked()) {
+    led_off();
+  } else {
+    led_on();
+  }
+
+  /* GPS Status */
+  struct ubx_nav_sol sol = gps_get_nav_sol();
+  uint8_t lock = sol.payload.gpsFix;
+  uint8_t satillite_count = sol.payload.numSV;
+
+  /* GPS Position */
+  if (lock == 0x2 || lock == 0x3 || lock == 0x4) {
+    struct ubx_nav_posllh pos = gps_get_nav_posllh();
+    lat_fmt = (double)pos.payload.lat / 10000000.0;
+    lon_fmt = (double)pos.payload.lon / 10000000.0;
+    altitude = pos.payload.height / 1000;
+  }
+
+  /* sprintf - full string */
+  len = sprintf(ARRAY_DBUFFER_WRITE_PTR(&rtty_dbuffer_string),
+		"$$%s,%02u:%02u:%02u,%02.6f,%03.6f,%ld,%u,%.2f,%.1f",
+		CALLSIGN, hours, minutes, seconds, lat_fmt, lon_fmt,
+		altitude, satillite_count, battery, temperature);
+
+  /* sprintf - checksum */
+  len += sprintf(ARRAY_DBUFFER_WRITE_PTR(&rtty_dbuffer_string) + len,
+		 "*%04X\n",
+		 crc_checksum(ARRAY_DBUFFER_WRITE_PTR(&rtty_dbuffer_string)));
+
+  /* swap buffers */
+  ARRAY_DBUFFER_SWAP(&rtty_dbuffer_string);
+
+  /**
+   * End
+   * ---------------------------------------------------------------------------
+   */
+
+  /* Set the final length */
+  rtty_set_length(len);
+
+  /* Sleep Wait */
+  while (rtty_active()) {
+    system_sleep();
+  }
+}
+
+/**
+ * MAIN
+ * =============================================================================
+ */
 int main(void)
 {
   /**
@@ -189,18 +328,24 @@ int main(void)
   SystemCoreClock = system_cpu_clock_get_hz();
 
   /* Configure Sleep Mode */
-  system_set_sleepmode(SYSTEM_SLEEPMODE_IDLE_0);
-  //TODO: system_set_sleepmode(SYSTEM_SLEEPMODE_STANDBY);
+  //system_set_sleepmode(SYSTEM_SLEEPMODE_STANDBY);
+  system_set_sleepmode(SYSTEM_SLEEPMODE_IDLE_2); /* Disable CPU, AHB and APB */
 
-  /* Configure the SysTick for 50Hz triggering */
-  SysTick_Config(SystemCoreClock / 50);
+  /* Configure the Power Manager */
+  powermananger_init();
 
+  /* Timer 0 for 50Hz triggering */
+  timer0_tick_init(50);
 
   /**
    * System initialisation
    * ---------------------------------------------------------------------------
    */
 
+  /* Set the wdt here. We should get to the first reset in one min */
+  wdt_init();
+  wdt_reset_count();
+
   led_init();
   gps_init();
 
@@ -227,31 +372,25 @@ int main(void)
 
   //si_trx_state_tx();
 
+  led_on();
+
   while (1) {
-    /* Send the last packet */
-    while (rtty_active());
+    /* Watchdog */
+    wdt_reset_count();
 
-    /* Send requests to the gps */
-    gps_update();
-
-    /* Wait between frames */
-    led_on();
-    for (int i = 0; i < 100*1000; i++);
-    led_off();
-    for (int i = 0; i < 100*1000; i++);
-
-    /* Set the next packet */
-    set_telemetry_string();
-
-    //system_sleep();
+    /* Send the next packet */
+    output_telemetry_string();
   }
 }
 
 /**
  * Called at 50Hz
  */
-void SysTick_Handler(void)
+void TC0_Handler(void)
 {
-  /* Output RTTY */
-  rtty_tick();
+  if (tc_get_status(TC0) & TC_STATUS_CHANNEL_0_MATCH) {
+    tc_clear_status(TC0, TC_STATUS_CHANNEL_0_MATCH);
+
+    rtty_tick();
+  }
 }
diff --git a/firmware/src/rtty.c b/firmware/src/rtty.c
index f452dc4..1c5ef13 100644
--- a/firmware/src/rtty.c
+++ b/firmware/src/rtty.c
@@ -25,6 +25,7 @@
 #include <string.h>
 
 #include "samd20.h"
+#include "rtty.h"
 #include "hw_config.h"
 #include "system/port.h"
 
@@ -33,7 +34,7 @@
  */
 #define RTTY_ACTIVATE()
 #define RTTY_DEACTIVATE()
-#define RTTY_SET(b)	    port_pin_set_output_level(SI406X_GPIO1_PIN, b);
+#define RTTY_SET(b)	    port_pin_set_output_level(SI406X_GPIO1_PIN, !b);
 #define RTTY_NEXT()
 
 /**
@@ -42,11 +43,6 @@
 #define ASCII_BITS	8
 #define BITS_PER_CHAR	11
 
-/**
- * Output String
- */
-#define RTTY_STRING_MAX	0x200
-
 /**
  * Where we currently are in the rtty output byte
  *
@@ -59,13 +55,12 @@ uint8_t rtty_phase;
 /**
  * Where we are in the current output string
  */
-uint32_t rtty_index;
+int32_t rtty_index;
 
 /**
  * Details of the string that is currently being output
  */
-char rtty_string[RTTY_STRING_MAX];
-uint32_t rtty_string_length = 0;
+int32_t rtty_string_length = 0;
 
 /**
  * Returns 1 if we're currently outputting.
@@ -75,25 +70,35 @@ int rtty_active(void) {
 }
 
 /**
- * Sets an output string.
+ * Starts RTTY output
  *
- * Returns 0 on success, 1 if a string is already active or 2 if the
- * specified string was too long.
+ * Returns 0 on success, 1 if already active
  */
-int rtty_set_string(char* string, uint32_t length) {
-  if (length > RTTY_STRING_MAX) return 2; // To long
-
+int rtty_start(void) {
   if (!rtty_active()) {
-    // Copy
-    memcpy(rtty_string, string, length);
-    rtty_string_length = length;
-    // Initialise
+
+    /* Initialise */
+    rtty_string_length = RTTY_STRING_MAX;
     rtty_index = 0;
     rtty_phase = 0;
 
-    return 0; // Success
+    return 0; /* Success */
   } else {
-    return 1; // Already active
+    return 1; /* Already active */
+  }
+}
+/**
+ * Returns the index of the current byte being outputted from the buffer
+ */
+int32_t rtty_get_index(void) {
+  return rtty_index;
+}
+/**
+ * Sets the final length of the RTTY string
+ */
+void rtty_set_length(int32_t length) {
+  if (length <= RTTY_STRING_MAX) {
+    rtty_string_length = length;
   }
 }
 
@@ -106,21 +111,17 @@ void rtty_tick(void) {
 
     if (rtty_phase == 0) { // Start
       // Low
-      //RTTY_SET(0);
-      port_pin_set_output_level(SI406X_GPIO1_PIN, 0);
+      RTTY_SET(0);
     } else if (rtty_phase < ASCII_BITS + 1) {
       // Data
-      if ((rtty_string[rtty_index] >> (rtty_phase - 1)) & 1) {
-	//RTTY_SET(1);
-	port_pin_set_output_level(SI406X_GPIO1_PIN, 1);
+      if ((ARRAY_DBUFFER_READ_PTR(&rtty_dbuffer_string)[rtty_index] >> (rtty_phase - 1)) & 1) {
+	RTTY_SET(1);
       } else {
-	//RTTY_SET(0);
-	port_pin_set_output_level(SI406X_GPIO1_PIN, 0);
+	RTTY_SET(0);
       }
     } else if (rtty_phase < BITS_PER_CHAR) { // Stop
       // High
-      //RTTY_SET(1);
-      port_pin_set_output_level(SI406X_GPIO1_PIN, 1);
+      RTTY_SET(1);
     }
 
     rtty_phase++;
@@ -133,7 +134,7 @@ void rtty_tick(void) {
       }
     }
   } else {
-//    RTTY_DEACTIVATE();
+    RTTY_DEACTIVATE();
   }
 }
 
diff --git a/firmware/src/si4060.c b/firmware/src/si4060.c
index 0a1acb6..95f3c70 100644
--- a/firmware/src/si4060.c
+++ b/firmware/src/si4060.c
@@ -362,7 +362,7 @@ float si4060_get_temperature(void)
   spi_select();
   spi_write(CMD_GET_ADC_READING);
   spi_write(0x10); /* Temperature */
-  spi_write((0xC << 4) | 0); /* UDTIME = 0xC, Att = 0 */
+  spi_write(0); /* ADC_CFG = 0 */
   spi_deselect();
 
   /* do not deselect after reading CTS */
@@ -377,12 +377,15 @@ float si4060_get_temperature(void)
   spi_read(); spi_read();
 
   /* Battery */
-  result = (spi_read() & 0x7) << 8;
-  result |= spi_read() & 0xFF;
+  spi_read(); spi_read();
+
+  /* Temperature */
+  result =  (spi_read() & 0xFF) << 8;
+  result |= (spi_read() & 0xFF);
 
   spi_deselect();
 
-  return ((((float)result) * 568.0) / 2560.0) - 297.0;
+  return (((float)result * 568.0) / 2560.0) - 297.0;
 }
 
 /*
diff --git a/firmware/src/system/wdt.c b/firmware/src/system/wdt.c
index 1f67497..16cf19f 100644
--- a/firmware/src/system/wdt.c
+++ b/firmware/src/system/wdt.c
@@ -165,6 +165,6 @@ void wdt_reset_count(void)
 
   WDT_WAIT_FOR_SYNC(hw);
 
-  /* Disable the Watchdog module */
+  /* Reset the Watchdog module */
   hw->CLEAR.reg = WDT_CLEAR_CLEAR_KEY;
 }
diff --git a/firmware/src/telemetry.c b/firmware/src/telemetry.c
index e939933..08dddb7 100644
--- a/firmware/src/telemetry.c
+++ b/firmware/src/telemetry.c
@@ -26,16 +26,10 @@
 #include <string.h>
 
 #include "samd20.h"
-#include "semihosting.h"
-#include "analogue.h"
-#include "gps.h"
-#include "rtty.h"
-#include "ubx_messages.h"
-#include "si4060.h"
+#include "system/gclk.h"
+#include "system/interrupt.h"
+#include "tc/tc_driver.h"
 
-//#define SEMIHOST_LOG
-
-char telemetry_string[0x200];
 
 /**
  * CRC Function for the XMODEM protocol.
@@ -77,52 +71,46 @@ uint16_t crc_checksum(char *string)
 
   return crc;
 }
+
 /**
- * Sets output telemetry
+ * Initialises a timer interupt at the given frequency
  */
-void set_telemetry_string(void)
+void timer0_tick_init(uint32_t frequency)
 {
-  double lat_fmt = 0.0;
-  double lon_fmt = 0.0;
-  uint32_t altitude = 0;
+  /* Calculate the wrap value for the given frequency */
+  uint32_t gclk0_frequency = system_gclk_chan_get_hz(0);
+  uint32_t count = gclk0_frequency / frequency;
 
-  /* Analogue */
-  float battery = get_battery();
+  /* Configure Timer 0 */
+  bool t0_capture_channel_enables[]    = {false, false};
+  uint32_t t0_compare_channel_values[] = {count, 0x0000};
+  tc_init(TC0,
+	  GCLK_GENERATOR_0,
+	  TC_COUNTER_SIZE_32BIT,
+	  TC_CLOCK_PRESCALER_DIV1,
+	  TC_WAVE_GENERATION_MATCH_FREQ,
+	  TC_RELOAD_ACTION_GCLK,
+	  TC_COUNT_DIRECTION_UP,
+	  TC_WAVEFORM_INVERT_OUTPUT_NONE,
+	  false,			/* Oneshot  */
+	  true,				/* Run in standby */
+	  0x0000,			/* Initial value */
+	  count,			/* Top value */
+	  t0_capture_channel_enables,	/* Capture Channel Enables */
+	  t0_compare_channel_values);	/* Compare Channels Values */
 
-  /* Time */
-  struct ubx_nav_timeutc time = gps_get_nav_timeutc();
-  uint8_t hours = time.payload.hour;
-  uint8_t minutes = time.payload.min;
-  uint8_t seconds = time.payload.sec;
+  /* Enable Events */
+  struct tc_events event;
+  memset(&event, 0, sizeof(struct tc_events));
+  event.generate_event_on_compare_channel[0] = true;
+  event.event_action = TC_EVENT_ACTION_RETRIGGER;
+  tc_enable_events(TC0, &event);
 
-  /* GPS Status */
-  struct ubx_nav_sol sol = gps_get_nav_sol();
-  uint8_t lock = sol.payload.gpsFix;
-  uint8_t satillite_count = sol.payload.numSV;
+  /* Enable Interrupt */
+  TC0->COUNT32.INTENSET.reg = (1 << 4);
+  irq_register_handler(TC0_IRQn, 0); /* Highest Priority */
 
-  /* GPS Position */
-  if (lock == 0x2 || lock == 0x3 || lock == 0x4) {
-    struct ubx_nav_posllh pos = gps_get_nav_posllh();
-    lat_fmt = (double)pos.payload.lat / 10000000.0;
-    lon_fmt = (double)pos.payload.lon / 10000000.0;
-    altitude = pos.payload.height / 1000;
-  }
-
-
-//#ifdef SEMIHOST_LOG
-//  semihost_printf("Batt %f, Temp %f\n", battery, temperature);
-//  semihost_printf("%02.7f,%03.7f,%ld\n", lat_fmt, lon_fmt, altitude);
-//  semihost_printf("Lock: %d Sats: %d\n", lock, satillite_count);
-//  semihost_printf("%02u:%02u:%02u\n", hours, minutes, seconds);
-//#endif
-
-  /* sprintf */
-  uint16_t len = sprintf(telemetry_string,
-			 "$$UBSEDS2,%02u:%02u:%02u,%02.6f,%03.6f,%ld,%u,%.2f",
-			 hours, minutes, seconds, lat_fmt, lon_fmt, altitude,
-			 satillite_count, battery);
-
-  sprintf(telemetry_string + len, "*%04X\n", crc_checksum(telemetry_string));
-
-  rtty_set_string(telemetry_string, strlen(telemetry_string));
+  /* Enable Timer */
+  tc_enable(TC0);
+  tc_start_counter(TC0);
 }