diff --git a/firmware/inc/contestia.h b/firmware/inc/contestia.h
index f34e2bf..68f9e07 100644
--- a/firmware/inc/contestia.h
+++ b/firmware/inc/contestia.h
@@ -35,6 +35,7 @@
 
 
 void contestia_start(char* data);
+void contestia_preamble(void);
 uint8_t contestia_tick(void);
 
 #endif /* CONTESTIA_H */
diff --git a/firmware/inc/rtty.h b/firmware/inc/rtty.h
index 7221585..f29fffb 100644
--- a/firmware/inc/rtty.h
+++ b/firmware/inc/rtty.h
@@ -33,6 +33,7 @@
 
 
 void rtty_start(uint8_t data);
+void rtty_preamble(void);
 uint8_t rtty_tick(void);
 
 #endif /* RTTY_H */
diff --git a/firmware/inc/si_trx.h b/firmware/inc/si_trx.h
index a9a5f2d..62b5862 100644
--- a/firmware/inc/si_trx.h
+++ b/firmware/inc/si_trx.h
@@ -29,7 +29,7 @@
 
 float si_trx_get_temperature(void);
 
-void si_trx_on(uint8_t modulation_type);
+void si_trx_on(uint8_t modulation_type, uint16_t deviation);
 void si_trx_off(void);
 void si_trx_switch_channel(int16_t channel);
 
