diff --git a/firmware/Peripherals.md b/firmware/Peripherals.md
index 9d5fb69..3504b29 100644
--- a/firmware/Peripherals.md
+++ b/firmware/Peripherals.md
@@ -5,7 +5,6 @@
 |*GLCK*|
 ||gclk0|main clock, internal osc8m|4 MHz
 ||gclk1|tcxo clock, fed from xosc
-||gclk2|rtc feed, fed from gclk1. divide by 4096
 
 |*TC*||
 ||tc0|telemetry tick timer. 32-bit
@@ -13,7 +12,7 @@
 ||tc2|counts cycles of tcxo. 32-bit
 ||tc3|^^^^^
 ||tc4|osc8m event source
-||tc5|telemetry pwm 8-bit
+||tc5|telemetry pwm 16-bit
 
 |*EXTINT*|
 ||extint[5]|gps timepulse
diff --git a/firmware/inc/aprs.h b/firmware/inc/aprs.h
new file mode 100644
index 0000000..898b06b
--- /dev/null
+++ b/firmware/inc/aprs.h
@@ -0,0 +1,30 @@
+/*
+ * Outputs aprs to the si_trx
+ * Copyright (C) 2015  Richard Meadows <richardeoin>
+ *
+ * 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.
+ */
+
+#ifndef APRS_H
+#define APRS_H
+
+
+
+#endif /* APRS_H */
diff --git a/firmware/inc/ax25.h b/firmware/inc/ax25.h
new file mode 100644
index 0000000..0c86de6
--- /dev/null
+++ b/firmware/inc/ax25.h
@@ -0,0 +1,77 @@
+/*
+ * Outputs ax25 to the si_trx
+ * Copyright (C) 2015  Richard Meadows <richardeoin>
+ *
+ * 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.
+ */
+
+#ifndef AX25_H
+#define AX25_H
+
+#include "ax25_sintable.h"
+
+/**
+ * Parameters based on the size of the sintable
+ */
+#define AX25_SINTABLE_SIZE	(AX25_SINTABLE_LENGTH*4)
+#define AX25_SINTABLE_LUMASK	(AX25_SINTABLE_LENGTH-1)
+#define AX25_SINTABLE_MASK	(AX25_SINTABLE_SIZE-1)
+#define AX25_SINTABLE_PART(phase)	((phase >> AX25_SINTABLE_ORDER) & 3)
+
+/**
+ * Bell-202
+ */
+#define AX25_BAUD		2200
+#define AX25_MARKFREQ		1
+#define AX25_SPACEFREQ		2
+
+#define AX25_BITSTUFFINGCOUNT	5
+
+
+
+/**
+ * How many points we output in a single symbol-time
+ */
+#define AX25_OVERSAMPLING	2
+#define AX25_TICK_RATE		(AX25_BAUD * AX25_OVERSAMPLING)
+
+/**
+ * Define the phase velocities for mark and space
+ *
+ * This is how many entries in the sin table we step by
+ */
+#define AX25_MARKPHASEVELOCITY	(AX25_SINTABLE_SIZE/AX25_OVERSAMPLING)
+#define AX25_SPACEPHASEVELOCITY	(AX25_MARKPHASEVELOCITY*(2.2f/1.2f))
+// TODO ^^
+
+
+
+
+
+enum ax25_symbol_t {
+  AX25_MARK,
+  AX25_SPACE,
+};
+
+
+void ax25_start();
+uint8_t ax25_tick(void);
+
+#endif /* AX25_H */
diff --git a/firmware/inc/hw_config.h b/firmware/inc/hw_config.h
index 576daa1..4c416c7 100644
--- a/firmware/inc/hw_config.h
+++ b/firmware/inc/hw_config.h
@@ -121,6 +121,7 @@
 /**
  * XOSC
  */
+//#define USE_XOSC
 #define XOSC_FREQUENCY		16369000
 #define XOSC_GCLK1_DIVIDE	4
 
