diff --git a/src/bin/temp_rh_measurement.rs b/src/bin/temp_rh_measurement.rs
index 78272e6..4cf083f 100644
--- a/src/bin/temp_rh_measurement.rs
+++ b/src/bin/temp_rh_measurement.rs
@@ -1,9 +1,17 @@
 #![no_std]
 #![no_main]
 
+use static_cell::StaticCell;
+
+use core::mem;
+use embassy_nrf::interrupt::{self, InterruptExt};
 use embassy_nrf::peripherals::TWISPI0;
 use embassy_time::Timer;
-use nrf_softdevice::Softdevice;
+use nrf_softdevice::ble::advertisement_builder::{
+    Flag, LegacyAdvertisementBuilder, LegacyAdvertisementPayload, ServiceList, ServiceUuid16,
+};
+use nrf_softdevice::ble::{gatt_server, peripheral};
+use nrf_softdevice::{raw, Softdevice};
 
 use defmt::*;
 use embassy_executor::Spawner;
@@ -12,6 +20,12 @@ use embassy_nrf::twim;
 use embassy_nrf::{bind_interrupts, peripherals};
 use {defmt_rtt as _, panic_probe as _};
 
+//
+//
+// Sensing code
+//
+//
+
 struct GenericTempHumiditySensor<T: twim::Instance> {
     address: u8,
     driver: twim::Twim<'static, T>,
@@ -49,17 +63,105 @@ bind_interrupts!(struct Irqs {
 type TempHumiditySensor = GenericTempHumiditySensor<TWISPI0>;
 
 #[embassy_executor::task]
-async fn measurement_task(mut sensor: TempHumiditySensor) {
+async fn measurement_task(mut sensor: TempHumiditySensor, server: &'static Server) {
     loop {
         let (temp, humidity) = sensor.read_temp_and_humidity().await;
         println!("{} °C\t{} %", temp, humidity);
-        Timer::after_millis(50).await;
+
+        let temp: u32 = (temp * 100.0) as u32;
+        let humidity: u32 = (humidity * 100.0) as u32;
+
+        unwrap!(server.bas.temperature_set(&temp));
+        unwrap!(server.bas.humidity_set(&humidity));
+
+        Timer::after_secs(5).await;
     }
 }
 
+//
+//
+// BLE code
+//
+//
+
+#[nrf_softdevice::gatt_service(uuid = "181a")]
+struct EnvironmentalService {
+    #[characteristic(uuid = "2a6e", read, notify)]
+    temperature: u32,
+    #[characteristic(uuid = "2a6f", read, notify)]
+    humidity: u32,
+}
+
+#[nrf_softdevice::gatt_server]
+struct Server {
+    bas: EnvironmentalService,
+}
+
+#[embassy_executor::task]
+async fn softdevice_task(sd: &'static Softdevice) -> ! {
+    sd.run().await
+}
+
+#[embassy_executor::task]
+async fn ble_task(sd: &'static Softdevice, server: &'static Server) {
+    static ADV_DATA: LegacyAdvertisementPayload = LegacyAdvertisementBuilder::new()
+        .flags(&[Flag::GeneralDiscovery, Flag::LE_Only])
+        .services_16(ServiceList::Complete, &[ServiceUuid16::BATTERY])
+        .full_name("CommonSense")
+        .build();
+
+    static SCAN_DATA: LegacyAdvertisementPayload = LegacyAdvertisementBuilder::new()
+        .services_128(
+            ServiceList::Complete,
+            &[0x9e7312e0_2354_11eb_9f10_fbc30a62cf38_u128.to_le_bytes()],
+        )
+        .build();
+
+    loop {
+        info!("Creating config");
+        let config = peripheral::Config::default();
+        info!("Creating adv object");
+        let adv = peripheral::ConnectableAdvertisement::ScannableUndirected {
+            adv_data: &ADV_DATA,
+            scan_data: &SCAN_DATA,
+        };
+
+        let conn = unwrap!(peripheral::advertise_connectable(sd, adv, &config).await);
+
+        let _e = gatt_server::run(&conn, server, |e| match e {
+            ServerEvent::Bas(e) => match e {
+                EnvironmentalServiceEvent::HumidityCccdWrite { notifications } => {
+                    info!("humidity notifications: {}", notifications)
+                }
+                EnvironmentalServiceEvent::TemperatureCccdWrite { notifications } => {
+                    info!("temperature notifications: {}", notifications)
+                }
+            },
+        })
+        .await;
+
+        Timer::after_secs(10).await;
+    }
+}
+
+//
+//
+// Main code
+//
+//
+
+static SERVER: StaticCell<Server> = StaticCell::new();
+
 #[embassy_executor::main]
 async fn main(spawner: Spawner) {
-    let p = embassy_nrf::init(Default::default());
+    // Set up peripherals
+
+    interrupt::SPIM0_SPIS0_TWIM0_TWIS0_SPI0_TWI0.set_priority(interrupt::Priority::P2);
+
+    let mut embassy_config = embassy_nrf::config::Config::default();
+    embassy_config.gpiote_interrupt_priority = interrupt::Priority::P2;
+    embassy_config.time_interrupt_priority = interrupt::Priority::P2;
+    let p = embassy_nrf::init(embassy_config);
 
     let config = twim::Config::default();
     let twi = twim::Twim::new(p.TWISPI0, Irqs, p.P0_14, p.P0_13, config);
@@ -67,7 +169,48 @@ async fn main(spawner: Spawner) {
 
     let mut led = Output::new(p.P0_20, Level::Low, OutputDrive::Standard);
 
-    unwrap!(spawner.spawn(measurement_task(sensor)));
+    // Set up softdevice
+
+    let config = nrf_softdevice::Config {
+        clock: Some(raw::nrf_clock_lf_cfg_t {
+            source: raw::NRF_CLOCK_LF_SRC_RC as u8,
+            rc_ctiv: 16,
+            rc_temp_ctiv: 2,
+            accuracy: raw::NRF_CLOCK_LF_ACCURACY_500_PPM as u8,
+        }),
+        conn_gap: Some(raw::ble_gap_conn_cfg_t {
+            conn_count: 1,
+            event_length: 24,
+        }),
+        conn_gatt: Some(raw::ble_gatt_conn_cfg_t { att_mtu: 256 }),
+        gatts_attr_tab_size: Some(raw::ble_gatts_cfg_attr_tab_size_t {
+            attr_tab_size: raw::BLE_GATTS_ATTR_TAB_SIZE_DEFAULT,
+        }),
+        gap_role_count: Some(raw::ble_gap_cfg_role_count_t {
+            adv_set_count: 1,
+            periph_role_count: 1,
+        }),
+        gap_device_name: Some(raw::ble_gap_cfg_device_name_t {
+            p_value: b"CommonSense" as *const u8 as _,
+            current_len: 9,
+            max_len: 9,
+            write_perm: unsafe { mem::zeroed() },
+            _bitfield_1: raw::ble_gap_cfg_device_name_t::new_bitfield_1(
+                raw::BLE_GATTS_VLOC_STACK as u8,
+            ),
+        }),
+        ..Default::default()
+    };
+
+    let sd = Softdevice::enable(&config);
+    let server = unwrap!(Server::new(sd));
+    let server: &'static mut Server = SERVER.init(server);
+
+    // Start tasks
+
+    unwrap!(spawner.spawn(softdevice_task(sd)));
+    unwrap!(spawner.spawn(ble_task(sd, server)));
+    unwrap!(spawner.spawn(measurement_task(sensor, server)));
 
     loop {
         led.set_high();