diff --git a/firmware/inc/telemetry.h b/firmware/inc/telemetry.h
index 11ae41b..c259d5a 100644
--- a/firmware/inc/telemetry.h
+++ b/firmware/inc/telemetry.h
@@ -41,18 +41,12 @@ enum telemetry_t {
 /**
  * Output String
  */
-#define TELEMETRY_STRING_MAX	0x1F0
-#define TELEMETRY_LARGEST_BLOCK	0x10
-/**
- * It's actually a double buffer which we swap for mid-string updates
- */
-ARRAY_DBUFFER_T(char, TELEMETRY_STRING_MAX+TELEMETRY_LARGEST_BLOCK) telemetry_dbuffer_string;
+#define TELEMETRY_STRING_MAX	0x200
+char telemetry_string[TELEMETRY_STRING_MAX];
 
 int telemetry_active(void);
-int telemetry_start(enum telemetry_t type);
+int telemetry_start(enum telemetry_t type, int32_t length);
 int telemetry_start_rsid(rsid_code_t rsid);
-int32_t telemetry_get_index(void);
-void telemetry_set_length(int32_t length);
 
 
 float timer0_tick_init(float frequency);
diff --git a/firmware/src/contestia.c b/firmware/src/contestia.c
index 3fd1d5b..22aeb5a 100644
--- a/firmware/src/contestia.c
+++ b/firmware/src/contestia.c
@@ -28,6 +28,8 @@
 #include "si_trx.h"
 #include "mfsk.h"
 
+#define PREAMBLE_LENGTH CONTESTIA_NUMBER_OF_TONES
+
 /**
  * Current output tones
  */
@@ -36,6 +38,7 @@ int8_t contestia_tones[CONTESTIA_NUMBER_OF_TONES];
  * Where we are in the current output tones
  */
 uint32_t contestia_tone_index = 0xFFFFFFFE;
+uint8_t contestia_preamble_index = 0;
 
 /**
  * Starts the transmission of a contestia block
@@ -45,24 +48,44 @@ void contestia_start(char* block) {
   contestia_mfsk_encode_block(block, contestia_tones);
   contestia_tone_index = 0;
 }
+void contestia_preamble(void) {
+  contestia_preamble_index = PREAMBLE_LENGTH;
+}
+
+void contestia_set_tone(uint8_t tone) {
+  /* Align this to a channel */
+  int16_t channel = tone - (CONTESTIA_NUMBER_OF_TONES / 2);
+
+  si_trx_switch_channel(channel * CONTESTIA_CHANNEL_SPACING);
+}
 
 /**
  * Called at the baud rate, outputs tones
  */
 uint8_t contestia_tick(void) {
 
+  if (contestia_preamble_index) {
+    contestia_preamble_index--;
+
+    if (contestia_preamble_index & (CONTESTIA_NUMBER_OF_TONES/4)) {
+      contestia_set_tone(CONTESTIA_NUMBER_OF_TONES - 1);
+    } else {
+      contestia_set_tone(0);
+    }
+
+    return 1;
+  }
+
   if (contestia_tone_index < CONTESTIA_NUMBER_OF_TONES) {
     uint8_t binary_code;
     uint8_t grey_code;
-    int16_t channel;
 
     /* Output grey code */
     binary_code = contestia_tones[contestia_tone_index];
     grey_code = (binary_code >> 1) ^ binary_code;
 
-    /* Align this to a channel */
-    channel = grey_code - (CONTESTIA_NUMBER_OF_TONES / 2);
-    si_trx_switch_channel(channel * CONTESTIA_CHANNEL_SPACING);
+    /* Transmit this tone */
+    contestia_set_tone(grey_code);
 
   } else {
     return 0;
diff --git a/firmware/src/main.c b/firmware/src/main.c
index bd0f625..df2a9fe 100644
--- a/firmware/src/main.c
+++ b/firmware/src/main.c
@@ -46,7 +46,7 @@
 #include "spi_bitbang.h"
 #include "system/interrupt.h"
 
-#define CALLSIGN	"UBSEDSx"
+#define CALLSIGN	"UBSEDSX"
 
 /* Set the modulation mode */
 //#define RTTY
@@ -153,6 +153,8 @@ void output_telemetry_string(enum telemetry_t type)
   double lat_fmt = 0.0;
   double lon_fmt = 0.0;
   uint32_t altitude = 0;
+  uint16_t len;
+  uint8_t dollars = 5;
 
   /**
    * Analogue, Callsign, Time
@@ -161,7 +163,7 @@ void output_telemetry_string(enum telemetry_t type)
 
   /* Analogue */
   float battery = get_battery();
-  float temperature = si_trx_get_temperature(); // REENTRANCY!!!!!!
+  float temperature = si_trx_get_temperature(); // Requires control of the radio - radio on also??
 
   /* GPS Time */
   gps_update_time();
@@ -177,66 +179,23 @@ void output_telemetry_string(enum telemetry_t type)
   uint8_t minutes = time.payload.min;
   uint8_t seconds = time.payload.sec;
 
-  /* init double buffers */
-  ARRAY_DBUFFER_INIT(&telemetry_dbuffer_string);
-
-  /* sprintf - initial string */
-  uint16_t len = sprintf(ARRAY_DBUFFER_WRITE_PTR(&telemetry_dbuffer_string),
-			 "$$%s,%02u:%02u:%02u,",
-			 CALLSIGN, hours, minutes, seconds);
-
-  /* swap buffers */
-  ARRAY_DBUFFER_SWAP(&telemetry_dbuffer_string);
-
-
-
-/**
- * Starting up the radio blocks on high-prio interrupt for ~100ms: todo fixme
- *
- * Therefore don't touch gps until it's done
- */
-
-  /* RSID */
-  /* start - SI NOW BELONGS TO TELEMETRY, WE CANNOT ACCESS */
-  if (type == TELEMETRY_CONTESTIA) {
-    telemetry_start_rsid(RSID_CONTESTIA_32_1000);
-  }
-
-  /* Sleep Wait for RSID to be done */
-  while (telemetry_active()) {
-    system_sleep();
-  }
-
-  /* Main telemetry */
-  telemetry_start(type);
-
-  /**
-   * Position, Status, Checksum
-   * ---------------------------------------------------------------------------
-   */
-
-  /* Sleep Wait */
-  while (telemetry_get_index() < (len - 9)) {
-    system_sleep();
-  }
-
   /* Request updates from the gps */
   gps_update_position();
   if (gps_is_locked()) {
-//    led_on();
+    led_on();
   } else {
-//    led_off();
+    led_off();
   }
 
-  /* Wait for the gps update. Move on if it's urgent */
-  while (gps_update_position_pending() && telemetry_get_index() < (len - 6)) {
+  /* Wait for the gps update */
+  while (gps_update_position_pending()) {
     system_sleep();
   }
 
   if (gps_is_locked()) {
-//    led_off();
+    led_off();
   } else {
-//    led_on();
+    led_on();
   }
 
   /* GPS Status */
@@ -252,27 +211,40 @@ void output_telemetry_string(enum telemetry_t type)
     altitude = pos.payload.height / 1000;
   }
 
+  /* sprintf - preamble */
+  memset(telemetry_string, '$', dollars);
+  len = dollars;
+
   /* sprintf - full string */
-  len = sprintf(ARRAY_DBUFFER_WRITE_PTR(&telemetry_dbuffer_string),
-		"$$%s,%02u:%02u:%02u,%02.6f,%03.6f,%ld,%u,%.2f,%.1f",
+  len += sprintf(telemetry_string + len,
+		"%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(&telemetry_dbuffer_string) + len,
+  /* sprintf - checksum. don't include dollars */
+  len += sprintf(telemetry_string + len,
 		 "*%04X\r",
-		 crc_checksum(ARRAY_DBUFFER_WRITE_PTR(&telemetry_dbuffer_string)));
+		 crc_checksum(telemetry_string + 5));
 
-  /* swap buffers */
-  ARRAY_DBUFFER_SWAP(&telemetry_dbuffer_string);
 
-  /**
-   * End
-   * ---------------------------------------------------------------------------
-   */
 
-  /* Set the final length */
-  telemetry_set_length(len);
+/**
+ * Starting up the radio blocks on high-prio interrupt for ~100ms: todo fixme
+ */
+
+  /* RSID */
+  /* start - SI NOW BELONGS TO TELEMETRY, WE CANNOT ACCESS */
+  if (type == TELEMETRY_CONTESTIA) {
+    telemetry_start_rsid(RSID_CONTESTIA_32_1000);
+  }
+
+  /* Sleep Wait for RSID to be done */
+  while (telemetry_active()) {
+    system_sleep();
+  }
+
+  /* Main telemetry */
+  telemetry_start(type, len);
 
   /* Sleep Wait */
   while (telemetry_active()) {
@@ -329,8 +301,7 @@ int main(void)
   /* Initialise Si4060 interface */
   si_trx_init();
 
-//  led_on();
-
+  led_on();
 
   while (1) {
     /* Watchdog */
@@ -339,8 +310,20 @@ int main(void)
     /* Send the next packet */
     output_telemetry_string(TELEMETRY_RTTY);
 
-    telemetry_start(TELEMETRY_PIPS);
-    telemetry_set_length(5);
+    telemetry_start(TELEMETRY_PIPS, 5);
+
+    /* Sleep Wait */
+    while (telemetry_active()) {
+      system_sleep();
+    }
+
+
+
+
+    /* Send the next packet */
+    output_telemetry_string(TELEMETRY_CONTESTIA);
+
+    telemetry_start(TELEMETRY_PIPS, 5);
 
     /* Sleep Wait */
     while (telemetry_active()) {
diff --git a/firmware/src/rtty.c b/firmware/src/rtty.c
index a95f63c..3f181ce 100644
--- a/firmware/src/rtty.c
+++ b/firmware/src/rtty.c
@@ -28,15 +28,17 @@
 #include "rtty.h"
 #include "hw_config.h"
 #include "si_trx.h"
+#include "system/port.h"
 
 
 /**
  * Interface to the physical world.
  */
 #define RTTY_CHANNEL_DEVIATION	(RTTY_CHANNEL_SPACING / 2)
-#define RTTY_CHANNEL(b)		(b ? -RTTY_CHANNEL_DEVIATION : RTTY_CHANNEL_DEVIATION)
+#define RTTY_CHANNEL(b)		(b ? RTTY_CHANNEL_DEVIATION : -RTTY_CHANNEL_DEVIATION)
 #define RTTY_SET(b)		si_trx_switch_channel(RTTY_CHANNEL(b))
 
+//#define RTTY_SET(b)		port_pin_set_output_level(SI406X_GPIO1_PIN, b);
 
 /**
  * Formatting 8N2
@@ -44,6 +46,8 @@
 #define ASCII_BITS	8
 #define BITS_PER_CHAR	11
 
+#define PREAMBLE_LENGTH  50
+
 /**
  * Current output data
  */
@@ -57,18 +61,28 @@ uint8_t rtty_data;
  * 11 = Stop Bit
  */
 uint8_t rtty_phase = 0xFE;
+uint8_t rtty_preamble_count = 0;
 
 void rtty_start(uint8_t data) {
   /* Start transmission */
   rtty_phase = 0;
   rtty_data = data;
 }
+void rtty_preamble(void) {
+  rtty_preamble_count = PREAMBLE_LENGTH;
+}
 
 /**
  * Called at the baud rate, outputs bits of rtty
  */
 uint8_t rtty_tick(void) {
 
+  if (rtty_preamble_count) { /* Do preamble */
+    rtty_preamble_count--;
+    RTTY_SET(1);
+    return 1;
+  }
+
   if (rtty_phase == 0) {			/* *** Start *** */
     RTTY_SET(0);
 
diff --git a/firmware/src/si_trx.c b/firmware/src/si_trx.c
index f54d3ff..6dc7f1e 100644
--- a/firmware/src/si_trx.c
+++ b/firmware/src/si_trx.c
@@ -35,11 +35,6 @@
 #define VCXO_FREQUENCY	SI406X_TCXO_FREQUENCY
 #define RF_DEVIATION	200
 
-/**
- * The LSB tuning resolution of the frac-n pll as currently
- * configured.
- */
-float lsb_tuning_resolution = 0;
 
 
 /**
@@ -299,9 +294,12 @@ static void si_trx_set_tx_pa_duty_cycle(uint8_t pa_duty_cycle)
 /**
  * Set the synthesiser to the given frequency.
  *
+ * frequency: Floating-point value for the frequency
+ * deviation: FSK-mode deviation, in channels. Usually 1
+ *
  * Returns the LSB tuning resolution of the frac-n pll synthesiser.
  */
-static float si_trx_set_frequency(uint32_t frequency)
+static float si_trx_set_frequency(uint32_t frequency, uint16_t deviation)
 {
   uint8_t outdiv, band, nprescaler;
 
@@ -351,7 +349,7 @@ static float si_trx_set_frequency(uint32_t frequency)
   si_trx_frequency_control_set_divider(n, m);
 
   /* Set the external pin frequency deviation to the LSB tuning resoultion */
-  si_trx_modem_set_deviation(1);
+  si_trx_modem_set_deviation(deviation);
 
   /* Return the LSB tuning resolution of the frac-n pll synthesiser. */
   return f_pfd / (float)(1 << 19);
@@ -360,7 +358,7 @@ static float si_trx_set_frequency(uint32_t frequency)
 /**
  * Resets the transceiver
  */
-void si_trx_reset(uint8_t modulation_type)
+void si_trx_reset(uint8_t modulation_type, uint16_t deviation)
 {
   _si_trx_sdn_enable();  /* active high shutdown = reset */
 
@@ -387,7 +385,7 @@ void si_trx_reset(uint8_t modulation_type)
                                 SI_GPIO_PIN_CFG_GPIO_MODE_INPUT | SI_GPIO_PIN_CFG_PULL_ENABLE,
                                 SI_GPIO_PIN_CFG_DRV_STRENGTH_LOW);
 
-  si_trx_set_frequency(RADIO_FREQUENCY);
+  si_trx_set_frequency(RADIO_FREQUENCY, deviation);
   si_trx_set_tx_power(RADIO_POWER);
 
   /* RTTY from GPIO1 */
@@ -402,9 +400,9 @@ void si_trx_reset(uint8_t modulation_type)
 /**
  * Enables the radio and starts transmitting
  */
-void si_trx_on(uint8_t modulation_type)
+void si_trx_on(uint8_t modulation_type, uint16_t deviation)
 {
-  si_trx_reset(modulation_type);
+  si_trx_reset(modulation_type, deviation);
   si_trx_start_tx(0);
 }
 /**
@@ -413,6 +411,8 @@ void si_trx_on(uint8_t modulation_type)
 void si_trx_off(void)
 {
   si_trx_state_ready();
+
+  /* Physical shutdown */
   _si_trx_sdn_enable();
 }
 
diff --git a/firmware/src/telemetry.c b/firmware/src/telemetry.c
index 41d8dd2..d511492 100644
--- a/firmware/src/telemetry.c
+++ b/firmware/src/telemetry.c
@@ -40,24 +40,6 @@
 #include "hw_config.h"
 
 
-#include "system/port.h"
-/**
- * Turns the status LED on
- */
-static inline void led_on(void)
-{
-  port_pin_set_output_level(LED0_PIN, 0);	/* LED is active low */
-}
-/**
- * Turns the status lED off
- */
-static inline void led_off(void)
-{
-  port_pin_set_output_level(LED0_PIN, 1);	/* LED is active low */
-}
-
-
-
 /**
  * CYCLIC REDUNDANCY CHECK (CRC)
  * =============================================================================
@@ -95,8 +77,7 @@ uint16_t crc_checksum(char *string)
 
   crc = 0xFFFF;
 
-  // Calculate checksum ignoring the first two $s
-  for (i = 2; i < strlen(string); i++) {
+  for (i = 0; i < strlen(string); i++) {
     c = string[i];
     crc = crc_xmodem_update(crc, c);
   }
@@ -140,13 +121,13 @@ int telemetry_active(void) {
  *
  * Returns 0 on success, 1 if already active
  */
-int telemetry_start(enum telemetry_t type) {
+int telemetry_start(enum telemetry_t type, int32_t length) {
   if (!telemetry_active()) {
 
     /* Initialise */
     telemetry_type = type;
     telemetry_index = 0;
-    telemetry_string_length = TELEMETRY_STRING_MAX;
+    telemetry_string_length = length;
 
     /* Setup timer tick */
     switch(telemetry_type) {
@@ -193,23 +174,11 @@ int telemetry_start_rsid(rsid_code_t rsid) {
   }
 }
 
-/**
- * Returns the index of the current byte being outputted from the buffer
- */
-int32_t telemetry_get_index(void) {
-  return telemetry_index;
-}
-/**
- * Sets the final length of the TELEMETRY string
- */
-void telemetry_set_length(int32_t length) {
-  if (length <= TELEMETRY_STRING_MAX) {
-    telemetry_string_length = length;
-  }
-}
+
+
 
 uint8_t is_telemetry_finished(void) {
-  if (telemetry_index > telemetry_string_length) {
+  if (telemetry_index >= telemetry_string_length) {
     /* All done, deactivate */
     telemetry_string_length = 0;
 
@@ -223,8 +192,6 @@ uint8_t is_telemetry_finished(void) {
   }
   return 0;
 }
-
-
 /**
  * Called at the telemetry mode's baud rate
  */
@@ -235,8 +202,9 @@ void telemetry_tick(void) {
 
       if (!radio_on) {
         /* Contestia: We use the modem offset to modulate */
-        si_trx_on(SI_MODEM_MOD_TYPE_CW);
+        si_trx_on(SI_MODEM_MOD_TYPE_CW, 1);
         radio_on = 1;
+        contestia_preamble();
       }
 
       if (!contestia_tick()) {
@@ -244,7 +212,7 @@ void telemetry_tick(void) {
         if (is_telemetry_finished()) return;
 
         /* Let's start again */
-        char* block = &ARRAY_DBUFFER_READ_PTR(&telemetry_dbuffer_string)[telemetry_index];
+        char* block = &telemetry_string[telemetry_index];
         telemetry_index += CONTESTIA_CHARACTERS_PER_BLOCK;
 
         contestia_start(block);
@@ -255,8 +223,9 @@ void telemetry_tick(void) {
 
       if (!radio_on) {
         /* RTTY: We use the modem offset to modulate */
-        si_trx_on(SI_MODEM_MOD_TYPE_CW);
+        si_trx_on(SI_MODEM_MOD_TYPE_CW, 1);
         radio_on = 1;
+        rtty_preamble();
       }
 
       if (!rtty_tick()) {
@@ -264,7 +233,7 @@ void telemetry_tick(void) {
         if (is_telemetry_finished()) return;
 
         /* Let's start again */
-        uint8_t data = ARRAY_DBUFFER_READ_PTR(&telemetry_dbuffer_string)[telemetry_index];
+        uint8_t data = telemetry_string[telemetry_index];
         telemetry_index++;
 
         rtty_start(data);
@@ -282,7 +251,7 @@ void telemetry_tick(void) {
 
       if (!radio_on) {
         /* RSID: We PWM frequencies with the external pin */
-        si_trx_on(SI_MODEM_MOD_TYPE_2FSK);
+        si_trx_on(SI_MODEM_MOD_TYPE_2FSK, 1);
         telemetry_gpio1_pwm_init();
 
         radio_on = 1;
@@ -302,7 +271,7 @@ void telemetry_tick(void) {
 
       if (!radio_on) { /* Turn on */
         /* Pips: Cw */
-        si_trx_on(SI_MODEM_MOD_TYPE_CW); radio_on = 1;
+        si_trx_on(SI_MODEM_MOD_TYPE_CW, 1); radio_on = 1;
         timer0_tick_frequency(PIPS_ON_FREQUENCY);
 
       } else { /* Turn off */
@@ -348,8 +317,6 @@ float timer0_tick_init(float frequency)
 {
   //si_gclk_setup();
 
-  led_on();
-
   /* Calculate the wrap value for the given frequency */
   //float gclk_frequency = SI406X_TCXO_FREQUENCY;
   float gclk_frequency = (float)system_gclk_chan_get_hz(0);