diff --git a/firmware/inc/si_trx_defs.h b/firmware/inc/si_trx_defs.h
index 27a68d0..91b01f7 100644
--- a/firmware/inc/si_trx_defs.h
+++ b/firmware/inc/si_trx_defs.h
@@ -235,6 +235,11 @@ enum {
   SI_MODEM_MOD_GPIO_3			= (0x03 << 5),
   SI_MODEM_MOD_DIRECT_MODE_SYNC		= (0x00 << 7), /* default */
   SI_MODEM_MOD_DIRECT_MODE_ASYNC	= (0x01 << 7),
+  SI_MODEM_DATA_RATE			= 0x03,
+  SI_MODEM_TX_NCO_MODE			= 0x06,
+  SI_MODEM_TX_NCO_TXOSR_10X		= (0x00 << 26),
+  SI_MODEM_TX_NCO_TXOSR_40X		= (0x01 << 26),
+  SI_MODEM_TX_NCO_TXOSR_20X		= (0x02 << 26),
   SI_MODEM_FREQ_DEV			= 0x0a,
   SI_MODEM_FREQ_OFFSET			= 0x0d,
   SI_MODEM_CLKGEN_BAND			= 0x51,
diff --git a/firmware/inc/telemetry.h b/firmware/inc/telemetry.h
index 4b7b51d..342268c 100644
--- a/firmware/inc/telemetry.h
+++ b/firmware/inc/telemetry.h
@@ -51,7 +51,7 @@ void telemetry_stop(void);
 float telemetry_si_temperature(void);
 
 float timer0_tick_init(float frequency);
-void timer0_tick_frequency(float frequency);
+uint32_t timer0_tick_frequency(float frequency);
 void timer0_tick_deinit();
 void telemetry_gpio1_pwm_init(void);
 void telemetry_gpio1_pwm_duty(float duty_cycle);
diff --git a/firmware/src/ax25.c b/firmware/src/ax25.c
new file mode 100644
index 0000000..583ec5e
--- /dev/null
+++ b/firmware/src/ax25.c
@@ -0,0 +1,111 @@
+/*
+ * Outputs ax25 to the si_trx
+ * Copyright (C) 2015  Richard Meadows <richardeoin>
+ *
+ * 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 "samd20.h"
+#include "ax25.h"
+#include "ax25_sintable.h"
+#include "si_trx.h"
+
+#define AX25_MARKSINTABLE	sintable_512_1500hz
+#define AX25_SPACESINTABLE	sintable_512_2500hz
+
+
+uint32_t ax25_phase;
+uint32_t ax25_phasevelocity;
+uint32_t ax25_oversampling_count;
+
+uint16_t* ax25_sintable;
+
+void ax25_start()
+{
+  /* Init */
+  ax25_phase = 0;
+  ax25_phasevelocity = AX25_MARKPHASEVELOCITY;
+  ax25_sintable = AX25_MARKSINTABLE;
+  ax25_oversampling_count = -1LL;
+}
+
+
+uint32_t toggle = 0;
+enum ax25_symbol_t ax25_get_next_symbol(void) {
+  return (toggle++ & 1) ? AX25_MARK : AX25_SPACE;
+}
+
+
+void telemetry_gpio1_pwm_duty(float duty_cycle);
+
+/**
+ * Called at our tick rate, outputs tones
+ *
+ * Returns 1 when more work todo, 0 when finished
+ */
+uint8_t ax25_tick(void)
+{
+  int16_t deviation;
+  uint32_t sintable_phase = ax25_phase & AX25_SINTABLE_LUMASK;
+
+  /* Set the instantaneous fm deviation based on the current phase */
+  switch (AX25_SINTABLE_PART(ax25_phase)) {
+  case 0: 				/* 0° - 90° */
+    deviation = ax25_sintable[sintable_phase];
+    break;
+  case 1:				/* 90° - 180° */
+    deviation = ax25_sintable[AX25_SINTABLE_LUMASK - sintable_phase];
+    break;
+  case 2:				/* 180° - 270° */
+    deviation = -ax25_sintable[sintable_phase];
+    break;
+  case 3:				/* 270° - 360° */
+    deviation = -ax25_sintable[AX25_SINTABLE_LUMASK - sintable_phase];
+    break;
+  default:
+    deviation = 0;
+  }
+
+//  si_trx_switch_channel(deviation);
+
+//  float duty = 0.5 + ((float)deviation/192.0) / 2.0;
+  float duty = (ax25_get_next_symbol() == AX25_SPACE) ? 1.0 : 0.0;
+
+  telemetry_gpio1_pwm_duty(duty);
+
+  /* Update with next bit */
+  if (ax25_oversampling_count++ >= AX25_OVERSAMPLING) {
+    ax25_oversampling_count = 0;
+
+    /* Set phase velocity for next symbol */
+    if (0) {//ax25_get_next_symbol() == AX25_SPACE) {
+      ax25_phasevelocity = AX25_SPACEPHASEVELOCITY;
+      ax25_sintable = AX25_SPACESINTABLE;
+    } else {
+      ax25_phasevelocity = AX25_MARKPHASEVELOCITY;
+      ax25_sintable = AX25_MARKSINTABLE;
+    }
+  }
+  /* Update phase */
+  ax25_phase += ax25_phasevelocity;
+
+
+  return 1;
+}
diff --git a/firmware/src/contestia.c b/firmware/src/contestia.c
index 83dada8..388749f 100644
--- a/firmware/src/contestia.c
+++ b/firmware/src/contestia.c
@@ -63,6 +63,8 @@ void contestia_set_tone(uint8_t tone) {
 
 /**
  * Called at the baud rate, outputs tones
+ *
+ * Returns 1 when more work todo, 0 when finished
  */
 uint8_t contestia_tick(void) {
 
diff --git a/firmware/src/main.c b/firmware/src/main.c
index 9daf8a0..e6d647b 100644
--- a/firmware/src/main.c
+++ b/firmware/src/main.c
@@ -285,7 +285,9 @@ void init(void)
   //wdt_reset_count();
 
   /* Enables the xosc on gclk1 */
+#ifdef USE_XOSC
   xosc_init();
+#endif
 
   led_init();
   gps_init();
@@ -322,6 +324,17 @@ int main(void)
 
   led_on();
 
+
+  telemetry_start(TELEMETRY_APRS, 0xFFFF);
+
+  while (1) {
+    system_sleep();
+  }
+
+
+
+
+
   while (1) {
     /* Sleep wait for next telemetry */
     while (telemetry_trigger_flag == 0) {
diff --git a/firmware/src/si_trx.c b/firmware/src/si_trx.c
index 6dc7f1e..6da520d 100644
--- a/firmware/src/si_trx.c
+++ b/firmware/src/si_trx.c
@@ -37,6 +37,18 @@
 
 
 
+void _si_trx_transfer_nocts(int tx_count, int rx_count, uint8_t *data)
+{
+  /* Send command */
+  _si_trx_cs_enable();
+
+  for (int i = 0; i < tx_count; i++) {
+    spi_bitbang_transfer(data[i]);
+  }
+
+  _si_trx_cs_disable();
+}
+
 /**
  * Generic SPI Send / Receive
  */
@@ -247,6 +259,25 @@ static void si_trx_modem_set_deviation(uint32_t deviation)
 			  SI_MODEM_FREQ_DEV,
 			  deviation);
 }
+
+
+static void si_trx_modem_set_tx_datarate(uint32_t rate)
+{
+  uint32_t nco_max_count = ((float)SI406X_TCXO_FREQUENCY / 10);
+
+  /* Set TX_NCO_MODE */
+  _si_trx_set_property_32(SI_PROPERTY_GROUP_MODEM,
+                          SI_MODEM_TX_NCO_MODE,
+                          (SI_MODEM_TX_NCO_TXOSR_10X |
+                           (nco_max_count & 0x03FFFFFF)));
+
+  /* Set DATA_RATE */
+  _si_trx_set_property_24(SI_PROPERTY_GROUP_MODEM,
+                          SI_MODEM_DATA_RATE,
+                          rate & 0xFFFFFF);
+}
+
+
 /**
  * Sets the modem frequency offset manually. In units of the
  * resolution of the frac-n pll synthsiser.
@@ -255,9 +286,20 @@ static void si_trx_modem_set_deviation(uint32_t deviation)
  */
 static void si_trx_modem_set_offset(int16_t offset)
 {
-  _si_trx_set_property_16(SI_PROPERTY_GROUP_MODEM,
-                          SI_MODEM_FREQ_OFFSET,
-                          offset);
+  /* _si_trx_set_property_16(SI_PROPERTY_GROUP_MODEM, */
+  /*                         SI_MODEM_FREQ_OFFSET, */
+  /*                         offset); */
+
+  uint8_t buffer[6];
+
+  buffer[0] = SI_CMD_SET_PROPERTY;
+  buffer[1] = SI_PROPERTY_GROUP_MODEM; // group
+  buffer[2] = 2;
+  buffer[3] = SI_MODEM_FREQ_OFFSET; // prop
+  buffer[4] = (offset >> 8);
+  buffer[5] = (offset);
+
+  _si_trx_transfer_nocts(6, 0, buffer);
 }
 
 /**
@@ -388,8 +430,10 @@ void si_trx_reset(uint8_t modulation_type, uint16_t deviation)
   si_trx_set_frequency(RADIO_FREQUENCY, deviation);
   si_trx_set_tx_power(RADIO_POWER);
 
+  si_trx_modem_set_tx_datarate(2200);
+
   /* RTTY from GPIO1 */
-  si_trx_modem_set_modulation(SI_MODEM_MOD_DIRECT_MODE_ASYNC,
+  si_trx_modem_set_modulation(SI_MODEM_MOD_DIRECT_MODE_SYNC, // ASYNC
 			      SI_MODEM_MOD_GPIO_1,
 			      SI_MODEM_MOD_SOURCE_DIRECT,
 			      modulation_type);
diff --git a/firmware/src/telemetry.c b/firmware/src/telemetry.c
index 6f358fd..1bf2760 100644
--- a/firmware/src/telemetry.c
+++ b/firmware/src/telemetry.c
@@ -30,6 +30,7 @@
 #include "rtty.h"
 #include "contestia.h"
 #include "rsid.h"
+#include "ax25.h"
 #include "pips.h"
 #include "si_trx.h"
 #include "si_trx_defs.h"
@@ -150,6 +151,8 @@ int telemetry_start(enum telemetry_t type, int32_t length) {
       timer0_tick_init(PIPS_OFF_FREQUENCY);
       break;
     case TELEMETRY_APRS:
+      timer0_tick_init(AX25_TICK_RATE);
+      break;
     case TELEMETRY_RSID: /* Not used - see function below */
       break;
     }
@@ -283,7 +286,7 @@ void telemetry_tick(void) {
         /* RSID: We PWM frequencies with the external pin */
         telemetry_gpio1_pwm_init();
 
-        si_trx_on(SI_MODEM_MOD_TYPE_2FSK, 1);
+        si_trx_on(SI_MODEM_MOD_TYPE_2GFSK, 1);
         radio_on = 1;
 
         return;
@@ -299,6 +302,23 @@ void telemetry_tick(void) {
       }
       break;
 
+    case TELEMETRY_APRS: /* ---- ---- APRS */
+
+      if (!radio_on) {
+        /* ARPS: We use the modem offset to modulate */
+
+        telemetry_gpio1_pwm_init();
+        //telemetry_gpio1_pwm_duty(0.5);
+
+        si_trx_on(SI_MODEM_MOD_TYPE_2GFSK, 400);
+
+        radio_on = 1;
+        ax25_start();
+      }
+      ax25_tick();
+
+      break;
+
     case TELEMETRY_PIPS: /* ---- ---- A pips mode! */
 
       if (!radio_on) { /* Turn on */
@@ -313,6 +333,7 @@ void telemetry_tick(void) {
         telemetry_index++;
         if (is_telemetry_finished()) return;
       }
+      break;
     }
   }
 }
@@ -330,15 +351,23 @@ void telemetry_tick(void) {
  */
 float timer0_tick_init(float frequency)
 {
+#ifdef USE_XOSC
+  const enum gclk_generator tick_gclk_gen = GCLK_GENERATOR_1;
+  const uint8_t tick_gclk_gen_num = 1;
+#else
+  const enum gclk_generator tick_gclk_gen = GCLK_GENERATOR_0;
+  const uint8_t tick_gclk_gen_num = 0;
+#endif
+
   /* Calculate the wrap value for the given frequency */
-  float gclk_frequency = (float)system_gclk_gen_get_hz(1);
+  float gclk_frequency = (float)system_gclk_gen_get_hz(tick_gclk_gen_num);
   uint32_t count = (uint32_t)(gclk_frequency / frequency);
 
   /* Configure Timer 0 */
   bool t0_capture_channel_enables[]    = {false, false};
   uint32_t t0_compare_channel_values[] = {count, 0x0000};
   tc_init(TC0,
-          GCLK_GENERATOR_1,
+          tick_gclk_gen,
 	  TC_COUNTER_SIZE_32BIT,
 	  TC_CLOCK_PRESCALER_DIV1,
 	  TC_WAVE_GENERATION_MATCH_FREQ,
@@ -371,9 +400,11 @@ float timer0_tick_init(float frequency)
   return gclk_frequency / (float)count;
 }
 /**
- * Changes the timer0 frequency
+ * Changes the timer0 frequency.
+ *
+ * Returns the timer count that this corresponds to.
  */
-void timer0_tick_frequency(float frequency)
+uint32_t timer0_tick_frequency(float frequency)
 {
   float gclk_frequency = (float)system_gclk_chan_get_hz(0);
   uint32_t count = (uint32_t)(gclk_frequency / frequency);
@@ -381,7 +412,10 @@ void timer0_tick_frequency(float frequency)
   tc_set_compare_value(TC0,
                        TC_COMPARE_CAPTURE_CHANNEL_0,
                        count);
+  /* We need to reset the count here so it's not beyond the capture limit */
   tc_set_count_value(TC0, 0);
+
+  return count;
 }
 /**
  * Disables the timer
@@ -405,7 +439,7 @@ void TC0_Handler(void)
 
 
 
-#define GPIO1_PWM_STEPS 200 // ~ 20kHz on a 4 MHz clock
+#define GPIO1_PWM_STEPS 208 // // ~ 2kHz on a 4 MHz clock
 
 /**
  * Initialised PWM at the given duty cycle on the GPIO1 pin of the radio
@@ -415,12 +449,12 @@ void telemetry_gpio1_pwm_init(void)
   bool capture_channel_enables[]    = {false, true};
   uint32_t compare_channel_values[] = {0x0000, 0x0000}; // Set duty cycle at 0% by default
 
-  //float gclk_frequency = (float)system_gclk_chan_get_hz(0);
+  //float gclk_frequency = (float)system_gclk_gen_get_hz(0);
 
   tc_init(TC5,
 	  GCLK_GENERATOR_0,
 	  TC_COUNTER_SIZE_8BIT,
-	  TC_CLOCK_PRESCALER_DIV1,
+	  TC_CLOCK_PRESCALER_DIV16,
 	  TC_WAVE_GENERATION_NORMAL_PWM,
 	  TC_RELOAD_ACTION_GCLK,
 	  TC_COUNT_DIRECTION_UP,