diff --git a/README.md b/README.md
index c4fea26..21b12f0 100644
--- a/README.md
+++ b/README.md
@@ -110,7 +110,6 @@ Sensor driver code contributions are welcome!
 
 ### Planned features
 
-* Continuous transmission mode for Horus 4FSK
 * Configurable transmission frequencies and schedules based on location / altitude
 * Support for more I²C sensors
 * RTTY on both Si4032 (70 cm, non-standard shift) and Si5351 (HF + 2m) with configurable shift
diff --git a/src/bmp280_handler.c b/src/bmp280_handler.c
index 0bd1bb2..d4bc824 100644
--- a/src/bmp280_handler.c
+++ b/src/bmp280_handler.c
@@ -1,8 +1,11 @@
 #include "drivers/bmp280/bmp280.h"
 #include "bmp280_handler.h"
+#include "log.h"
 
 bmp280 bmp280_dev;
 
+static bool bmp280_initialization_required = true;
+
 bool bmp280_handler_init()
 {
     bmp280_dev.port = &DEFAULT_I2C_PORT;
@@ -17,12 +20,9 @@ bool bmp280_handler_init()
             .standby = BMP280_STANDBY_250,
     };
 
-    bool bmp280_init_success = bmp280_init(&bmp280_dev, &bmp280_params);
-    if (!bmp280_init_success) {
-        // TODO
-    }
-
-    return bmp280_init_success;
+    bool success = bmp280_init(&bmp280_dev, &bmp280_params);
+    bmp280_initialization_required = !success;
+    return success;
 }
 
 bool bmp280_read(int32_t *temperature_celsius_100, uint32_t *pressure_mbar_100, uint32_t *humidity_percentage_100)
