diff --git a/legacy-code/locator.c b/legacy-code/locator.c
deleted file mode 100644
index 24d537a..0000000
--- a/legacy-code/locator.c
+++ /dev/null
@@ -1,32 +0,0 @@
-// Based on HamLib's locator routines
-// OK1TE 2018-10
-
-#include "locator.h"
-#include "config.h"
-
-const static uint8_t loc_char_range[] = { 18, 10, 24, 10, 24, 10 };
-const float precision = 1E+7;
-
-uint8_t longlat2locator(int32_t longitude, int32_t latitude, char locator[]) {
-	if (!locator)
-		return 0;
-
-	for (uint8_t x_or_y = 0; x_or_y < 2; ++x_or_y) {
-		float ordinate = ((x_or_y == 0) ? (longitude / 2) / precision : latitude / precision) + 90;
-		uint32_t divisions = 1;
-
-		for (uint8_t pair = 0; pair < PAIR_COUNT; ++pair) {
-			divisions *= loc_char_range[pair];
-			const float square_size = 180.0 / divisions;
-
-			uint8_t locvalue = (uint8_t) (ordinate / square_size);
-			ordinate -= square_size * locvalue;
-			locvalue += (loc_char_range[pair] == 10) ? '0':'A';
-			locator[pair * 2 + x_or_y] = locvalue;
-		}
-	}
-
-	locator[PAIR_COUNT * 2] = 0;
-
-	return 1;
-}
diff --git a/legacy-code/locator.h b/legacy-code/locator.h
deleted file mode 100644
index e0376df..0000000
--- a/legacy-code/locator.h
+++ /dev/null
@@ -1,17 +0,0 @@
-// Based on HamLib's locator routines
-// OK1TE 2018-10
-#ifndef __LOCATOR_H_
-#define __LOCATOR_H_
-
-#include <stdint.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-uint8_t longlat2locator(int32_t longitude, int32_t latitude, char locator[]);
-
-#ifdef __cplusplus
-}
-#endif
-#endif
diff --git a/src/codecs/aprs/aprs.c b/src/codecs/aprs/aprs.c
index 09e093b..1a4eebe 100644
--- a/src/codecs/aprs/aprs.c
+++ b/src/codecs/aprs/aprs.c
@@ -2,18 +2,14 @@
 #include <stdlib.h>
 #include "aprs.h"
 
-/*
+/**
+ * TODO: add function to generate weather report
+ *
  * Examples:
-2019-11-27 04:37:21 EET: OH2NJR>APRS,WIDE1-1,qAR,OH2HCY-2:;434.700-C*111111z6030.65N/02444.84Er434.700MHz T118 -200 R50k OH2RUC Nurmijarvi
-2019-11-27 04:52:08 EET: OH2NJR>APX210,TCPIP*,qAC,APRSFI-I3:=6030.35N/02443.91E_154/001g002t034r000P000p000b10029
-
-  TODO: speed and bearing before altitude!
-2019-11-27 13:39:45 EET: OH3EUJ>APRARX,SONDEGATE,TCPIP,qAR,OH3EUJ:;R1920638 *113959h6051.11N/02331.80EO063/021/A=011240 Clb=6.6m/s t=-13.2C 402.970 MHz Type=RS41-SG Radiosonde http://bit.ly/2Bj4Sfk !woM!
-2019-11-27 13:40:00 EET: OH3EUJ>APRARX,SONDEGATE,TCPIP,qAR,OH3EUJ:;R1920638 *114015h6051.17N/02331.98EO056/026/A=011536 Clb=5.0m/s t=-13.8C 402.970 MHz Type=RS41-SG Radiosonde http://bit.ly/2Bj4Sfk !wja!
+ * 2019-11-27 04:37:21 EET: OH2NJR>APRS,WIDE1-1,qAR,OH2HCY-2:;434.700-C*111111z6030.65N/02444.84Er434.700MHz T118 -200 R50k OH2RUC Nurmijarvi
+ * 2019-11-27 04:52:08 EET: OH2NJR>APX210,TCPIP*,qAC,APRSFI-I3:=6030.35N/02443.91E_154/001g002t034r000P000p000b10029
  */
 
-// TODO: add function to generate weather report
-
 volatile uint16_t aprs_packet_counter = 0;
 
 static void convert_degrees_to_dmh(long x, int8_t *degrees, uint8_t *minutes, uint8_t *h_minutes)
@@ -43,22 +39,56 @@ size_t aprs_generate_position_without_timestamp(uint8_t *payload, size_t length,
 
     aprs_packet_counter++;
 
-    // TODO: speed + bearing
-
     return snprintf((char *) payload,
             length,
-            ("!%02d%02d.%02u%c%c%03d%02u.%02u%c%c/A=%06ld/P%dS%dT%dV%d%s"),
+            ("!%02d%02d.%02u%c%c%03d%02u.%02u%c%c%03d/%03d/A=%06ld/P%dS%dT%ldV%dC%d%s"),
             abs(la_degrees), la_minutes, la_h_minutes,
             la_degrees > 0 ? 'N' : 'S',
             symbol_table,
             abs(lo_degrees), lo_minutes, lo_h_minutes,
             lo_degrees > 0 ? 'E' : 'W',
             symbol,
+            (int16_t) ((float) data->gps.heading_raw / 100000.0f),
+            (int16_t) (((float) data->gps.speed_raw / 100.0f) * 3.6f / 1.852f),
             (data->gps.alt_raw / 1000) * 3280 / 1000,
             aprs_packet_counter,
             data->gps.sats_raw,
-            (int16_t) data->temperature_celsius_100,
-            data->battery_voltage_millivolts, // TODO: unit?
+            data->internal_temperature_celsius_100 / 10,
+            data->battery_voltage_millivolts,
+            (int16_t) ((float) data->gps.climb_raw / 100.0f),
+            comment
+    );
+}
+
+size_t aprs_generate_position_with_timestamp(uint8_t *payload, size_t length, telemetry_data *data,
+        char symbol_table, char symbol, char *comment)
+{
+    int8_t la_degrees, lo_degrees;
+    uint8_t la_minutes, la_h_minutes, lo_minutes, lo_h_minutes;
+
+    convert_degrees_to_dmh(data->gps.lat_raw / 10, &la_degrees, &la_minutes, &la_h_minutes);
+    convert_degrees_to_dmh(data->gps.lon_raw / 10, &lo_degrees, &lo_minutes, &lo_h_minutes);
+
+    aprs_packet_counter++;
+
+    return snprintf((char *) payload,
+            length,
+            ("/%02d%02d%02dz%02d%02d.%02u%c%c%03d%02u.%02u%c%c%03d/%03d/A=%06ld/P%dS%dT%ldV%dC%d%s"),
+            data->gps.hours, data->gps.minutes, data->gps.seconds,
+            abs(la_degrees), la_minutes, la_h_minutes,
+            la_degrees > 0 ? 'N' : 'S',
+            symbol_table,
+            abs(lo_degrees), lo_minutes, lo_h_minutes,
+            lo_degrees > 0 ? 'E' : 'W',
+            symbol,
+            (int16_t) ((float) data->gps.heading_raw / 100000.0f),
+            (int16_t) (((float) data->gps.speed_raw / 100.0f) * 3.6f / 1.852f),
+            (data->gps.alt_raw / 1000) * 3280 / 1000,
+            aprs_packet_counter,
+            data->gps.sats_raw,
+            data->internal_temperature_celsius_100 / 10,
+            data->battery_voltage_millivolts,
+            (int16_t) ((float) data->gps.climb_raw / 100.0f),
             comment
     );
 }