@@ -33,6 +33,7 @@ bool bmp280_read(int32_t *temperature_celsius_100, uint32_t *pressure_mbar_100,
 
     bool success = bmp280_read_fixed(&bmp280_dev, &temperature_raw, &pressure_raw, &humidity_raw);
     if (!success) {
+        log_error("BMP280 read failed\n");
         return false;
     }
 
@@ -43,6 +44,7 @@ bool bmp280_read(int32_t *temperature_celsius_100, uint32_t *pressure_mbar_100,
         // Pressure unit is Pascal (= mbar * 100) * 256
         *pressure_mbar_100 = (uint32_t) (((float) pressure_raw) / 256.0f);
     }
+    // NOTE: Only BME280 sensors will report humidity. For BMP280 humidity readings will be zero.
     if (humidity_percentage_100) {
         *humidity_percentage_100 = (uint32_t) (((float) humidity_raw) * 100.0f / 1024.0f);
     }
@@ -52,5 +54,39 @@ bool bmp280_read(int32_t *temperature_celsius_100, uint32_t *pressure_mbar_100,
 
 bool bmp280_read_telemetry(telemetry_data *data)
 {
-    return bmp280_read(&data->temperature_celsius_100, &data->pressure_mbar_100, &data->humidity_percentage_100);
+    bool success;
+
+    if (bmp280_initialization_required) {
+        log_info("BMP280 re-init\n");
+        success = bmp280_handler_init();
+        log_info("BMP280 re-init: %d\n", success);
+        if (!success) {
+            data->temperature_celsius_100 = 0;
+            data->pressure_mbar_100 = 0;
+            data->humidity_percentage_100 = 0;
+            return false;
+        }
+    }
+
+    success = bmp280_read(&data->temperature_celsius_100, &data->pressure_mbar_100, &data->humidity_percentage_100);
+
+    if (!success) {
+        log_info("BMP280 re-init\n");
+        success = bmp280_handler_init();
+        log_info("BMP280 re-init: %d\n", success);
+
+        if (success) {
+            success = bmp280_read(&data->temperature_celsius_100, &data->pressure_mbar_100, &data->humidity_percentage_100);
+        }
+
+        if (!success) {
+            data->temperature_celsius_100 = 0;
+            data->pressure_mbar_100 = 0;
+            data->humidity_percentage_100 = 0;
+        }
+    }
+
+    bmp280_initialization_required = !success;
+
+    return success;
 }
diff --git a/src/codecs/horus/horus_packet_v2.c b/src/codecs/horus/horus_packet_v2.c
index cf1b5fa..b03d099 100644
--- a/src/codecs/horus/horus_packet_v2.c
+++ b/src/codecs/horus/horus_packet_v2.c
@@ -44,6 +44,8 @@ size_t horus_packet_v2_create(uint8_t *payload, size_t length, telemetry_data *d
         horus_packet.Sats += 200;
     }
 
+    memset(horus_packet.CustomData, 0, sizeof(horus_packet.CustomData));
+
     uint8_t *custom_data_pointer = horus_packet.CustomData;
 
     // Unit: cm/s
diff --git a/src/config.h b/src/config.h
index 3442f36..f11590c 100644
--- a/src/config.h
+++ b/src/config.h
@@ -11,17 +11,21 @@
  */
 
 // Set the tracker amateur radio call sign here
-#define CALLSIGN "MYCALL"
+#define CALLSIGN "OH3VHH"
 
 // Disabling LEDs will save power
 // Red LED: Lit during initialization and transmit.
 // Green LED: Blinking fast when there is no GPS fix. Blinking slowly when the GPS has a fix.
 #define LEDS_ENABLE true
 
+// Disable LEDs above the specified altitude (in meters) to save power. Set to zero to disable this behavior.
+#define LEDS_DISABLE_ALTITUDE_METERS 1000
+
 // Allow powering off the sonde by pressing the button for over a second (when the sonde is not transmitting)
 #define ALLOW_POWER_OFF false
 
-// Enable use of an externally connected I²C BMP280 atmospheric sensor
+// Enable use of an externally connected I²C BMP280/BME280 atmospheric sensor
+// NOTE: Only BME280 sensors will report humidity. For BMP280 humidity readings will be zero.
 #define SENSOR_BMP280_ENABLE false
 
 // Enable use of an externally connected I²C Si5351 clock generator chip for HF radio transmissions
@@ -36,6 +40,9 @@
 // Threshold for time-synchronized modes regarding how far from scheduled transmission time the transmission is still allowed
 #define RADIO_TIME_SYNC_THRESHOLD_MS 2000
 
+// Enable this setting to require 3D fix (altitude required, enable for airborne use), otherwise 2D fix is enough
+#define GPS_REQUIRE_3D_FIX true
+
 // Enable NMEA output from GPS via external serial port. This disables use of I²C bus (Si5351 and sensors) because the pins are shared.
 #define GPS_NMEA_OUTPUT_VIA_SERIAL_PORT_ENABLE false
 
diff --git a/src/config_internal.h b/src/config_internal.h
index 1a39e18..2ff46a0 100644
--- a/src/config_internal.h
+++ b/src/config_internal.h
@@ -27,4 +27,7 @@ extern char *aprs_comment_templates[];
 extern char *fsq_comment_templates[];
 extern char *ftjt_message_templates[];
 
+void set_green_led(bool enabled);
+void set_red_led(bool enabled);
+
 #endif
diff --git a/src/drivers/bmp280/bmp280.c b/src/drivers/bmp280/bmp280.c
index ec4f462..83fdb88 100644
--- a/src/drivers/bmp280/bmp280.c
+++ b/src/drivers/bmp280/bmp280.c
@@ -48,6 +48,8 @@
 
 #define BMP280_RESET_VALUE     0xB6
 
+#define BMP280_TIMEOUT_COUNTER 0xFFFFF
+
 void bmp280_init_default_params(bmp280_params_t *params)
 {
     params->mode = BMP280_MODE_NORMAL;
@@ -132,6 +134,8 @@ static int write_register8(bmp280 *dev, uint8_t reg, uint8_t value)
 
 bool bmp280_init(bmp280 *dev, bmp280_params_t *params)
 {
+    uint32_t timeout;
+
     if (dev->addr != BMP280_I2C_ADDRESS_0
         && dev->addr != BMP280_I2C_ADDRESS_1) {
 
@@ -152,12 +156,16 @@ bool bmp280_init(bmp280 *dev, bmp280_params_t *params)
     }
 
     // Wait until finished copying over the NVP data.
-    while (1) {
+    timeout = BMP280_TIMEOUT_COUNTER;
+    while (timeout-- > 0) {
         uint8_t status;
         if (read_data(dev, BMP280_REG_STATUS, &status, 1) && (status & 1) == 0) {
             break;
         }
     }
+    if (timeout == 0) {
+        return false;
+    }
 
     if (!read_calibration_data(dev)) {
         return false;
diff --git a/src/drivers/si5351/si5351.cpp b/src/drivers/si5351/si5351.cpp
index 4ac777b..788702d 100644
--- a/src/drivers/si5351/si5351.cpp
+++ b/src/drivers/si5351/si5351.cpp
@@ -57,12 +57,16 @@ Si5351::Si5351(i2c_port *port, uint8_t i2c_addr) :
  */
 bool Si5351::init(uint8_t xtal_load_c, uint32_t xo_freq, int32_t corr)
 {
+    uint32_t timeout = 0xFFFFFF;
     // Wait for SYS_INIT flag to be clear, indicating that device is ready
     uint8_t status_reg = 0;
     do {
         status_reg = si5351_read(SI5351_DEVICE_STATUS);
-        // TODO: timeout
-    } while (status_reg >> 7 == 1);
+    } while (status_reg >> 7 == 1 && timeout-- > 0);
+
+    if (timeout == 0) {
+        return false;
+    }
 
     // Set crystal load capacitance
     si5351_write(SI5351_CRYSTAL_LOAD, (xtal_load_c & SI5351_CRYSTAL_LOAD_MASK) | 0b00010010);
diff --git a/src/gps.h b/src/gps.h
index da95760..31775a7 100644
--- a/src/gps.h
+++ b/src/gps.h
@@ -3,12 +3,26 @@
 
 #include <stdint.h>
 #include <stdbool.h>
+#include "config.h"
 
 #define POWER_SAFE_MODE_STATE_ACQUISITION 0
 #define POWER_SAFE_MODE_STATE_TRACKING 1
 #define POWER_SAFE_MODE_STATE_POWER_OPTIMIZED_TRACKING 2
 #define POWER_SAFE_MODE_STATE_INACTIVE 3
 
+#define GPS_FIX_NO_FIX 0
+#define GPS_FIX_DEAD_RECKONING_ONLY 1
+#define GPS_FIX_2D 2
+#define GPS_FIX_3D 3
+#define GPS_FIX_GPS_AND_DEAD_RECKONING 4
+#define GPS_FIX_TIME_ONLY 5
+
+#if GPS_REQUIRE_3D_FIX
+#define GPS_HAS_FIX(gps_data) (gps_data.fix_ok && (gps_data.fix == GPS_FIX_3D))
+#else
+#define GPS_HAS_FIX(gps_data) (gps_data.fix_ok && (gps_data.fix == GPS_FIX_2D || gps_data.fix == GPS_FIX_3D))
+#endif
+
 typedef struct _gps_data {
     bool updated;
 
diff --git a/src/hal/i2c.c b/src/hal/i2c.c
index dc3a4da..9bfa0fb 100644
--- a/src/hal/i2c.c
+++ b/src/hal/i2c.c
@@ -7,6 +7,8 @@
 #include "delay.h"
 #include "log.h"
 
+#define I2C_TIMEOUT_COUNTER 0x4FFFF
+
 struct _i2c_port {
     I2C_TypeDef *i2c;
 };
@@ -61,7 +63,7 @@ void i2c_init()
         delay_ms(1);
     }
     if (count == 0) {
-        log_error("ERROR: I²C bus busy during initialization");
+        log_error("ERROR: I²C bus busy during initialization\n");
     }
 
     I2C_Cmd(I2C_PORT, ENABLE);
@@ -97,11 +99,11 @@ static void i2c_reset_bus(i2c_port *port)
 
 static int i2c_wait_until_not_busy(i2c_port *port)
 {
-    __IO uint32_t timeout = 0xFFFFFF;
+    __IO uint32_t timeout = I2C_TIMEOUT_COUNTER;
 
     while (I2C_GetFlagStatus(I2C_PORT, I2C_FLAG_BUSY)) {
         if (--timeout == 0) {
-            log_error("ERROR: Timeout - I2C bus busy");
+            log_error("ERROR: Timeout - I2C bus busy\n");
             i2c_reset_bus(port);
             return HAL_ERROR_TIMEOUT;
         }
@@ -112,11 +114,11 @@ static int i2c_wait_until_not_busy(i2c_port *port)
 
 static bool i2c_wait_event(i2c_port *port, uint32_t flag)
 {
-    __IO uint32_t timeout = 0xFFFFFF;
+    __IO uint32_t timeout = I2C_TIMEOUT_COUNTER;
 
     while (I2C_CheckEvent(port->i2c, flag) != SUCCESS) {
         if (--timeout == 0) {
-            log_error("ERROR: WE event timeout: 0x%08lx", flag);
+            log_error("ERROR: WE event timeout: 0x%08lx\n", flag);
             i2c_reset_bus(port);
             return false;
         }
@@ -131,13 +133,13 @@ static int i2c_prepare_op(i2c_port *port, uint8_t address)
     I2C_GenerateSTART(port->i2c, ENABLE);
 
     if (!i2c_wait_event(port, I2C_EVENT_MASTER_MODE_SELECT)) {
-        log_error("ERROR: PO MMS timeout: 0x%02x", address);
+        log_error("ERROR: PO MMS timeout: 0x%02x\n", address);
         return HAL_ERROR_TIMEOUT;
     }
     I2C_Send7bitAddress(port->i2c, address, I2C_Direction_Transmitter);
 
     if (!i2c_wait_event(port, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) {
-        log_error("ERROR: PO MTMS timeout: 0x%02x", address);
+        log_error("ERROR: PO MTMS timeout: 0x%02x\n", address);
         return HAL_ERROR_TIMEOUT;
     }
 
@@ -153,7 +155,7 @@ static int i2c_prepare_op_with_register(i2c_port *port, uint8_t address, uint8_t
 
     I2C_SendData(port->i2c, reg);
     if (!i2c_wait_event(port, I2C_EVENT_MASTER_BYTE_TRANSMITTED)) {
-        log_error("ERROR: POWR MBT timeout: 0x%02x 0x%02x", address, reg);
+        log_error("ERROR: POWR MBT timeout: 0x%02x 0x%02x\n", address, reg);
         return HAL_ERROR_TIMEOUT;
     }
 
@@ -169,13 +171,13 @@ static int i2c_prepare_read(i2c_port *port, uint8_t address, uint8_t reg)
 
     I2C_GenerateSTART(port->i2c, ENABLE);
     if (!i2c_wait_event(port, I2C_EVENT_MASTER_MODE_SELECT)) {
-        log_error("ERROR: PR MMS timeout: 0x%02x 0x%02x", address, reg);
+        log_error("ERROR: PR MMS timeout: 0x%02x 0x%02x\n", address, reg);
         return HAL_ERROR_TIMEOUT;
     }
 
     I2C_Send7bitAddress(port->i2c, address, I2C_Direction_Receiver);
     if (!i2c_wait_event(port, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)) {
-        log_error("ERROR: PR MRMS timeout: 0x%02x 0x%02x", address, reg);
+        log_error("ERROR: PR MRMS timeout: 0x%02x 0x%02x\n", address, reg);
         return HAL_ERROR_TIMEOUT;
     }
 
@@ -188,7 +190,7 @@ static int i2c_finish_read(i2c_port *port)
     I2C_AcknowledgeConfig(port->i2c, DISABLE);
 
     if (!i2c_wait_event(port, I2C_EVENT_MASTER_BYTE_RECEIVED)) {
-        log_error("ERROR: FR MBR timeout");
+        log_error("ERROR: FR MBR timeout\n");
         return HAL_ERROR_TIMEOUT;
     }
     I2C_ReceiveData(port->i2c);
@@ -206,7 +208,7 @@ static int i2c_finish_write(i2c_port *port)
     I2C_GenerateSTOP(port->i2c, ENABLE);
 
     if (!i2c_wait_event(port, I2C_EVENT_MASTER_BYTE_TRANSMITTED)) {
-        log_error("ERROR: FW MBT timeout");
+        log_error("ERROR: FW MBT timeout\n");
         return HAL_ERROR_TIMEOUT;
     }
 
@@ -227,7 +229,7 @@ int i2c_read_bytes(i2c_port *port, uint8_t address, uint8_t reg, uint8_t size, u
 
     for (uint8_t i = 0; i < size; i++) {
         if (!i2c_wait_event(port, I2C_EVENT_MASTER_BYTE_RECEIVED)) {
-            log_error("ERROR: MBR timeout: 0x%02x 0x%02x", address, reg);
+            log_error("ERROR: MBR timeout: 0x%02x 0x%02x\n", address, reg);
             return HAL_ERROR_TIMEOUT;
         }
         data[i] = I2C_ReceiveData(port->i2c);
@@ -258,7 +260,7 @@ int i2c_write_bytes(i2c_port *port, uint8_t address, uint8_t reg, uint8_t size,
     for (uint8_t i = 0; i < size; i++) {
         I2C_SendData(port->i2c, data[i]);
         if (!i2c_wait_event(port, I2C_EVENT_MASTER_BYTE_TRANSMITTED)) {
-            log_error("ERROR: MBF timeout: 0x%02x 0x%02x", address, reg);
+            log_error("ERROR: MBF timeout: 0x%02x 0x%02x\n", address, reg);
             return HAL_ERROR_TIMEOUT;
         }
     }
diff --git a/src/main.c b/src/main.c
index 4237b7d..5f5023f 100644
--- a/src/main.c
+++ b/src/main.c
@@ -34,19 +34,37 @@ void handle_timer_tick()
     if (leds_enabled) {
         // Blink fast until GPS fix is acquired
         if (counter % (SYSTEM_SCHEDULER_TIMER_TICKS_PER_SECOND / 4) == 0)  {
-            if (current_gps_data.fix_ok) {
+            if (GPS_HAS_FIX(current_gps_data)) {
                 if (counter == 0) {
                     led_state = !led_state;
-                    system_set_green_led(led_state);
+                    set_green_led(led_state);
                 }
             } else {
                 led_state = !led_state;
-                system_set_green_led(led_state);
+                set_green_led(led_state);
             }
         }
     }
 }
 
+void set_green_led(bool enabled)
+{
+    if ((LEDS_DISABLE_ALTITUDE_METERS > 0) && (current_gps_data.altitude_mm / 1000 > LEDS_DISABLE_ALTITUDE_METERS)) {
+        enabled = false;
+    }
+
+    system_set_green_led(enabled);
+}
+
+void set_red_led(bool enabled)
+{
+    if ((LEDS_DISABLE_ALTITUDE_METERS > 0) && (current_gps_data.altitude_mm / 1000 > LEDS_DISABLE_ALTITUDE_METERS)) {
+        enabled = false;
+    }
+
+    system_set_red_led(enabled);
+}
+
 int main(void)
 {
     bool success;
@@ -59,8 +77,8 @@ int main(void)
     log_info("System init\n");
     system_init();
 
-    system_set_green_led(false);
-    system_set_red_led(true);
+    set_green_led(false);
+    set_red_led(true);
 
     if (gps_nmea_output_enabled) {
         log_info("External USART init\n");
@@ -85,13 +103,25 @@ gps_init:
     si4032_init();
 
     if (bmp280_enabled) {
-        log_info("BMP280 init\n");
-        bmp280_enabled = bmp280_handler_init();
+        for (int i = 0; i < 3; i++) {
+            log_info("BMP280 init\n");
+            success = bmp280_handler_init();
+            if (success) {
+                break;
+            }
+            log_error("BMP280 init failed, retrying...");
+        }
     }
 
     if (si5351_enabled) {
-        log_info("Si5351 init\n");
-        si5351_enabled = si5351_handler_init();
+        for (int i = 0; i < 3; i++) {
+            log_info("Si5351 init\n");
+            success = si5351_handler_init();
+            if (success) {
+                break;
+            }
+            log_error("Si5351 init failed, retrying...");
+        }
     }
 
     log_info("Radio module init\n");
@@ -102,11 +132,11 @@ gps_init:
     log_info("System initialized!\n");
 
     if (leds_enabled) {
-        system_set_green_led(true);
-        system_set_red_led(false);
+        set_green_led(true);
+        set_red_led(false);
     } else {
-        system_set_green_led(false);
-        system_set_red_led(false);
+        set_green_led(false);
+        set_red_led(false);
     }
 
     system_initialized = true;
diff --git a/src/radio.c b/src/radio.c
index 2175a57..d8daefa 100644
--- a/src/radio.c
+++ b/src/radio.c
@@ -494,7 +494,7 @@ static bool radio_start_transmit(radio_transmit_entry *entry)
     log_info("TX start\n");
 
     if (leds_enabled) {
-        system_set_red_led(true);
+        set_red_led(true);
     }
 
     radio_shared_state.radio_transmission_active = true;
@@ -568,7 +568,7 @@ static bool radio_stop_transmit(radio_transmit_entry *entry)
 
     usart_gps_enable(true);
     if (leds_enabled) {
-        system_set_red_led(false);
+        set_red_led(false);
     }
 
     return success;
diff --git a/src/radio_si5351.c b/src/radio_si5351.c
index 781db54..ebc8564 100644
--- a/src/radio_si5351.c
+++ b/src/radio_si5351.c
@@ -33,6 +33,7 @@ bool radio_start_transmit_si5351(radio_transmit_entry *entry, radio_module_state
             break;
     }
 
+    // TODO: handle Si5351 errors
     if (use_fast_si5351) {
         si5351_set_drive_strength_fast(SI5351_CLOCK_CLK0, entry->tx_power);
         if (set_frequency_early) {
@@ -97,6 +98,7 @@ void radio_handle_main_loop_si5351(radio_transmit_entry *entry, radio_module_sta
         return;
     }
 
+    // TODO: handle Si5351 errors
     if (radio_si5351_state_change) {
         radio_si5351_state_change = false;
         si5351_set_frequency(SI5351_CLOCK_CLK0, radio_si5351_freq);
@@ -111,6 +113,7 @@ inline void radio_handle_data_timer_si5351()
         return;
     }
 
+    // TODO: handle Si5351 errors
     switch (radio_current_transmit_entry->data_mode) {
         case RADIO_DATA_MODE_CW: {
             cw_symbol_rate_multiplier--;
diff --git a/src/telemetry.c b/src/telemetry.c
index e36f4a8..14588c4 100644
--- a/src/telemetry.c
+++ b/src/telemetry.c
@@ -5,9 +5,12 @@
 #include "bmp280_handler.h"
 #include "locator.h"
 #include "config.h"
+#include "log.h"
 
 void telemetry_collect(telemetry_data *data)
 {
+    log_info("Collecting telemetry...\n");
+
     data->button_adc_value = system_get_button_adc_value();
     data->battery_voltage_millivolts = system_get_battery_voltage_millivolts();
     data->internal_temperature_celsius_100 = si4032_read_temperature_celsius_100();
@@ -20,7 +23,7 @@ void telemetry_collect(telemetry_data *data)
 
     // Zero out position data if we don't have a valid GPS fix.
     // This is done to avoid transmitting invalid position information.
-    if (!data->gps.fix_ok) {
+    if (!GPS_HAS_FIX(data->gps)) {
         data->gps.latitude_degrees_1000000 = 0;
         data->gps.longitude_degrees_1000000 = 0;
         data->gps.altitude_mm = 0;
@@ -31,4 +34,6 @@ void telemetry_collect(telemetry_data *data)
 
     locator_from_lonlat(data->gps.longitude_degrees_1000000, data->gps.latitude_degrees_1000000,
             LOCATOR_PAIR_COUNT_FULL, data->locator);
+
+    log_info("Telemetry collected!\n");
 }