diff --git a/src/codecs/ax25/ax25.c b/src/codecs/ax25/ax25.c
index bc6f3de..3c1dd45 100644
--- a/src/codecs/ax25/ax25.c
+++ b/src/codecs/ax25/ax25.c
@@ -23,7 +23,7 @@ static inline uint16_t ax25_calculate_crc_for_bit(uint16_t crc, bool bit)
 static inline uint16_t ax25_calculate_crc_for_byte(uint16_t crc, uint8_t byte)
 {
     uint8_t temp = byte;
-    for (int i = 0; i < 8; i++, temp >>= 1U) {
+    for (uint16_t i = 0; i < 8; i++, temp >>= 1U) {
         bool bit = ((temp & 1U) == 1U) ? true : false;
         crc = ax25_calculate_crc_for_bit(crc, bit);
     }
@@ -31,11 +31,11 @@ static inline uint16_t ax25_calculate_crc_for_byte(uint16_t crc, uint8_t byte)
     return crc;
 }
 
-static uint16_t ax25_calculate_crc(size_t length, uint8_t *data)
+static uint16_t ax25_calculate_crc(uint16_t length, uint8_t *data)
 {
     uint16_t crc = 0xFFFF;
 
-    for (size_t i = 0; i < length; i++) {
+    for (uint16_t i = 0; i < length; i++) {
         uint8_t byte = data[i];
         crc = ax25_calculate_crc_for_byte(crc, byte);
     }
@@ -43,19 +43,19 @@ static uint16_t ax25_calculate_crc(size_t length, uint8_t *data)
     return crc;
 }
 
-static size_t ax25_encode_digipeater_path(char *input, char *packet_data)
+static uint16_t ax25_encode_digipeater_path(char *input, char *packet_data)
 {
-    size_t digipeaters_length = (size_t) strlen(input);
-    size_t packet_data_index = 0;
+    uint16_t digipeaters_length = strlen(input);
+    uint16_t packet_data_index = 0;
 
-    for (size_t index = 0; index < digipeaters_length; index++) {
+    for (uint16_t index = 0; index < digipeaters_length; index++) {
         if (input[index] == ',' || index == digipeaters_length - 1) {
             if (input[index] != ',') {
                 packet_data[packet_data_index] = input[index] == '-' ? ' ' : input[index];
                 packet_data_index++;
             }
 
-            size_t fill_count = 7 - (packet_data_index % 7);
+            uint16_t fill_count = 7 - (packet_data_index % 7);
             while (fill_count > 0 && fill_count < 7) {
                 packet_data[packet_data_index] = ' ';
                 fill_count--;
@@ -72,8 +72,8 @@ static size_t ax25_encode_digipeater_path(char *input, char *packet_data)
     return packet_data_index;
 }
 
-size_t ax25_encode_packet_aprs(char *source, uint8_t source_ssid, char *destination, uint8_t destination_ssid,
-        char *digipeater_addresses, char *information_field, size_t length, uint8_t *packet_data)
+uint16_t ax25_encode_packet_aprs(char *source, uint8_t source_ssid, char *destination, uint8_t destination_ssid,
+        char *digipeater_addresses, char *information_field, uint16_t length, uint8_t *packet_data)
 {
     // TODO: use length to limit packet size
 
@@ -90,11 +90,11 @@ size_t ax25_encode_packet_aprs(char *source, uint8_t source_ssid, char *destinat
     header->destination_ssid = destination_ssid;
 
     char *digipeater_addresses_start = ((char *) header) + 1 + 14;
-    size_t digipeater_addresses_length = ax25_encode_digipeater_path(digipeater_addresses, digipeater_addresses_start);
+    uint16_t digipeater_addresses_length = ax25_encode_digipeater_path(digipeater_addresses, digipeater_addresses_start);
 
     // Perform bit-shifting for all addresses
     uint8_t *actual_data_start = ((uint8_t *) header) + 1;
-    for (int i = 0; i < 14 + digipeater_addresses_length; i++) {
+    for (uint16_t i = 0; i < 14 + digipeater_addresses_length; i++) {
         actual_data_start[i] = actual_data_start[i] << 1U;
     }
     actual_data_start[13 + digipeater_addresses_length] |= 1U;
@@ -105,10 +105,10 @@ size_t ax25_encode_packet_aprs(char *source, uint8_t source_ssid, char *destinat
     header_end->control_field = AX25_CONTROL_FIELD_UI_FRAME;
     header_end->protocol_id = AX25_PROTOCOL_ID_NO_LAYER_3;
 
-    size_t info_length = strlen(information_field);
+    uint16_t info_length = strlen(information_field);
     strcpy(header_end->information_field, information_field);
 
-    size_t crc_length = 14 + digipeater_addresses_length + 2 + info_length;
+    uint16_t crc_length = 14 + digipeater_addresses_length + 2 + info_length;
     uint16_t crc = ax25_calculate_crc(crc_length, actual_data_start);
 
     ax25_packet_footer *footer = (ax25_packet_footer *) (((uint8_t *) header_end->information_field) + info_length);
diff --git a/src/codecs/ax25/ax25.h b/src/codecs/ax25/ax25.h
index 171a4d5..a9999f6 100644
--- a/src/codecs/ax25/ax25.h
+++ b/src/codecs/ax25/ax25.h
@@ -27,7 +27,7 @@ typedef struct _ax25_packet_footer {
     uint8_t flag;
 } ax25_packet_footer;
 
-size_t ax25_encode_packet_aprs(char *source, uint8_t source_ssid, char *destination, uint8_t destination_ssid,
-        char *digipeater_addresses, char *information_field, size_t length, uint8_t *packet_data);
+uint16_t ax25_encode_packet_aprs(char *source, uint8_t source_ssid, char *destination, uint8_t destination_ssid,
+        char *digipeater_addresses, char *information_field, uint16_t length, uint8_t *packet_data);
 
 #endif
diff --git a/src/codecs/bell/bell.c b/src/codecs/bell/bell.c
index 83161ed..2a2209f 100644
--- a/src/codecs/bell/bell.c
+++ b/src/codecs/bell/bell.c
@@ -17,7 +17,7 @@ typedef struct _bell_encoder {
     uint16_t data_length;
     uint8_t *data;
 
-    size_t current_byte_index;
+    uint16_t current_byte_index;
     uint8_t current_bit_index;
     uint8_t current_byte;
     uint8_t bit_stuffing_counter;
diff --git a/src/config.h b/src/config.h
index 3196a25..ff1d862 100644
--- a/src/config.h
+++ b/src/config.h
@@ -1,21 +1,25 @@
 #ifndef __CONFIG_H
 #define __CONFIG_H
 
+// Enable semihosting to receive debug logs during development
+//#define SEMIHOSTING_ENABLE
+
 #include <stdbool.h>
 
-#define RADIO_PAYLOAD_MAX_LENGTH 256
+#define RADIO_PAYLOAD_MAX_LENGTH 384
+#define APRS_COMMENT_MAX_LENGTH 128
 
 #define SENSOR_BMP280_ENABLE false
 
 #define RADIO_SI5351_ENABLE true
 
-#define RADIO_POST_TRANSMIT_DELAY 2000
+#define RADIO_POST_TRANSMIT_DELAY 5000
 
 // Si4032 transmit power: 0..100%
 #define RADIO_SI4032_TX_POWER 100
-#define RADIO_SI4032_TX_FREQUENCY_CW   432500000
-#define RADIO_SI4032_TX_FREQUENCY_RTTY 434250000
-#define RADIO_SI4032_TX_FREQUENCY_APRS 434250000
+#define RADIO_SI4032_TX_FREQUENCY_CW   432060000
+#define RADIO_SI4032_TX_FREQUENCY_RTTY 432060000
+#define RADIO_SI4032_TX_FREQUENCY_APRS 432500000
 
 #define RADIO_SI5351_TX_POWER 100
 #define RADIO_SI5351_TX_FREQUENCY_JT9        14078700UL
@@ -25,12 +29,14 @@
 #define RADIO_SI5351_TX_FREQUENCY_FSQ        7105350UL     // Base freq is 1350 Hz higher than dial freq in USB
 #define RADIO_SI5351_TX_FREQUENCY_FT8        14085000UL    // Was: 14075000UL
 
+#define LOCATOR_PAIR_COUNT_FULL 6 // max. 6 (12 characters WWL)
+
 #define WSPR_CALLSIGN "OH3BHX"
-#define WSPR_LOCATOR "AA00"
+//#define WSPR_LOCATOR_FIXED "AA00"
 #define WSPR_DBM 10
 
 #define FT8_CALLSIGN "OH3BHX"
-#define FT8_LOCATOR "AA00"
+//#define FT8_LOCATOR_FIXED "AA00"
 
 #define FSQ_CALLSIGN_FROM "OH3BHX"
 #define FSQ_CALLSIGN_TO "N0CALL"
@@ -67,9 +73,11 @@
 #define APRS_DESTINATION "APZ41N"
 #define APRS_DESTINATION_SSID '0'
 
-#define PAIR_COUNT 4 // max. 6 (12 characters WWL)
+#define RTTY_LOCATOR_PAIR_COUNT 4 // max. 6 (12 characters WWL)
 #define RTTY_7BIT   1 // if 0 --> 5 bits
 
+#define CW_LOCATOR_PAIR_COUNT 4 // max. 6 (12 characters WWL)
+
 extern bool bmp280_enabled;
 extern bool si5351_enabled;
 
diff --git a/src/drivers/si4032/si4032.c b/src/drivers/si4032/si4032.c
index 636e029..ecfe275 100644
--- a/src/drivers/si4032/si4032.c
+++ b/src/drivers/si4032/si4032.c
@@ -111,10 +111,20 @@ void si4032_set_modulation_type(si4032_modulation_type type)
 
 int32_t si4032_read_temperature_celsius_100()
 {
+    // Set the input for ADC to the temperature sensor, "Register 0Fh. ADC Configuration"—adcsel[2:0] = 000
+    // Set the reference for ADC, "Register 0Fh. ADC Configuration"—adcref[1:0] = 00
+    si4032_write(0x0f, 0b00000000);
+    // Set the temperature range for ADC, "Register 12h. Temperature Sensor Calibration"—tsrange[1:0]
+    // Range: –64 ... 64 °C, Slope 8 mV/°C, ADC8 LSB 0.5 °C
+    si4032_write(0x12, 0b00100000);
+    // Set entsoffs = 1, "Register 12h. Temperature Sensor Calibration"
+    // Trigger ADC reading, "Register 0Fh. ADC Configuration"—adcstart = 1
+    si4032_write(0x0f, 0b10000000);
+    // Read temperature value—Read contents of "Register 11h. ADC Value"
     int32_t raw_value = (int32_t) si4032_read(0x11);
-    int32_t temperature = (int32_t) (-64 + (raw_value * 5 / 10) - 16);
-    // TODO: correct unit/scale
-    si4032_write(0x0f, 0x80);
+
+    int32_t temperature = (int32_t) (-6400 + (raw_value * 100 * 500 / 1000));
+
     return temperature;
 }
 
diff --git a/src/drivers/ubxg6010/ubxg6010.c b/src/drivers/ubxg6010/ubxg6010.c
index 735f9a1..08d728b 100644
--- a/src/drivers/ubxg6010/ubxg6010.c
+++ b/src/drivers/ubxg6010/ubxg6010.c
@@ -5,6 +5,7 @@
 #include "hal/delay.h"
 
 #include "ubxg6010.h"
+#include "log.h"
 
 gps_data current_gps_data;
 
@@ -15,22 +16,22 @@ static uBloxChecksum
 ubxg6010_calculate_checksum(const uint8_t msgClass, const uint8_t msgId, const uint8_t *message, uint16_t size)
 {
     uBloxChecksum ck = {0, 0};
-    uint8_t i;
+
     ck.ck_a += msgClass;
     ck.ck_b += ck.ck_a;
     ck.ck_a += msgId;
     ck.ck_b += ck.ck_a;
 
-    ck.ck_a += size & 0xff;
+    ck.ck_a += size & 0xffU;
     ck.ck_b += ck.ck_a;
-    ck.ck_a += size >> 8;
+    ck.ck_a += size >> 8U;
     ck.ck_b += ck.ck_a;
 
-
-    for (i = 0; i < size; i++) {
+    for (uint16_t i = 0; i < size; i++) {
         ck.ck_a += message[i];
         ck.ck_b += ck.ck_a;
     }
+
     return ck;
 }
 
@@ -75,6 +76,19 @@ void ubxg6010_send_packet(uBloxPacket *packet)
             packet->header.payloadSize);
 }
 
+bool ubxg6010_send_packet_and_wait_for_ack(uBloxPacket *packet)
+{
+    int retries = 3;
+    bool success;
+
+    do {
+        ubxg6010_send_packet(packet);
+        success = ubxg6010_wait_for_ack();
+    } while (!success && retries-- > 0);
+
+    return success;
+}
+
 void ubxg6010_get_current_gps_data(gps_data *data)
 {
     system_disable_irq();
@@ -82,8 +96,10 @@ void ubxg6010_get_current_gps_data(gps_data *data)
     system_enable_irq();
 }
 
-void ubxg6010_init()
+bool ubxg6010_init()
 {
+    bool success;
+
     usart_gps_init(38400, true);
     delay_ms(10);
 
@@ -96,8 +112,8 @@ void ubxg6010_init()
                     .payloadSize=sizeof(uBloxCFGRSTPayload)
             },
             .data.cfgrst = {
-                    .navBbrMask=0xffff,
-                    .resetMode=1,
+                    .navBbrMask=0xffff, // Coldstart
+                    .resetMode=1, // Controlled Software reset
                     .reserved1=0
             },
     };
@@ -111,7 +127,7 @@ void ubxg6010_init()
     ubxg6010_send_packet(&msgcfgrst);
     delay_ms(800);
 
-    uBloxPacket msgcgprt = {
+    uBloxPacket msgcfgprt = {
             .header = {
                     0xb5,
                     0x62,
@@ -123,19 +139,27 @@ void ubxg6010_init()
                     .portID=1,
                     .reserved1=0,
                     .txReady=0,
-                    .mode=0b00100011000000,
+                    .mode=0b0000100011000000, // 8 bits, no parity, 1 stop bit
                     .baudRate=38400,
-                    .inProtoMask=1,
-                    .outProtoMask=1,
+                    .inProtoMask=1, // UBX protocol for input
+                    .outProtoMask=1, // UBX protocol for output
                     .flags=0,
                     .reserved2={0, 0}
             },
     };
-    ubxg6010_send_packet(&msgcgprt);
+    ubxg6010_send_packet(&msgcfgprt);
 
     usart_gps_init(38400, true);
     delay_ms(10);
 
+    /**
+     * Low Power Mode
+     * 0: Max. performance mode
+     * 1: Power Save Mode (>= FW 6.00 only)
+     * 2-3: reserved
+     * 4: Eco mode
+     * 5-255: reserved
+     */
     uBloxPacket msgcfgrxm = {
             .header = {
                     0xb5,
@@ -145,15 +169,17 @@ void ubxg6010_init()
                     .payloadSize=sizeof(uBloxCFGRXMPayload)
             },
             .data.cfgrxm = {
-                    .reserved1=8,
-                    .lpMode=4
+                    .reserved1=8, // Always set to 8
+                    .lpMode=0 // Low power mode: Eco mode -- TODO: set back to Eco mode
             }
     };
 
-    do {
-        ubxg6010_send_packet(&msgcfgrxm);
-    } while (!ubxg6010_wait_for_ack());
+    success = ubxg6010_send_packet_and_wait_for_ack(&msgcfgrxm);
+    if (!success) {
+        return false;
+    }
 
+    // Configure rate of 1 for message: 0x01 0x02 Geodetic Position Solution
     uBloxPacket msgcfgmsg = {
             .header = {
                     0xb5,
@@ -169,20 +195,58 @@ void ubxg6010_init()
             }
     };
 
-    do {
-        ubxg6010_send_packet(&msgcfgmsg);
-    } while (!ubxg6010_wait_for_ack());
+    success = ubxg6010_send_packet_and_wait_for_ack(&msgcfgmsg);
+    if (!success) {
+        return false;
+    }
 
+    // Rate of 1 for message: 0x01 0x06 Navigation Solution Information
     msgcfgmsg.data.cfgmsg.msgID = 0x6;
-    do {
-        ubxg6010_send_packet(&msgcfgmsg);
-    } while (!ubxg6010_wait_for_ack());
+    msgcfgmsg.data.cfgmsg.rate = 1;
+    success = ubxg6010_send_packet_and_wait_for_ack(&msgcfgmsg);
+    if (!success) {
+        return false;
+    }
 
+    // Configure rate of 1 for message: 0x01 0x21 UTC Time Solution
     msgcfgmsg.data.cfgmsg.msgID = 0x21;
-    do {
-        ubxg6010_send_packet(&msgcfgmsg);
-    } while (!ubxg6010_wait_for_ack());
+    msgcfgmsg.data.cfgmsg.rate = 1;
+    success = ubxg6010_send_packet_and_wait_for_ack(&msgcfgmsg);
+    if (!success) {
+        return false;
+    }
 
+    // Configure rate of 2 for message: 0x01 0x12 Velocity Solution in NED
+    msgcfgmsg.data.cfgmsg.msgID = 0x12;
+    msgcfgmsg.data.cfgmsg.rate = 2;
+    success = ubxg6010_send_packet_and_wait_for_ack(&msgcfgmsg);
+    if (!success) {
+        return false;
+    }
+
+    // TODO: Is this message supported: Configure rate of 1 for message: 0x01 0x07 Position Velocity Time Solution
+    /*msgcfgmsg.data.cfgmsg.msgID = 0x07;
+    success = ubxg6010_send_packet_and_wait_for_ack(&msgcfgmsg);
+    if (!success) {
+        return false;
+    }*/
+
+    /**
+     * Dynamic Platform model:
+     * - 0 Portable
+     * - 2 Stationary
+     * - 3 Pedestrian
+     * - 4 Automotive
+     * - 5 Sea
+     * - 6 Airborne with <1g Acceleration
+     * - 7 Airborne with <2g Acceleration
+     * - 8 Airborne with <4g Acceleration
+     *
+     * Position Fixing Mode.
+     * - 1: 2D only
+     * - 2: 3D only
+     * - 3: Auto 2D/3D
+     */
     uBloxPacket msgcfgnav5 = {
             .header = {
                     0xb5,
@@ -192,31 +256,32 @@ void ubxg6010_init()
                     .payloadSize=sizeof(uBloxCFGNAV5Payload)
             },
             .data.cfgnav5={
-                    .mask=0b00000001111111111,
-                    .dynModel=7,
-                    .fixMode=2,
-                    .fixedAlt=0,
-                    .fixedAltVar=10000,
-                    .minElev=5,
-                    .drLimit=0,
-                    .pDop=25,
-                    .tDop=25,
-                    .pAcc=100,
-                    .tAcc=300,
-                    .staticHoldThresh=0,
-                    .dgpsTimeOut=2,
+                    .mask=0b0000001111111111, // Configure all settings
+                    .dynModel=7, // Dynamic model: Airborne with <2g Acceleration
+                    .fixMode=2, // Fix mode: 3D only
+                    .fixedAlt=0, // Fixed altitude (mean sea level) for 2D fix mode.
+                    .fixedAltVar=10000, // Fixed altitude variance for 2D mode.
+                    .minElev=5, // Minimum Elevation for a GNSS satellite to be used in NAV (degrees)
+                    .drLimit=0, // Maximum time to perform dead reckoning (linear extrapolation) in case of GPS signal loss (seconds)
+                    .pDop=25, // Position DOP Mask to use
+                    .tDop=25, // Time DOP Mask to use
+                    .pAcc=100, // Position Accuracy Mask (m)
+                    .tAcc=300, // Time Accuracy Mask (m)
+                    .staticHoldThresh=0, // Static hold threshold (cm/s)
+                    .dgpsTimeOut=2, // DGPS timeout, firmware 7 and newer only
                     .reserved2=0,
                     .reserved3=0,
                     .reserved4=0
             },
     };
 
-    do {
-        ubxg6010_send_packet(&msgcfgnav5);
-    } while (!ubxg6010_wait_for_ack());
-}
+    success = ubxg6010_send_packet_and_wait_for_ack(&msgcfgnav5);
+    if (!success) {
+        return false;
+    }
 
-#include "log.h"
+    return true;
+}
 
 static void ubxg6010_handle_packet(uBloxPacket *pkt)
 {
@@ -229,32 +294,58 @@ static void ubxg6010_handle_packet(uBloxPacket *pkt)
         return;
     }
 
-    if (pkt->header.messageClass == 0x01 && pkt->header.messageId == 0x07) {
+    if (pkt->header.messageClass == 0x01) {
         log_info("class: %02X, id %02X\n", pkt->header.messageClass, pkt->header.messageId);
+    }
+
+    if (pkt->header.messageClass == 0x01 && pkt->header.messageId == 0x07) {
+        // TODO: It seems NAV PVT message is not supported by UBXG6010, confirm this
+        log_info("class: %02X, id %02X, fix: %d\n", pkt->header.messageClass, pkt->header.messageId,
+                pkt->data.navpvt.fixType);
         current_gps_data.ok_packets += 1;
+        current_gps_data.time_of_week_millis = pkt->data.navpvt.iTOW;
+        current_gps_data.year = pkt->data.navpvt.year;
+        current_gps_data.month = pkt->data.navpvt.month;
+        current_gps_data.day = pkt->data.navpvt.day;
+        current_gps_data.hours = pkt->data.navpvt.hour;
+        current_gps_data.minutes = pkt->data.navpvt.min;
+        current_gps_data.seconds = pkt->data.navpvt.sec;
+
         current_gps_data.fix = pkt->data.navpvt.fixType;
         current_gps_data.lat_raw = pkt->data.navpvt.lat;
         current_gps_data.lon_raw = pkt->data.navpvt.lon;
         current_gps_data.alt_raw = pkt->data.navpvt.hMSL;
-        current_gps_data.hours = pkt->data.navpvt.hour;
-        current_gps_data.minutes = pkt->data.navpvt.min;
-        current_gps_data.seconds = pkt->data.navpvt.sec;
         current_gps_data.sats_raw = pkt->data.navpvt.numSV;
         current_gps_data.speed_raw = pkt->data.navpvt.gSpeed;
+        current_gps_data.heading_raw = pkt->data.navpvt.headMot;
+        current_gps_data.climb_raw = -pkt->data.navpvt.velD;
+    } else if (pkt->header.messageClass == 0x01 && pkt->header.messageId == 0x12) {
+        log_info("class: %02X, id %02X\n", pkt->header.messageClass, pkt->header.messageId);
+        current_gps_data.ok_packets += 1;
+        current_gps_data.time_of_week_millis = pkt->data.navvelned.iTOW;
+        current_gps_data.speed_raw = pkt->data.navvelned.gSpeed;
+        current_gps_data.heading_raw = pkt->data.navvelned.headMot;
+        current_gps_data.climb_raw = -pkt->data.navvelned.velD;
     } else if (pkt->header.messageClass == 0x01 && pkt->header.messageId == 0x02) {
         log_info("class: %02X, id %02X\n", pkt->header.messageClass, pkt->header.messageId);
         current_gps_data.ok_packets += 1;
+        current_gps_data.time_of_week_millis = pkt->data.navposllh.iTOW;
         current_gps_data.lat_raw = pkt->data.navposllh.lat;
         current_gps_data.lon_raw = pkt->data.navposllh.lon;
         current_gps_data.alt_raw = pkt->data.navposllh.hMSL;
     } else if (pkt->header.messageClass == 0x01 && pkt->header.messageId == 0x06) {
-        log_info("class: %02X, id %02X\n", pkt->header.messageClass, pkt->header.messageId);
-        log_info("flags: %02X\n", pkt->data.navsol.flags);
-        log_info("SV: %d\n", pkt->data.navsol.numSV);
+        log_info("class: %02X, id %02X, flags: %02X, SV: %d, fix: %d\n", pkt->header.messageClass,
+                pkt->header.messageId,
+                pkt->data.navsol.flags, pkt->data.navsol.numSV, pkt->data.navsol.gpsFix);
+        current_gps_data.time_of_week_millis = pkt->data.navsol.iTOW;
+        current_gps_data.week = pkt->data.navsol.week;
         current_gps_data.fix = pkt->data.navsol.gpsFix;
         current_gps_data.sats_raw = pkt->data.navsol.numSV;
     } else if (pkt->header.messageClass == 0x01 && pkt->header.messageId == 0x21) {
         log_info("class: %02X, id %02X\n", pkt->header.messageClass, pkt->header.messageId);
+        current_gps_data.year = pkt->data.navtimeutc.year;
+        current_gps_data.month = pkt->data.navtimeutc.month;
+        current_gps_data.day = pkt->data.navtimeutc.day;
         current_gps_data.hours = pkt->data.navtimeutc.hour;
         current_gps_data.minutes = pkt->data.navtimeutc.min;
         current_gps_data.seconds = pkt->data.navtimeutc.sec;
diff --git a/src/drivers/ubxg6010/ubxg6010.h b/src/drivers/ubxg6010/ubxg6010.h
index 628c1c7..3c89d77 100644
--- a/src/drivers/ubxg6010/ubxg6010.h
+++ b/src/drivers/ubxg6010/ubxg6010.h
@@ -94,9 +94,20 @@ typedef struct {
     uint8_t min;        //Minute of Hour, range 0..59 (UTC) [- min]
     uint8_t sec;        //Seconds of Minute, range 0..59 (UTC) [- s]
     uint8_t valid;        //Validity Flags (see graphic below) [- -]
-
 } uBloxNAVTIMEUTCPayload;
 
+typedef struct {
+    uint32_t iTOW;        //GPS Millisecond Time of Week [- ms]
+    int32_t velN;          //NED north velocity [- cm/s]
+    int32_t velE;          //NED east velocity [- cm/s]
+    int32_t velD;          //NED down velocity [- cm/s]
+    uint32_t speed;        //Speed (3-D) [- cm/s]
+    uint32_t gSpeed;        //Ground Speed (2-D) [- cm/s]
+    int32_t headMot;    //Heading of motion (2-D) [1e-5 deg]
+    uint32_t sAcc;        //Speed accuracy estimate [- cm/s]
+    uint32_t headAcc;    //Heading accuracy estimate (both motion and vehicle) [1e-5 deg]
+} uBloxNAVVELNEDPayload;
+
 typedef struct {
     uint8_t portID;        //Port Identifier Number (see Serial [- -]
     uint8_t reserved1;        //Reserved [- -]
@@ -107,7 +118,6 @@ typedef struct {
     uint16_t outProtoMask;        //A mask describing which output protocols are active. [- -]
     uint16_t flags;        //Flags bit mask (see graphic below) [- -]
     uint8_t reserved2[2];        //Reserved [- -]
-
 } uBloxCFGPRTPayload;
 
 typedef struct {
@@ -173,6 +183,7 @@ typedef union {
     uBloxNAVPOSLLHPayload navposllh;
     uBloxNAVSOLPayload navsol;
     uBloxNAVTIMEUTCPayload navtimeutc;
+    uBloxNAVVELNEDPayload navvelned;
     uBloxACKACKayload ackack;
     uBloxCFGRSTPayload cfgrst;
     uBloxCFGRXMPayload cfgrxm;
@@ -183,7 +194,7 @@ typedef struct __attribute__((packed)) {
     ubloxPacketData data;
 } uBloxPacket;
 
-void ubxg6010_init();
+bool ubxg6010_init();
 
 void ubxg6010_get_current_gps_data(gps_data *data);
 
diff --git a/src/gps.h b/src/gps.h
index 74a8860..c2d9682 100644
--- a/src/gps.h
+++ b/src/gps.h
@@ -4,14 +4,22 @@
 #include <stdint.h>
 
 typedef struct _gps_data {
+    uint32_t time_of_week_millis;
+    int16_t week;
+    uint16_t year;
+    uint8_t month;
+    uint8_t day;
+    uint8_t seconds;
+    uint8_t minutes;
+    uint8_t hours;
+
     int32_t lat_raw;
     int32_t lon_raw;
     int32_t alt_raw;
     int32_t speed_raw;
+    int32_t heading_raw;
+    int32_t climb_raw;
     uint8_t sats_raw;
-    uint8_t seconds;
-    uint8_t minutes;
-    uint8_t hours;
     uint8_t fix;
     uint16_t ok_packets;
     uint16_t bad_packets;
diff --git a/src/hal/pwm.c b/src/hal/pwm.c
index 49b0678..f628e7a 100644
--- a/src/hal/pwm.c
+++ b/src/hal/pwm.c
@@ -9,8 +9,8 @@
 
 uint16_t pwm_timer_dma_buffer[PWM_TIMER_DMA_BUFFER_SIZE];
 
-size_t (*pwm_handle_dma_transfer_half)(size_t buffer_size, uint16_t *buffer) = NULL;
-size_t (*pwm_handle_dma_transfer_full)(size_t buffer_size, uint16_t *buffer) = NULL;
+uint16_t (*pwm_handle_dma_transfer_half)(uint16_t buffer_size, uint16_t *buffer) = NULL;
+uint16_t (*pwm_handle_dma_transfer_full)(uint16_t buffer_size, uint16_t *buffer) = NULL;
 
 DMA_Channel_TypeDef *pwm_dma_channel = DMA1_Channel2;
 
diff --git a/src/hal/pwm.h b/src/hal/pwm.h
index 2e8609d..d8a7be3 100644
--- a/src/hal/pwm.h
+++ b/src/hal/pwm.h
@@ -21,8 +21,8 @@ void pwm_dma_interrupt_enable(bool enabled);
 void pwm_dma_start();
 void pwm_dma_stop();
 
-extern size_t (*pwm_handle_dma_transfer_half)(size_t buffer_size, uint16_t *buffer);
-extern size_t (*pwm_handle_dma_transfer_full)(size_t buffer_size, uint16_t *buffer);
+extern uint16_t (*pwm_handle_dma_transfer_half)(uint16_t buffer_size, uint16_t *buffer);
+extern uint16_t (*pwm_handle_dma_transfer_full)(uint16_t buffer_size, uint16_t *buffer);
 
 extern uint16_t pwm_timer_dma_buffer[PWM_TIMER_DMA_BUFFER_SIZE];
 
diff --git a/src/locator.c b/src/locator.c
new file mode 100644
index 0000000..bed6ff1
--- /dev/null
+++ b/src/locator.c
@@ -0,0 +1,27 @@
+// Based on HamLib's locator routines
+// OK1TE 2018-10
+
+#include "locator.h"
+
+const static uint8_t loc_char_range[] = {18, 10, 24, 10, 24, 10};
+const float precision = 1E+7f;
+
+void locator_from_lonlat(int32_t longitude, int32_t latitude, uint8_t pair_count, char *locator)
+{
+    for (uint8_t x_or_y = 0; x_or_y < 2; ++x_or_y) {
+        float ordinate = ((x_or_y == 0) ? (longitude / 2.0f) / precision : latitude / precision) + 90;
+        uint32_t divisions = 1;
+
+        for (uint8_t pair = 0; pair < pair_count; pair++) {
+            divisions *= loc_char_range[pair];
+            const float square_size = 180.0f / divisions;
+
+            uint8_t locvalue = (uint8_t) (ordinate / square_size);
+            ordinate -= square_size * (float) locvalue;
+            locvalue += (loc_char_range[pair] == 10) ? '0' : 'A';
+            locator[pair * 2 + x_or_y] = locvalue;
+        }
+    }
+
+    locator[pair_count * 2] = 0;
+}
diff --git a/src/locator.h b/src/locator.h
new file mode 100644
index 0000000..74dfdc3
--- /dev/null
+++ b/src/locator.h
@@ -0,0 +1,8 @@
+#ifndef __LOCATOR_H
+#define __LOCATOR_H
+
+#include <stdint.h>
+
+void locator_from_lonlat(int32_t longitude, int32_t latitude, uint8_t pair_count, char *locator);
+
+#endif
diff --git a/src/log.h b/src/log.h
index 685948c..ce68cf2 100644
--- a/src/log.h
+++ b/src/log.h
@@ -1,7 +1,7 @@
 #ifndef __LOG_H
 #define __LOG_H
 
-#define SEMIHOSTING_ENABLE
+#include "config.h"
 
 #ifdef SEMIHOSTING_ENABLE
 
diff --git a/src/main.c b/src/main.c
index 94092f5..1595a51 100644
--- a/src/main.c
+++ b/src/main.c
@@ -14,6 +14,8 @@
 uint32_t counter = 0;
 bool led_state = true;
 
+gps_data current_gps_data;
+
 void handle_timer_tick()
 {
     if (!system_initialized) {
@@ -22,15 +24,28 @@ void handle_timer_tick()
 
     radio_handle_timer_tick();
 
-    counter = (counter + 1) % 10000;
+    counter = (counter + 1) % SYSTEM_SCHEDULER_TIMER_TICKS_PER_SECOND;
     if (counter == 0) {
-        led_state = !led_state;
-        system_set_green_led(led_state);
+        ubxg6010_get_current_gps_data(&current_gps_data);
+    }
+
+    // Blink fast until GPS fix is acquired
+    if (counter % (SYSTEM_SCHEDULER_TIMER_TICKS_PER_SECOND / 4) == 0)  {
+        if (current_gps_data.fix >= 3) {
+            if (counter == 0) {
+                led_state = !led_state;
+                system_set_green_led(led_state);
+            }
+        } else {
+            led_state = !led_state;
+            system_set_green_led(led_state);
+        }
     }
 }
 
 int main(void)
 {
+    bool success;
     system_handle_timer_tick = handle_timer_tick;
     usart_gps_handle_incoming_byte = ubxg6010_handle_incoming_byte;
 
@@ -45,8 +60,14 @@ int main(void)
     log_info("SPI init\n");
     spi_init();
 
+gps_init:
     log_info("GPS init\n");
-    ubxg6010_init();
+    success = ubxg6010_init();
+    if (!success) {
+        log_error("GPS initialization failed, retrying...\n")
+        goto gps_init;
+    }
+
     log_info("Si4032 init\n");
     si4032_init();
 
diff --git a/src/payload.h b/src/payload.h
index 919a0f4..9c0e357 100644
--- a/src/payload.h
+++ b/src/payload.h
@@ -7,7 +7,7 @@
 #include "telemetry.h"
 
 typedef struct _payload_encoder {
-    size_t (*encode)(uint8_t *payload, size_t length, telemetry_data *data);
+    uint16_t (*encode)(uint8_t *payload, uint16_t length, telemetry_data *data);
 } payload_encoder;
 
 #endif
diff --git a/src/radio.c b/src/radio.c
index 34da41d..7aef30e 100644
--- a/src/radio.c
+++ b/src/radio.c
@@ -53,12 +53,17 @@ typedef struct _radio_transmit_entry {
     fsk_encoder fsk_encoder;
 } radio_transmit_entry;
 
+#ifdef SEMIHOSTING_ENABLE
+char logged_payload[512];
+#endif
+
 static bool si4032_use_dma = false;
 
+static radio_transmit_entry *radio_current_transmit_entry = NULL;
+static uint8_t radio_current_transmit_entry_index = 0;
+
 static volatile bool radio_transmission_active = false;
 static volatile bool radio_transmission_finished = false;
-static volatile radio_transmit_entry *radio_current_transmit_entry = NULL;
-static volatile int radio_current_transmit_entry_index = 0;
 static volatile uint32_t radio_post_transmit_delay_counter = 0;
 static volatile uint32_t radio_next_symbol_counter = 0;
 
@@ -68,8 +73,8 @@ static volatile uint64_t radio_si5351_freq = 0;
 static volatile bool radio_si4032_state_change = false;
 static volatile uint32_t radio_si4032_freq = 0;
 
-static volatile radio_transmit_entry *radio_start_transmit_entry = NULL;
-static volatile radio_transmit_entry *radio_stop_transmit_entry = NULL;
+static radio_transmit_entry *radio_start_transmit_entry = NULL;
+static radio_transmit_entry *radio_stop_transmit_entry = NULL;
 static volatile bool radio_transmit_next_symbol_flag = false;
 
 static volatile uint32_t radio_symbol_count_interrupt = 0;
@@ -81,7 +86,7 @@ static volatile int8_t radio_dma_transfer_stop_after_counter = -1;
 static volatile bool radio_manual_transmit_active = false;
 
 uint8_t radio_current_payload[RADIO_PAYLOAD_MAX_LENGTH];
-size_t radio_current_payload_length = 0;
+uint16_t radio_current_payload_length = 0;
 
 fsk_tone *radio_current_fsk_tones = NULL;
 int8_t radio_current_fsk_tone_count = 0;
@@ -93,17 +98,25 @@ uint32_t radio_current_symbol_delay_ms_100 = 0;
 telemetry_data current_telemetry_data;
 
 uint8_t aprs_packet[RADIO_PAYLOAD_MAX_LENGTH];
+char aprs_comment[APRS_COMMENT_MAX_LENGTH];
 
 static volatile uint32_t start_tick = 0, end_tick = 0;
 
-static size_t radio_fill_pwm_buffer(size_t offset, size_t length, uint16_t *buffer);
+static uint16_t radio_fill_pwm_buffer(uint16_t offset, uint16_t length, uint16_t *buffer);
 
-size_t radio_aprs_encode(uint8_t *payload, size_t length, telemetry_data *telemetry_data)
+uint16_t radio_aprs_encode(uint8_t *payload, uint16_t length, telemetry_data *telemetry_data)
 {
-    aprs_generate_position_without_timestamp(
-            aprs_packet, sizeof(aprs_packet), telemetry_data, APRS_SYMBOL_TABLE, APRS_SYMBOL, APRS_COMMENT);
 
-    log_debug("aprs: %s\n", aprs_packet);
+    gps_data *gps = &telemetry_data->gps;
+
+    snprintf(aprs_comment, sizeof(aprs_comment), " RS41ng test, time is %02d:%02d:%02d, locator %s, TOW %lu ms, gs %lu, hd %ld",
+            gps->hours, gps->minutes, gps->seconds, telemetry_data->locator, gps->time_of_week_millis,
+            gps->speed_raw, gps->heading_raw);
+
+    aprs_generate_position_without_timestamp(
+            aprs_packet, sizeof(aprs_packet), telemetry_data, APRS_SYMBOL_TABLE, APRS_SYMBOL, aprs_comment);
+
+    log_debug("APRS packet: %s\n", aprs_packet);
 
     return ax25_encode_packet_aprs(APRS_CALLSIGN, APRS_SSID, APRS_DESTINATION, APRS_DESTINATION_SSID, APRS_RELAYS,
             (char *) aprs_packet, length, payload);
@@ -113,20 +126,27 @@ payload_encoder radio_aprs_payload_encoder = {
         .encode = radio_aprs_encode,
 };
 
-size_t radio_ft8_encode(uint8_t *payload, size_t length, telemetry_data *telemetry_data)
+uint16_t radio_ft8_encode(uint8_t *payload, uint16_t length, telemetry_data *telemetry_data)
 {
-    // TODO: Encode locator for FT8
-    return snprintf((char *) payload, length, "%s %s", FT8_CALLSIGN, FT8_LOCATOR);
+    char locator[5];
+
+#ifdef FT8_LOCATOR_FIXED
+    locator = FT8_LOCATOR_FIXED;
+#else
+    strlcpy(locator, telemetry_data->locator, 4);
+#endif
+
+    return snprintf((char *) payload, length, "%s %s", FT8_CALLSIGN, locator);
 }
 
 payload_encoder radio_ft8_payload_encoder = {
         .encode = radio_ft8_encode,
 };
 
-size_t radio_wspr_encode(uint8_t *payload, size_t length, telemetry_data *telemetry_data)
+uint16_t radio_wspr_encode(uint8_t *payload, uint16_t length, telemetry_data *telemetry_data)
 {
-    // TODO: Encode locator for WSPR
-    return snprintf((char *) payload, length, "");
+    // Not used for WSPR
+    return 0;
 }
 
 payload_encoder radio_wspr_payload_encoder = {
@@ -266,16 +286,20 @@ static bool radio_start_transmit(radio_transmit_entry *entry)
 
     log_info("Full payload length: %d\n", radio_current_payload_length);
 
-    for (int i = 0; i < radio_current_payload_length; i++) {
+#ifdef SEMIHOSTING_ENABLE
+    int16_t len = 0;
+
+    for (uint16_t i = 0; i < radio_current_payload_length; i++) {
         char c = radio_current_payload[i];
         if (c >= 0x20 && c <= 0x7e) {
-            log_info("%c", c);
+            len += sprintf(logged_payload + len, "%c", c);
         } else {
-            log_info(" [%02X] ", c);
+            len += sprintf(logged_payload + len, " [%02X] ", c);
         }
     }
 
-    log_info("\n");
+    log_info("%s\n", logged_payload);
+#endif
 
     switch (entry->data_mode) {
         case RADIO_DATA_MODE_CW:
@@ -298,14 +322,20 @@ static bool radio_start_transmit(radio_transmit_entry *entry)
         case RADIO_DATA_MODE_FSQ_2:
         case RADIO_DATA_MODE_FSQ_3:
         case RADIO_DATA_MODE_FSQ_4_5:
-        case RADIO_DATA_MODE_FSQ_6:
-            // TODO: Encode WSPR locator
-            jtencode_encoder_new(&entry->fsk_encoder, entry->jtencode_mode_type, WSPR_CALLSIGN, WSPR_LOCATOR, WSPR_DBM,
-                    FSQ_CALLSIGN_FROM, FSQ_CALLSIGN_TO, FSQ_COMMAND);
+        case RADIO_DATA_MODE_FSQ_6: {
+            char locator[5];
+#ifdef WSPR_LOCATOR_FIXED
+            locator = WSPR_LOCATOR_FIXED;
+#else
+            strlcpy(locator, current_telemetry_data.locator, 4);
+#endif
+            jtencode_encoder_new(&entry->fsk_encoder, entry->jtencode_mode_type, WSPR_CALLSIGN, locator,
+                    WSPR_DBM, FSQ_CALLSIGN_FROM, FSQ_CALLSIGN_TO, FSQ_COMMAND);
             radio_current_symbol_delay_ms_100 = entry->fsk_encoder_api->get_symbol_delay(&entry->fsk_encoder);
             radio_current_tone_spacing_hz_100 = entry->fsk_encoder_api->get_tone_spacing(&entry->fsk_encoder);
             entry->fsk_encoder_api->set_data(&entry->fsk_encoder, radio_current_payload_length, radio_current_payload);
             break;
+        }
         default:
             return false;
     }
@@ -341,7 +371,7 @@ static bool radio_start_transmit(radio_transmit_entry *entry)
 
 static bool radio_stop_transmit_si4032(radio_transmit_entry *entry)
 {
-    bool use_direct_mode;
+    bool use_direct_mode = false;
 
     switch (entry->data_mode) {
         case RADIO_DATA_MODE_CW:
@@ -572,7 +602,7 @@ void radio_handle_main_loop()
     if (radio_post_transmit_delay_counter == 0) {
         telemetry_collect(&current_telemetry_data);
         log_info("Battery: %d mV\n", current_telemetry_data.battery_voltage_millivolts);
-        log_info("Internal temperature: %ld C*100\n", current_telemetry_data.internal_temperature_celsius_100);
+        log_info("Internal temperature: %ld C\n", current_telemetry_data.internal_temperature_celsius_100 / 100);
         log_info("Time: %02d:%02d:%02d\n",
                 current_telemetry_data.gps.hours, current_telemetry_data.gps.minutes,
                 current_telemetry_data.gps.seconds);
@@ -683,10 +713,10 @@ void radio_handle_main_loop()
     }
 }
 
-static size_t radio_fill_pwm_buffer(size_t offset, size_t length, uint16_t *buffer)
+static uint16_t radio_fill_pwm_buffer(uint16_t offset, uint16_t length, uint16_t *buffer)
 {
-    size_t count = 0;
-    for (size_t i = offset; i < (offset + length); i++, count++) {
+    uint16_t count = 0;
+    for (uint16_t i = offset; i < (offset + length); i++, count++) {
         uint32_t frequency = radio_next_symbol_si4032(radio_current_transmit_entry);
         if (frequency == 0) {
             // TODO: fill the other side of the buffer with zeroes too?
@@ -714,7 +744,7 @@ static bool radio_stop_dma_transfer_if_requested()
     return false;
 }
 
-size_t radio_handle_pwm_transfer_half(size_t buffer_size, uint16_t *buffer)
+uint16_t radio_handle_pwm_transfer_half(uint16_t buffer_size, uint16_t *buffer)
 {
     if (radio_stop_dma_transfer_if_requested()) {
         return 0;
@@ -723,7 +753,7 @@ size_t radio_handle_pwm_transfer_half(size_t buffer_size, uint16_t *buffer)
         log_info("Should not be here, half-transfer!\n");
     }
 
-    size_t length = radio_fill_pwm_buffer(0, buffer_size / 2, buffer);
+    uint16_t length = radio_fill_pwm_buffer(0, buffer_size / 2, buffer);
     if (radio_dma_transfer_stop_after_counter < 0 && length < buffer_size / 2) {
         radio_dma_transfer_stop_after_counter = 2;
     }
@@ -731,7 +761,7 @@ size_t radio_handle_pwm_transfer_half(size_t buffer_size, uint16_t *buffer)
     return length;
 }
 
-size_t radio_handle_pwm_transfer_full(size_t buffer_size, uint16_t *buffer)
+uint16_t radio_handle_pwm_transfer_full(uint16_t buffer_size, uint16_t *buffer)
 {
     if (radio_stop_dma_transfer_if_requested()) {
         return 0;
@@ -740,7 +770,7 @@ size_t radio_handle_pwm_transfer_full(size_t buffer_size, uint16_t *buffer)
         log_info("Should not be here, transfer complete!\n");
     }
 
-    size_t length = radio_fill_pwm_buffer(buffer_size / 2, buffer_size / 2, buffer);
+    uint16_t length = radio_fill_pwm_buffer(buffer_size / 2, buffer_size / 2, buffer);
     if (radio_dma_transfer_stop_after_counter < 0 && length < buffer_size / 2) {
         radio_dma_transfer_stop_after_counter = 2;
     }
@@ -750,6 +780,8 @@ size_t radio_handle_pwm_transfer_full(size_t buffer_size, uint16_t *buffer)
 
 void radio_init()
 {
+    memset(&current_telemetry_data, 0, sizeof(current_telemetry_data));
+
     pwm_handle_dma_transfer_half = radio_handle_pwm_transfer_half;
     pwm_handle_dma_transfer_full = radio_handle_pwm_transfer_full;
 
diff --git a/src/syscalls/semihosting.c b/src/syscalls/semihosting.c
index 96707a9..9197235 100644
--- a/src/syscalls/semihosting.c
+++ b/src/syscalls/semihosting.c
@@ -4,18 +4,20 @@
 #define OPENOCD_SYS_WRITE0 0x04
 #define OPENOCD_SYS_WRITE  0x05
 
-/*
- *
-I've used the following code to check for a connected debugger in the past with an STM32F4xx series MCU (when the only choice was the StdPeriph library -- perhaps this has changed with HAL/LL, but the hardware register and corresponding bit is obviously the same):
-
-if (CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk)
-{
-    // Debugger is connected
-}
+/**
+ * TODO:
+ * I've used the following code to check for a connected debugger in the past with an STM32F4xx series MCU
+ * (when the only choice was the StdPeriph library -- perhaps this has changed with HAL/LL, but the hardware
+ * register and corresponding bit is obviously the same):
+ * if (CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk)
+ * {
+ *     // Debugger is connected
+ * }
  */
 
 void openocd_send_command(int command, void *message)
 {
+
     asm("mov r0, %[cmd];"
         "mov r1, %[msg];"
         "bkpt #0xAB"
diff --git a/src/telemetry.c b/src/telemetry.c
index 65eb0a4..6564b7a 100644
--- a/src/telemetry.c
+++ b/src/telemetry.c
@@ -3,14 +3,18 @@
 #include "drivers/si4032/si4032.h"
 #include "drivers/ubxg6010/ubxg6010.h"
 #include "bmp280_handler.h"
+#include "locator.h"
 #include "config.h"
 
 void telemetry_collect(telemetry_data *data)
 {
     data->battery_voltage_millivolts = system_get_battery_voltage_millivolts();
     data->internal_temperature_celsius_100 = si4032_read_temperature_celsius_100();
+
     if (bmp280_enabled) {
         bmp280_read_telemetry(data);
     }
+
     ubxg6010_get_current_gps_data(&data->gps);
+    locator_from_lonlat(data->gps.lon_raw, data->gps.lat_raw, LOCATOR_PAIR_COUNT_FULL, data->locator);
 }
diff --git a/src/telemetry.h b/src/telemetry.h
index 777318d..b04d96b 100644
--- a/src/telemetry.h
+++ b/src/telemetry.h
@@ -4,6 +4,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 
+#include "config.h"
 #include "gps.h"
 
 typedef struct _telemetry_data {
@@ -15,6 +16,8 @@ typedef struct _telemetry_data {
     uint32_t humidity_percentage_100;
 
     gps_data gps;
+
+    char locator[LOCATOR_PAIR_COUNT_FULL * 2 + 1];
 } telemetry_data;
 
 void telemetry_collect();