Bluetooth monitor and more
This commit is contained in:
		
							parent
							
								
									7501ef18a4
								
							
						
					
					
						commit
						fb6f10891d
					
				| 
						 | 
					@ -5,3 +5,4 @@ ve_*
 | 
				
			||||||
/music
 | 
					/music
 | 
				
			||||||
__pycache__
 | 
					__pycache__
 | 
				
			||||||
/roles/pi-squeezeserver/backup
 | 
					/roles/pi-squeezeserver/backup
 | 
				
			||||||
 | 
					venv
 | 
				
			||||||
							
								
								
									
										38
									
								
								full.yml
								
								
								
								
							
							
						
						
									
										38
									
								
								full.yml
								
								
								
								
							| 
						 | 
					@ -9,6 +9,7 @@
 | 
				
			||||||
#    - pi-lirc
 | 
					#    - pi-lirc
 | 
				
			||||||
#    - pi-sispmctl
 | 
					#    - pi-sispmctl
 | 
				
			||||||
#
 | 
					#
 | 
				
			||||||
 | 
					
 | 
				
			||||||
- hosts: musikserverwohnzimmeroben
 | 
					- hosts: musikserverwohnzimmeroben
 | 
				
			||||||
  roles:
 | 
					  roles:
 | 
				
			||||||
    - pi-standard-setup
 | 
					    - pi-standard-setup
 | 
				
			||||||
| 
						 | 
					@ -16,26 +17,29 @@
 | 
				
			||||||
    - pi-squeezelite-custom
 | 
					    - pi-squeezelite-custom
 | 
				
			||||||
    - pi-shairport
 | 
					    - pi-shairport
 | 
				
			||||||
    - pi-irserver
 | 
					    - pi-irserver
 | 
				
			||||||
    - pi-dhtsensor
 | 
					    #- pi-dhtsensor
 | 
				
			||||||
    - pi-squeezeserver
 | 
					    - pi-squeezeserver
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					- hosts: kitchenpi  
 | 
				
			||||||
 | 
					  roles:
 | 
				
			||||||
 | 
					    - pi-standard-setup
 | 
				
			||||||
 | 
					    - pi-hifiberry-amp
 | 
				
			||||||
 | 
					    - pi-squeezelite-custom
 | 
				
			||||||
 | 
					    - pi-shairport
 | 
				
			||||||
 | 
					    - pi-lirc
 | 
				
			||||||
 | 
					    - pi-dhtsensor
 | 
				
			||||||
 | 
					    - pi-disable-onboard-bluetooth
 | 
				
			||||||
 | 
					    - bluetooth-monitor
 | 
				
			||||||
 | 
					
 | 
				
			||||||
#- hosts: kitchenpi  
 | 
					- hosts: bedroompi
 | 
				
			||||||
#  roles:
 | 
					  roles:
 | 
				
			||||||
#    - pi-standard-setup
 | 
					    - pi-standard-setup
 | 
				
			||||||
#    - pi-hifiberry-amp
 | 
					    - pi-squeezelite-custom
 | 
				
			||||||
#    - pi-squeezelite-custom
 | 
					    - pi-shairport
 | 
				
			||||||
#    - pi-shairport
 | 
					    - pi-lirc
 | 
				
			||||||
#    - pi-lirc
 | 
					    - pi-dhtsensor
 | 
				
			||||||
#    - pi-dhtsensor
 | 
					    - pi-disable-onboard-bluetooth
 | 
				
			||||||
 | 
					    - bluetooth-monitor
 | 
				
			||||||
#- hosts: bedroompi
 | 
					 | 
				
			||||||
#  roles:
 | 
					 | 
				
			||||||
#    - pi-standard-setup
 | 
					 | 
				
			||||||
#    - pi-squeezelite-custom
 | 
					 | 
				
			||||||
#    - pi-shairport
 | 
					 | 
				
			||||||
#    - pi-lirc
 | 
					 | 
				
			||||||
#    - pi-dhtsensor
 | 
					 | 
				
			||||||
 | 
					
 | 
				
			||||||
#- hosts: octopi
 | 
					#- hosts: octopi
 | 
				
			||||||
#  roles:
 | 
					#  roles:
 | 
				
			||||||
| 
						 | 
					
 | 
				
			||||||
| 
						 | 
					@ -40,6 +40,7 @@ all:
 | 
				
			||||||
          sensor_room_name_ascii: wohnzimmeroben
 | 
					          sensor_room_name_ascii: wohnzimmeroben
 | 
				
			||||||
          sensor_room_name: WohnzimmerOben
 | 
					          sensor_room_name: WohnzimmerOben
 | 
				
			||||||
          hifiberry_overlay: hifiberry-dacplus
 | 
					          hifiberry_overlay: hifiberry-dacplus
 | 
				
			||||||
 | 
					          my_btmonitor_pl0: 68 # default is 73 - increase to make more sensitve (i.e. lower distances)
 | 
				
			||||||
        musicmouse:
 | 
					        musicmouse:
 | 
				
			||||||
          squeezelite_name: MusicMouse
 | 
					          squeezelite_name: MusicMouse
 | 
				
			||||||
          shairport_name: MusicMouse
 | 
					          shairport_name: MusicMouse
 | 
				
			||||||
| 
						 | 
					@ -52,6 +53,10 @@ all:
 | 
				
			||||||
          sensor_room_name_ascii: testraum
 | 
					          sensor_room_name_ascii: testraum
 | 
				
			||||||
          sensor_room_name: Test Raum
 | 
					          sensor_room_name: Test Raum
 | 
				
			||||||
        heatingpi:          
 | 
					        heatingpi:          
 | 
				
			||||||
 | 
					        server:
 | 
				
			||||||
 | 
					          sensor_room_name: Arbeitszimmer
 | 
				
			||||||
 | 
					          sensor_room_name_ascii: arbeitszimmer
 | 
				
			||||||
 | 
					          my_btmonitor_pl0: 78
 | 
				
			||||||
  vars:
 | 
					  vars:
 | 
				
			||||||
    ansible_user: root
 | 
					    ansible_user: root
 | 
				
			||||||
    ansible_python_interpreter: /usr/bin/python3
 | 
					    ansible_python_interpreter: /usr/bin/python3
 | 
				
			||||||
| 
						 | 
					@ -61,3 +66,5 @@ all:
 | 
				
			||||||
    home_assistant_token: eyJhbGciOiJIUzI1NiIsInR5cCI6IkpXVCJ9.eyJpc3MiOiJkM2QxYjAwYjkxZjY0MWVhYjA4YmZhMDYwYTg3YjRhNyIsImlhdCI6MTcwNDI3MDU5MSwiZXhwIjoyMDE5NjMwNTkxfQ.dzvejgEQd9hf-Yftzd7NkR5pv76GaLFczeOy-a2pa1o
 | 
					    home_assistant_token: eyJhbGciOiJIUzI1NiIsInR5cCI6IkpXVCJ9.eyJpc3MiOiJkM2QxYjAwYjkxZjY0MWVhYjA4YmZhMDYwYTg3YjRhNyIsImlhdCI6MTcwNDI3MDU5MSwiZXhwIjoyMDE5NjMwNTkxfQ.dzvejgEQd9hf-Yftzd7NkR5pv76GaLFczeOy-a2pa1o
 | 
				
			||||||
    configure_wifi: false
 | 
					    configure_wifi: false
 | 
				
			||||||
    wifi_ssid: BauerWLAN
 | 
					    wifi_ssid: BauerWLAN
 | 
				
			||||||
 | 
					    my_btmonitor_mqtt_username: my_btmonitor
 | 
				
			||||||
 | 
					    my_btmonitor_mqtt_password: 8aBIAC14jaKKbla
 | 
				
			||||||
| 
						 | 
					
 | 
				
			||||||
| 
						 | 
					@ -1,23 +0,0 @@
 | 
				
			||||||
import asyncio
 | 
					 | 
				
			||||||
from bleak import BleakScanner
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
async def main():
 | 
					 | 
				
			||||||
    stop_event = asyncio.Event()
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
    # TODO: add something that calls stop_event.set()
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
    def callback(device, advertising_data):
 | 
					 | 
				
			||||||
        print("device", device.address, "advertising_data", advertising_data)
 | 
					 | 
				
			||||||
        # TODO: do something with incoming data
 | 
					 | 
				
			||||||
        pass
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
    async with BleakScanner(callback, scanning_mode="active") as scanner:
 | 
					 | 
				
			||||||
        # Important! Wait for an event to trigger stop, otherwise scanner
 | 
					 | 
				
			||||||
        # will stop immediately.
 | 
					 | 
				
			||||||
        await stop_event.wait()
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
    # scanner stops when block exits
 | 
					 | 
				
			||||||
    ...
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
asyncio.run(main())
 | 
					 | 
				
			||||||
| 
						 | 
					@ -1,38 +0,0 @@
 | 
				
			||||||
import asyncio
 | 
					 | 
				
			||||||
import logging
 | 
					 | 
				
			||||||
from bleak import BleakClient, BleakScanner
 | 
					 | 
				
			||||||
from bleak.assigned_numbers import AdvertisementDataType
 | 
					 | 
				
			||||||
from bleak.backends.bluezdbus.advertisement_monitor import OrPattern
 | 
					 | 
				
			||||||
from bleak.backends.bluezdbus.scanner import BlueZScannerArgs 
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
async def scan():
 | 
					 | 
				
			||||||
    args = BlueZScannerArgs(
 | 
					 | 
				
			||||||
        or_patterns=[OrPattern(0, AdvertisementDataType.MANUFACTURER_SPECIFIC_DATA, b"\x10\x05")]
 | 
					 | 
				
			||||||
    )
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
    async with BleakScanner(bluez=args, scanning_mode="passive") as scanner:
 | 
					 | 
				
			||||||
        async for _, advertisement_data in scanner.advertisement_data():
 | 
					 | 
				
			||||||
            mfr_data = advertisement_data.manufacturer_data
 | 
					 | 
				
			||||||
            if mfr_data.get(0x02e1):
 | 
					 | 
				
			||||||
                logging.info("scan(): found correct device: %s", mfr_data)
 | 
					 | 
				
			||||||
            else:
 | 
					 | 
				
			||||||
                logging.info("scan(): this should never happen: %s", mfr_data)
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
async def connect():
 | 
					 | 
				
			||||||
    device1 = await BleakScanner.find_device_by_address("01:B6:EC:10:CB:8F")
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
    async with BleakClient(device1):
 | 
					 | 
				
			||||||
        logging.info("connect(): connected to device")
 | 
					 | 
				
			||||||
        await asyncio.sleep(60)
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
async def main():
 | 
					 | 
				
			||||||
    logging.info("main(): starting scan")
 | 
					 | 
				
			||||||
    asyncio.create_task(scan())
 | 
					 | 
				
			||||||
    await asyncio.sleep(30)
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
logging.basicConfig(
 | 
					 | 
				
			||||||
    level=logging.INFO,
 | 
					 | 
				
			||||||
    format="%(asctime)-15s %(name)-8s %(levelname)s: %(message)s",
 | 
					 | 
				
			||||||
)
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
asyncio.run(main())
 | 
					 | 
				
			||||||
| 
						 | 
					@ -1,92 +0,0 @@
 | 
				
			||||||
import asyncio
 | 
					 | 
				
			||||||
from bleak import BleakScanner
 | 
					 | 
				
			||||||
from bleak.assigned_numbers import AdvertisementDataType
 | 
					 | 
				
			||||||
from bleak.backends.bluezdbus.advertisement_monitor import OrPattern
 | 
					 | 
				
			||||||
from bleak.backends.bluezdbus.scanner import BlueZScannerArgs 
 | 
					 | 
				
			||||||
from Crypto.Cipher import AES
 | 
					 | 
				
			||||||
from typing import Dict
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
irks = {
 | 
					 | 
				
			||||||
 "aa67542b82c0e05d65c27fb7e313aba5": "martins_apple_watch",
 | 
					 | 
				
			||||||
 "840e3892644c1ebd1594a9069c14ce0d" : "martins_iphone",
 | 
					 | 
				
			||||||
}
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
def resolve_rpa(rpa: bytes, irk: bytes) -> bool:
 | 
					 | 
				
			||||||
  """Compares the random address rpa to an irk (secret key) and return True if it matches"""
 | 
					 | 
				
			||||||
  assert len(rpa) == 6
 | 
					 | 
				
			||||||
  assert len(irk) == 16
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
  key = irk
 | 
					 | 
				
			||||||
  plain_text = b'\x00' * 16
 | 
					 | 
				
			||||||
  plain_text = bytearray(plain_text)
 | 
					 | 
				
			||||||
  plain_text[15] = rpa[3]
 | 
					 | 
				
			||||||
  plain_text[14] = rpa[4]
 | 
					 | 
				
			||||||
  plain_text[13] = rpa[5]
 | 
					 | 
				
			||||||
  plain_text = bytes(plain_text)
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
  cipher = AES.new(key, AES.MODE_ECB)
 | 
					 | 
				
			||||||
  cipher_text = cipher.encrypt(plain_text)
 | 
					 | 
				
			||||||
  return cipher_text[15] == rpa[0] and cipher_text[14] == rpa[1] and cipher_text[13] == rpa[2]
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
def addr_to_bytes(addr:str) -> bytes:
 | 
					 | 
				
			||||||
  """Converts a bluetooth mac address string with semicolons to bytes"""
 | 
					 | 
				
			||||||
  str_without_colons = addr.replace(":", "")
 | 
					 | 
				
			||||||
  bytearr =  bytearray.fromhex(str_without_colons)
 | 
					 | 
				
			||||||
  bytearr.reverse()
 | 
					 | 
				
			||||||
  return bytes(bytearr)
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
def decode_address(addr: str, irks: Dict[str, str]):
 | 
					 | 
				
			||||||
  """
 | 
					 | 
				
			||||||
  addr is a bluetooth address as a string  e.g. 4d:24:12:12:34:10
 | 
					 | 
				
			||||||
  irks is dict with irk as a hex string, mapping to device name
 | 
					 | 
				
			||||||
  """
 | 
					 | 
				
			||||||
  for irk, name in irks.items():
 | 
					 | 
				
			||||||
    if resolve_rpa(addr_to_bytes(addr), bytes.fromhex(irk)):
 | 
					 | 
				
			||||||
      return name
 | 
					 | 
				
			||||||
  return None
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
def estimate_distance(rssi, tx_power, pl0=54):
 | 
					 | 
				
			||||||
  """
 | 
					 | 
				
			||||||
  RSSI in dBm
 | 
					 | 
				
			||||||
  txPower is a transmitter parameter that calculated according to its physic layer and antenna in dBm
 | 
					 | 
				
			||||||
  Return value in meter
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
  You should calculate "PL0" in calibration stage:
 | 
					 | 
				
			||||||
  PL0 = txPower - RSSI; // When distance is distance0 (distance0 = 1m or more)
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
  SO, RSSI will be calculated by below formula:
 | 
					 | 
				
			||||||
  RSSI = txPower - PL0 - 10 * n * log(distance/distance0) - G(t)
 | 
					 | 
				
			||||||
  G(t) ~= 0 //This parameter is the main challenge in achiving to more accuracy.
 | 
					 | 
				
			||||||
  n = 2 (Path Loss Exponent, in the free space is 2)
 | 
					 | 
				
			||||||
  distance0 = 1 (m)
 | 
					 | 
				
			||||||
  distance = 10 ^ ((txPower - RSSI - PL0 ) / (10 * n))
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
  Read more details:
 | 
					 | 
				
			||||||
     https://en.wikipedia.org/wiki/Log-distance_path_loss_model
 | 
					 | 
				
			||||||
  """
 | 
					 | 
				
			||||||
  n = 2.7
 | 
					 | 
				
			||||||
  return 10**(( tx_power - rssi - pl0) / (10 * n))
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
def callback(device, advertising_data):
 | 
					 | 
				
			||||||
  print(device.address)
 | 
					 | 
				
			||||||
  decoded = decode_address(device.address, irks)
 | 
					 | 
				
			||||||
  if decoded:
 | 
					 | 
				
			||||||
    print(f"{decoded} @ {advertising_data.rssi} distance {estimate_distance(advertising_data.rssi, advertising_data.tx_power)}")
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
async def main():
 | 
					 | 
				
			||||||
    # Scan for 
 | 
					 | 
				
			||||||
    args = BlueZScannerArgs(
 | 
					 | 
				
			||||||
        or_patterns=[OrPattern(0, AdvertisementDataType.MANUFACTURER_SPECIFIC_DATA, b"\x10\x05")]
 | 
					 | 
				
			||||||
    )
 | 
					 | 
				
			||||||
    stop_event = asyncio.Event()
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
    #async with BleakScanner(bluez=args, scanning_mode="passive") as scanner:
 | 
					 | 
				
			||||||
    async with BleakScanner(callback) as scanner:
 | 
					 | 
				
			||||||
      await stop_event.wait()
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
asyncio.run(main())
 | 
					 | 
				
			||||||
| 
						 | 
					@ -1,10 +0,0 @@
 | 
				
			||||||
import asyncio
 | 
					 | 
				
			||||||
from bleak import BleakScanner
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
async def main():
 | 
					 | 
				
			||||||
    devices = await BleakScanner.discover()
 | 
					 | 
				
			||||||
    while True:
 | 
					 | 
				
			||||||
        for d in devices:
 | 
					 | 
				
			||||||
            print(d)
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
asyncio.run(main())
 | 
					 | 
				
			||||||
| 
						 | 
					@ -1,58 +0,0 @@
 | 
				
			||||||
"""Scan for iBeacons.
 | 
					 | 
				
			||||||
Copyright (c) 2022 Koen Vervloesem
 | 
					 | 
				
			||||||
SPDX-License-Identifier: MIT
 | 
					 | 
				
			||||||
"""
 | 
					 | 
				
			||||||
import asyncio
 | 
					 | 
				
			||||||
from uuid import UUID
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
from construct import Array, Byte, Const, Int8sl, Int16ub, Struct
 | 
					 | 
				
			||||||
from construct.core import ConstError
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
from bleak import BleakScanner
 | 
					 | 
				
			||||||
from bleak.backends.device import BLEDevice
 | 
					 | 
				
			||||||
from bleak.backends.scanner import AdvertisementData
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
ibeacon_format = Struct(
 | 
					 | 
				
			||||||
    "type_length" / Const(b"\x02\x15"),
 | 
					 | 
				
			||||||
    "uuid" / Array(16, Byte),
 | 
					 | 
				
			||||||
    "major" / Int16ub,
 | 
					 | 
				
			||||||
    "minor" / Int16ub,
 | 
					 | 
				
			||||||
    "power" / Int8sl,
 | 
					 | 
				
			||||||
)
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
def device_found(
 | 
					 | 
				
			||||||
    device: BLEDevice, advertisement_data: AdvertisementData
 | 
					 | 
				
			||||||
):
 | 
					 | 
				
			||||||
    """Decode iBeacon."""
 | 
					 | 
				
			||||||
    try:
 | 
					 | 
				
			||||||
        apple_data = advertisement_data.manufacturer_data[0x004C]
 | 
					 | 
				
			||||||
        print("apple data", apple_data)
 | 
					 | 
				
			||||||
        ibeacon = ibeacon_format.parse(apple_data)
 | 
					 | 
				
			||||||
        uuid = UUID(bytes=bytes(ibeacon.uuid))
 | 
					 | 
				
			||||||
        print(f"UUID     : {uuid}")
 | 
					 | 
				
			||||||
        print(f"Major    : {ibeacon.major}")
 | 
					 | 
				
			||||||
        print(f"Minor    : {ibeacon.minor}")
 | 
					 | 
				
			||||||
        print(f"TX power : {ibeacon.power} dBm")
 | 
					 | 
				
			||||||
        print(f"RSSI     : {device.rssi} dBm")
 | 
					 | 
				
			||||||
        print(47 * "-")
 | 
					 | 
				
			||||||
    except KeyError:
 | 
					 | 
				
			||||||
        # Apple company ID (0x004c) not found
 | 
					 | 
				
			||||||
        pass
 | 
					 | 
				
			||||||
    except ConstError:
 | 
					 | 
				
			||||||
        # No iBeacon (type 0x02 and length 0x15)
 | 
					 | 
				
			||||||
        pass
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
async def main():
 | 
					 | 
				
			||||||
    """Scan for devices."""
 | 
					 | 
				
			||||||
    scanner = BleakScanner()
 | 
					 | 
				
			||||||
    scanner.register_detection_callback(device_found)
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
    while True:
 | 
					 | 
				
			||||||
        await scanner.start()
 | 
					 | 
				
			||||||
        await asyncio.sleep(1.0)
 | 
					 | 
				
			||||||
        await scanner.stop()
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
asyncio.run(main())
 | 
					 | 
				
			||||||
| 
						 | 
					@ -1,3 +0,0 @@
 | 
				
			||||||
devices:
 | 
					 | 
				
			||||||
  martins_iphone: 840e3892644c1ebd1594a9069c14ce0d 
 | 
					 | 
				
			||||||
  martins_apple_watch: aa67542b82c0e05d65c27fb7e313aba5
 | 
					 | 
				
			||||||
| 
						 | 
					@ -0,0 +1,131 @@
 | 
				
			||||||
 | 
					#include <cmath>
 | 
				
			||||||
 | 
					#include <vector>
 | 
				
			||||||
 | 
					#include <iostream>
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					using real_t = double;
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					static constexpr real_t SPIKE_THRESHOLD = 1.0f; // Threshold for spike detection
 | 
				
			||||||
 | 
					static constexpr int   NUM_READINGS = 12;     // Number of readings to keep track of
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					class FilteredDistance {
 | 
				
			||||||
 | 
					   public:
 | 
				
			||||||
 | 
					    FilteredDistance(real_t minCutoff = 1e-1f, real_t beta = 1e-3, real_t dcutoff = 5e-3);
 | 
				
			||||||
 | 
					    void addMeasurement(real_t dist, real_t time_now_in_seconds);
 | 
				
			||||||
 | 
					    const real_t getMedianDistance() const;
 | 
				
			||||||
 | 
					    const real_t getDistance() const;
 | 
				
			||||||
 | 
					    const real_t getVariance() const;
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					    bool hasValue() const { return lastTime != 0; }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					   private:
 | 
				
			||||||
 | 
					    real_t minCutoff;
 | 
				
			||||||
 | 
					    real_t beta;
 | 
				
			||||||
 | 
					    real_t dcutoff;
 | 
				
			||||||
 | 
					    real_t x, dx;
 | 
				
			||||||
 | 
					    real_t lastDist;
 | 
				
			||||||
 | 
					    real_t lastTime;
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					    real_t getAlpha(real_t cutoff, real_t dT);
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					    real_t readings[NUM_READINGS];  // Array to store readings
 | 
				
			||||||
 | 
					    int readIndex;                 // Current position in the array
 | 
				
			||||||
 | 
					    real_t total;                   // Total of the readings
 | 
				
			||||||
 | 
					    real_t totalSquared;            // Total of the squared readings
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					    void initSpike(real_t dist);
 | 
				
			||||||
 | 
					    real_t removeSpike(real_t dist);
 | 
				
			||||||
 | 
					};
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					FilteredDistance::FilteredDistance(real_t minCutoff, real_t beta, real_t dcutoff)
 | 
				
			||||||
 | 
					    : minCutoff(minCutoff), beta(beta), dcutoff(dcutoff), x(0), dx(0), lastDist(0), lastTime(-1), total(0), totalSquared(0), readIndex(0) {
 | 
				
			||||||
 | 
					}
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					void FilteredDistance::initSpike(real_t dist) {
 | 
				
			||||||
 | 
					    for (size_t i = 0; i < NUM_READINGS; i++) {
 | 
				
			||||||
 | 
					        readings[i] = dist;
 | 
				
			||||||
 | 
					    }
 | 
				
			||||||
 | 
					    total = dist * NUM_READINGS;
 | 
				
			||||||
 | 
					    totalSquared = dist * dist * NUM_READINGS;  // Initialize sum of squared distances
 | 
				
			||||||
 | 
					}
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					real_t FilteredDistance::removeSpike(real_t dist) {
 | 
				
			||||||
 | 
					    total -= readings[readIndex];                               // Subtract the last reading
 | 
				
			||||||
 | 
					    totalSquared -= readings[readIndex] * readings[readIndex];  // Subtract the square of the last reading
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					    readings[readIndex] = dist;                                 // Read the sensor
 | 
				
			||||||
 | 
					    total += readings[readIndex];                               // Add the reading to the total
 | 
				
			||||||
 | 
					    totalSquared += readings[readIndex] * readings[readIndex];  // Add the square of the reading
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					    readIndex = (readIndex + 1) % NUM_READINGS;  // Advance to the next position in the array
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					    auto average = total / static_cast<real_t>(NUM_READINGS);  // Calculate the average
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					    if (std::fabs(dist - average) > SPIKE_THRESHOLD)
 | 
				
			||||||
 | 
					        return average;  // Spike detected, use the average as the filtered value
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					    return dist;  // No spike, return the new value
 | 
				
			||||||
 | 
					}
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					void FilteredDistance::addMeasurement(real_t dist, real_t time_now_in_seconds) {
 | 
				
			||||||
 | 
					    const bool initialized = lastTime >= 0;
 | 
				
			||||||
 | 
					    const real_t elapsed = time_now_in_seconds - lastTime;
 | 
				
			||||||
 | 
					    lastTime = time_now_in_seconds;
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					    if (!initialized) {
 | 
				
			||||||
 | 
					        x = dist;  // Set initial filter state to the first reading
 | 
				
			||||||
 | 
					        dx = 0;    // Initial derivative is unknown, so we set it to zero
 | 
				
			||||||
 | 
					        lastDist = dist;
 | 
				
			||||||
 | 
					        initSpike(dist);
 | 
				
			||||||
 | 
					    } else {
 | 
				
			||||||
 | 
					        real_t dT = std::max(elapsed, real_t(0.05));  // Convert microseconds to seconds, enforce a minimum dT
 | 
				
			||||||
 | 
					        const real_t alpha = getAlpha(minCutoff, dT);
 | 
				
			||||||
 | 
					        const real_t dAlpha = getAlpha(dcutoff, dT);
 | 
				
			||||||
 | 
					        dist = removeSpike(dist);
 | 
				
			||||||
 | 
					        x += alpha * (dist - x);
 | 
				
			||||||
 | 
					        dx = dAlpha * ((dist - lastDist) / dT);
 | 
				
			||||||
 | 
					        lastDist = x + beta * dx;
 | 
				
			||||||
 | 
					        std::cout << "alpha=" << alpha << 
 | 
				
			||||||
 | 
					                     " dAlpha=" << dAlpha << 
 | 
				
			||||||
 | 
					                     " dist=" << dist << 
 | 
				
			||||||
 | 
					                     " x=" << x << 
 | 
				
			||||||
 | 
					                     " dx=" << dx << 
 | 
				
			||||||
 | 
					                     " lastDist=" << lastDist << 
 | 
				
			||||||
 | 
					                     std::endl;  
 | 
				
			||||||
 | 
					    }
 | 
				
			||||||
 | 
					}
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					const real_t FilteredDistance::getDistance() const {
 | 
				
			||||||
 | 
					    return lastDist;
 | 
				
			||||||
 | 
					}
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					real_t FilteredDistance::getAlpha(real_t cutoff, real_t dT) {
 | 
				
			||||||
 | 
					    real_t tau = 1.0f / (2 * M_PI * cutoff);
 | 
				
			||||||
 | 
					    return 1.0f / (1.0f + tau / dT);
 | 
				
			||||||
 | 
					}
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					const real_t FilteredDistance::getVariance() const {
 | 
				
			||||||
 | 
					    auto mean = total / static_cast<real_t>(NUM_READINGS);
 | 
				
			||||||
 | 
					    auto meanOfSquares = totalSquared / static_cast<real_t>(NUM_READINGS);
 | 
				
			||||||
 | 
					    auto variance = meanOfSquares - (mean * mean);  // Variance formula: E(X^2) - (E(X))^2
 | 
				
			||||||
 | 
					    if (variance < 0.0f) return 0.0f;
 | 
				
			||||||
 | 
					    return variance;
 | 
				
			||||||
 | 
					}
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					int main(int argc, char**argv)
 | 
				
			||||||
 | 
					{
 | 
				
			||||||
 | 
					  FilteredDistance f;
 | 
				
			||||||
 | 
					  std::vector<real_t> values = {1.5, 2.9, 5.3, 15.1, 1.5, 2.5, 1.5, 2.9, 5.3, 15.1};
 | 
				
			||||||
 | 
					                             
 | 
				
			||||||
 | 
					  real_t time = 0.0;
 | 
				
			||||||
 | 
					  //std::cout << " result_cpp = [";
 | 
				
			||||||
 | 
					  for(int i=0; i < 1; ++i)
 | 
				
			||||||
 | 
					    for(auto value : values) {
 | 
				
			||||||
 | 
					      f.addMeasurement(value, time);
 | 
				
			||||||
 | 
					      time += 1.0;
 | 
				
			||||||
 | 
					      //std::cout << f.getDistance() << ", ";
 | 
				
			||||||
 | 
					    }
 | 
				
			||||||
 | 
					  //std::cout << "]" << std::endl;
 | 
				
			||||||
 | 
					  return 0;
 | 
				
			||||||
 | 
					}
 | 
				
			||||||
| 
						 | 
					@ -0,0 +1,91 @@
 | 
				
			||||||
 | 
					#from time import time
 | 
				
			||||||
 | 
					import math
 | 
				
			||||||
 | 
					from scipy import signal
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					# Taken from ESPresense C++ code
 | 
				
			||||||
 | 
					class FilteredDistance:
 | 
				
			||||||
 | 
					  NUM_READINGS = 100
 | 
				
			||||||
 | 
					  SPIKE_THRESHOLD = 1.0
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  def __init__(self, min_cutoff : float = 1e-1, beta : float = 1e-3, dcutoff : float = 5e-3):
 | 
				
			||||||
 | 
					      self.min_cutoff = min_cutoff
 | 
				
			||||||
 | 
					      self.beta = beta
 | 
				
			||||||
 | 
					      self.dcutoff = dcutoff
 | 
				
			||||||
 | 
					      self.x = 0
 | 
				
			||||||
 | 
					      self.dx = 0
 | 
				
			||||||
 | 
					      self.last_dist = 0
 | 
				
			||||||
 | 
					      self.last_time = -1.0
 | 
				
			||||||
 | 
					      self.total = 0
 | 
				
			||||||
 | 
					      self.read_index = 0
 | 
				
			||||||
 | 
					      self.readings = []
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  def _init_spike(self, dist : float):
 | 
				
			||||||
 | 
					    self.readings = [dist] * self.NUM_READINGS
 | 
				
			||||||
 | 
					    self.total = sum(self.readings)
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  def _remove_spike(self, dist: float):
 | 
				
			||||||
 | 
					     self.total -= self.readings[self.read_index]     
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					     self.readings[self.read_index] = dist
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					     self.total += dist
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					     self.read_index = (self.read_index + 1) % self.NUM_READINGS
 | 
				
			||||||
 | 
					     average = self.total / self.NUM_READINGS
 | 
				
			||||||
 | 
					     if abs(dist - average) > self.SPIKE_THRESHOLD:
 | 
				
			||||||
 | 
					        return average # spike detected
 | 
				
			||||||
 | 
					     else:
 | 
				
			||||||
 | 
					        return dist
 | 
				
			||||||
 | 
					  
 | 
				
			||||||
 | 
					  def _get_alpha(self, cutoff : float, dT : float):
 | 
				
			||||||
 | 
					      tau = 1 / (2 * math.pi * cutoff)
 | 
				
			||||||
 | 
					      return 1 / (1 + tau / dT)
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  def add_measurement(self, dist : float, time_now_in_seconds: float):
 | 
				
			||||||
 | 
					    initialized = (self.last_time >= 0.0)
 | 
				
			||||||
 | 
					    elapsed = time_now_in_seconds - self.last_time
 | 
				
			||||||
 | 
					    self.last_time = time_now_in_seconds
 | 
				
			||||||
 | 
					    if not initialized:
 | 
				
			||||||
 | 
					      self.x = dist
 | 
				
			||||||
 | 
					      self.dx = 0
 | 
				
			||||||
 | 
					      self.last_dist = dist
 | 
				
			||||||
 | 
					      self._init_spike(dist)
 | 
				
			||||||
 | 
					    else:
 | 
				
			||||||
 | 
					       dT = max(elapsed, 0.05)
 | 
				
			||||||
 | 
					       alpha = self._get_alpha(self.min_cutoff, dT)
 | 
				
			||||||
 | 
					       d_alpha = self._get_alpha(self.dcutoff, dT)
 | 
				
			||||||
 | 
					       dist = self._remove_spike(dist)
 | 
				
			||||||
 | 
					       self.x += alpha * (dist - self.x)
 | 
				
			||||||
 | 
					       self.dx = d_alpha * ((dist - self.last_dist) / dT)
 | 
				
			||||||
 | 
					       self.last_dist = self.x + self.beta * self.dx
 | 
				
			||||||
 | 
					       #print(f"{alpha=} {d_alpha=} {dist=} {self.x=} {self.dx=} {self.last_dist=}")
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  def get_distance(self):
 | 
				
			||||||
 | 
					     return self.last_dist
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					def run_test(times, values, **kwargs):
 | 
				
			||||||
 | 
					   f = FilteredDistance(**kwargs)
 | 
				
			||||||
 | 
					   result = []
 | 
				
			||||||
 | 
					   for t, value in zip(times, values):
 | 
				
			||||||
 | 
					     f.add_measurement(value, t)
 | 
				
			||||||
 | 
					     result.append(f.get_distance())
 | 
				
			||||||
 | 
					   return result
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					def smooth(y, box_pts):
 | 
				
			||||||
 | 
					    box = np.ones(box_pts)/box_pts
 | 
				
			||||||
 | 
					    y_smooth = np.convolve(y, box, mode='same')
 | 
				
			||||||
 | 
					    return y_smooth
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					if __name__ == "__main__":
 | 
				
			||||||
 | 
					  import numpy as np
 | 
				
			||||||
 | 
					  values = np.array([1] * 20 + [2, 4, 6, 7, 10, 16, 10, 13, 16, 24, 13] + [1] * 20 )
 | 
				
			||||||
 | 
					  
 | 
				
			||||||
 | 
					  times = np.arange(0, len(values)) * 10
 | 
				
			||||||
 | 
					  result_default = run_test(times, values)
 | 
				
			||||||
 | 
					  result_beta1 = smooth(values, 6)
 | 
				
			||||||
 | 
					  import matplotlib.pyplot as plt
 | 
				
			||||||
 | 
					  plt.plot(times, values, label="raw")
 | 
				
			||||||
 | 
					  #plt.plot(times, result_default, marker="o", label="filtered")
 | 
				
			||||||
 | 
					  plt.plot(times, result_beta1, marker='x', label="altered")
 | 
				
			||||||
 | 
					  plt.legend()
 | 
				
			||||||
 | 
					  plt.show()
 | 
				
			||||||
										
											Binary file not shown.
										
									
								
							| 
						 | 
					@ -0,0 +1,11 @@
 | 
				
			||||||
 | 
					[Unit]
 | 
				
			||||||
 | 
					Description=My Bluetooth monitor
 | 
				
			||||||
 | 
					After=network.target
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					[Service]
 | 
				
			||||||
 | 
					Type=simple
 | 
				
			||||||
 | 
					Restart=always
 | 
				
			||||||
 | 
					ExecStart=/usr/bin/my_btmonitor
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					[Install]
 | 
				
			||||||
 | 
					WantedBy=multi-user.target
 | 
				
			||||||
| 
						 | 
					@ -1,8 +1,22 @@
 | 
				
			||||||
- name: Apt install bluez and firmware
 | 
					- name: Apt install bluez, firmware and Python requirements
 | 
				
			||||||
  apt:
 | 
					  apt:
 | 
				
			||||||
    name: 
 | 
					    name: 
 | 
				
			||||||
      - bluez
 | 
					      - bluez
 | 
				
			||||||
      - bluez-firmware
 | 
					      - bluez-firmware
 | 
				
			||||||
      - firmware-realtek
 | 
					      - firmware-realtek
 | 
				
			||||||
      - firmware-realtek-rtl8723cs-bt
 | 
					      - firmware-realtek-rtl8723cs-bt
 | 
				
			||||||
      
 | 
					      - python3-pycryptodome
 | 
				
			||||||
 | 
					      - python3-bleak
 | 
				
			||||||
 | 
					      - python3-asyncio-mqtt
 | 
				
			||||||
 | 
					      - python3-numpy
 | 
				
			||||||
 | 
					- name: Copy monitor script
 | 
				
			||||||
 | 
					  template: src=my_btmonitor.py dest=/usr/bin/my_btmonitor owner=root mode=u+rwx
 | 
				
			||||||
 | 
					- name: Install systemd service file
 | 
				
			||||||
 | 
					  copy: src=my_btmonitor.service dest=/etc/systemd/system/
 | 
				
			||||||
 | 
					- name: Add script to autostart and start now
 | 
				
			||||||
 | 
					  systemd: name=my_btmonitor state=restarted enabled=yes daemon_reload=yes
 | 
				
			||||||
 | 
					#- name: Add to sysdweb
 | 
				
			||||||
 | 
					#  include_role:
 | 
				
			||||||
 | 
					#    name: pi-sysdweb
 | 
				
			||||||
 | 
					#  vars:
 | 
				
			||||||
 | 
					#    sysdweb_name: my_btmonitor  
 | 
				
			||||||
| 
						 | 
					@ -0,0 +1,149 @@
 | 
				
			||||||
 | 
					#!/usr/bin/env python3
 | 
				
			||||||
 | 
					import asyncio
 | 
				
			||||||
 | 
					from bleak import BleakScanner
 | 
				
			||||||
 | 
					from bleak.assigned_numbers import AdvertisementDataType
 | 
				
			||||||
 | 
					from bleak.backends.bluezdbus.advertisement_monitor import OrPattern
 | 
				
			||||||
 | 
					from bleak.backends.bluezdbus.scanner import BlueZScannerArgs 
 | 
				
			||||||
 | 
					from Cryptodome.Cipher import AES
 | 
				
			||||||
 | 
					from functools import partial
 | 
				
			||||||
 | 
					import asyncio_mqtt
 | 
				
			||||||
 | 
					import json
 | 
				
			||||||
 | 
					from typing import Dict
 | 
				
			||||||
 | 
					import collections
 | 
				
			||||||
 | 
					import numpy as np
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					# ------------------- Config ----------------------------------------------------------------
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					config = {
 | 
				
			||||||
 | 
					  "mqtt": {
 | 
				
			||||||
 | 
					    "hostname": "homeassistant.fritz.box",
 | 
				
			||||||
 | 
					    "username": "{{my_btmonitor_mqtt_username}}",
 | 
				
			||||||
 | 
					    "password": "{{my_btmonitor_mqtt_password}}",
 | 
				
			||||||
 | 
					    "room": "{{sensor_room_name_ascii}}"
 | 
				
			||||||
 | 
					  },
 | 
				
			||||||
 | 
					  "irk_to_devicename": {
 | 
				
			||||||
 | 
					    "aa67542b82c0e05d65c27fb7e313aba5": "martins_apple_watch",
 | 
				
			||||||
 | 
					    "840e3892644c1ebd1594a9069c14ce0d" : "martins_iphone",
 | 
				
			||||||
 | 
					  }
 | 
				
			||||||
 | 
					}
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					# --------------------------------------------------------------------------------------------
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					def resolve_rpa(rpa: bytes, irk: bytes) -> bool:
 | 
				
			||||||
 | 
					  """Compares the random address rpa to an irk (secret key) and return True if it matches"""
 | 
				
			||||||
 | 
					  assert len(rpa) == 6
 | 
				
			||||||
 | 
					  assert len(irk) == 16
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  key = irk
 | 
				
			||||||
 | 
					  plain_text = b'\x00' * 16
 | 
				
			||||||
 | 
					  plain_text = bytearray(plain_text)
 | 
				
			||||||
 | 
					  plain_text[15] = rpa[3]
 | 
				
			||||||
 | 
					  plain_text[14] = rpa[4]
 | 
				
			||||||
 | 
					  plain_text[13] = rpa[5]
 | 
				
			||||||
 | 
					  plain_text = bytes(plain_text)
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  cipher = AES.new(key, AES.MODE_ECB)
 | 
				
			||||||
 | 
					  cipher_text = cipher.encrypt(plain_text)
 | 
				
			||||||
 | 
					  return cipher_text[15] == rpa[0] and cipher_text[14] == rpa[1] and cipher_text[13] == rpa[2]
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					def addr_to_bytes(addr:str) -> bytes:
 | 
				
			||||||
 | 
					  """Converts a bluetooth mac address string with semicolons to bytes"""
 | 
				
			||||||
 | 
					  str_without_colons = addr.replace(":", "")
 | 
				
			||||||
 | 
					  bytearr =  bytearray.fromhex(str_without_colons)
 | 
				
			||||||
 | 
					  bytearr.reverse()
 | 
				
			||||||
 | 
					  return bytes(bytearr)
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					def decode_address(addr: str, irks: Dict[str, str]):
 | 
				
			||||||
 | 
					  """
 | 
				
			||||||
 | 
					  addr is a bluetooth address as a string  e.g. 4d:24:12:12:34:10
 | 
				
			||||||
 | 
					  irks is dict with irk as a hex string, mapping to device name
 | 
				
			||||||
 | 
					  """
 | 
				
			||||||
 | 
					  for irk, name in irks.items():
 | 
				
			||||||
 | 
					    if resolve_rpa(addr_to_bytes(addr), bytes.fromhex(irk)):
 | 
				
			||||||
 | 
					      return name
 | 
				
			||||||
 | 
					  return None
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					def estimate_distance(rssi, tx_power, pl0=73):
 | 
				
			||||||
 | 
					  """
 | 
				
			||||||
 | 
					  RSSI in dBm
 | 
				
			||||||
 | 
					  txPower is a transmitter parameter that calculated according to its physic layer and antenna in dBm
 | 
				
			||||||
 | 
					  Return value in meter
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  You should calculate "PL0" in calibration stage:
 | 
				
			||||||
 | 
					  PL0 = txPower - RSSI; // When distance is distance0 (distance0 = 1m or more)
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  SO, RSSI will be calculated by below formula:
 | 
				
			||||||
 | 
					  RSSI = txPower - PL0 - 10 * n * log(distance/distance0) - G(t)
 | 
				
			||||||
 | 
					  G(t) ~= 0 //This parameter is the main challenge in achiving to more accuracy.
 | 
				
			||||||
 | 
					  n = 2 (Path Loss Exponent, in the free space is 2)
 | 
				
			||||||
 | 
					  distance0 = 1 (m)
 | 
				
			||||||
 | 
					  distance = 10 ^ ((txPower - RSSI - PL0 ) / (10 * n))
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  Read more details:
 | 
				
			||||||
 | 
					     https://en.wikipedia.org/wiki/Log-distance_path_loss_model
 | 
				
			||||||
 | 
					  """
 | 
				
			||||||
 | 
					  n = 3.5
 | 
				
			||||||
 | 
					  return 10**((tx_power - rssi - pl0) / (10 * n))
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					def smooth(y, box_pts):
 | 
				
			||||||
 | 
					    box = np.ones(box_pts)/box_pts
 | 
				
			||||||
 | 
					    y_smooth = np.convolve(y, box, mode='same')
 | 
				
			||||||
 | 
					    return y_smooth
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					WINDOW_SIZE = 6
 | 
				
			||||||
 | 
					last_values = collections.deque(maxlen=WINDOW_SIZE)
 | 
				
			||||||
 | 
					def filter_distance(dist: float):
 | 
				
			||||||
 | 
					  global last_values
 | 
				
			||||||
 | 
					  last_values.append(dist)
 | 
				
			||||||
 | 
					  return smooth(np.array(last_values), 12)[-1]
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					async def on_device_found_callback(irks, mqtt_client, room, device, advertising_data):
 | 
				
			||||||
 | 
					  decoded_device_id = decode_address(device.address, irks)
 | 
				
			||||||
 | 
					  rssi = advertising_data.rssi
 | 
				
			||||||
 | 
					  tx_power = advertising_data.tx_power
 | 
				
			||||||
 | 
					  if decoded_device_id and tx_power is not None and rssi is not None:
 | 
				
			||||||
 | 
					    topic = f"my_btmonitor/devices/{decoded_device_id}/{room}"
 | 
				
			||||||
 | 
					    distance = estimate_distance(rssi, tx_power, {{my_btmonitor_pl0 | default('73')}} )
 | 
				
			||||||
 | 
					    filtered_distance = filter_distance(distance)
 | 
				
			||||||
 | 
					    data = {"id": decoded_device_id, 
 | 
				
			||||||
 | 
					            "name": decoded_device_id, 
 | 
				
			||||||
 | 
					            "rssi": rssi, 
 | 
				
			||||||
 | 
					            "tx_power": tx_power,
 | 
				
			||||||
 | 
					            "distance": filtered_distance,
 | 
				
			||||||
 | 
					            "unfiltered_distance": distance,
 | 
				
			||||||
 | 
					    }
 | 
				
			||||||
 | 
					    await mqtt_client.publish(topic, json.dumps(data).encode())
 | 
				
			||||||
 | 
					    #print(data)
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					async def main():
 | 
				
			||||||
 | 
					    stop_event = asyncio.Event()
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					    mqtt_conf = config['mqtt']
 | 
				
			||||||
 | 
					    while True:
 | 
				
			||||||
 | 
					      try:
 | 
				
			||||||
 | 
					        async with asyncio_mqtt.Client(hostname=mqtt_conf["hostname"], 
 | 
				
			||||||
 | 
					                                      username=mqtt_conf["username"], 
 | 
				
			||||||
 | 
					                                      password=mqtt_conf['password']) as mqtt_client:
 | 
				
			||||||
 | 
					          cb = partial(on_device_found_callback, config['irk_to_devicename'], mqtt_client, mqtt_conf['room'])
 | 
				
			||||||
 | 
					          active_scan = True
 | 
				
			||||||
 | 
					          if active_scan:
 | 
				
			||||||
 | 
					            async with BleakScanner(cb) as scanner:
 | 
				
			||||||
 | 
					              await stop_event.wait()
 | 
				
			||||||
 | 
					          else:
 | 
				
			||||||
 | 
					            # Doesn't work, because of the strange or_patters
 | 
				
			||||||
 | 
					            args = BlueZScannerArgs(
 | 
				
			||||||
 | 
					                or_patterns=[OrPattern(0, AdvertisementDataType.MANUFACTURER_SPECIFIC_DATA, b"\x00\x4c")]
 | 
				
			||||||
 | 
					            )
 | 
				
			||||||
 | 
					            async with BleakScanner(cb, bluez=args, scanning_mode="passive") as scanner:
 | 
				
			||||||
 | 
					              await stop_event.wait()
 | 
				
			||||||
 | 
					      except Exception as e:
 | 
				
			||||||
 | 
					        print("Error", e)              
 | 
				
			||||||
 | 
					        print("Starting again")
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					asyncio.run(main())
 | 
				
			||||||
| 
						 | 
					@ -0,0 +1,4 @@
 | 
				
			||||||
 | 
					---
 | 
				
			||||||
 | 
					- name: reboot
 | 
				
			||||||
 | 
					  reboot:
 | 
				
			||||||
 | 
					
 | 
				
			||||||
| 
						 | 
					@ -0,0 +1,7 @@
 | 
				
			||||||
 | 
					---
 | 
				
			||||||
 | 
					- name: Deactivate onboard bluetooth
 | 
				
			||||||
 | 
					  lineinfile:
 | 
				
			||||||
 | 
					    path: /boot/firmware/config.txt
 | 
				
			||||||
 | 
					    regexp: "^#?dtoverlay=disable-bt"
 | 
				
			||||||
 | 
					    line: "dtoverlay=disable-bt"
 | 
				
			||||||
 | 
					  notify: reboot
 | 
				
			||||||
| 
						 | 
					@ -1,6 +1,6 @@
 | 
				
			||||||
[Unit]
 | 
					[Unit]
 | 
				
			||||||
Description=IR server for IR remotes
 | 
					Description=IR server for IR remotes
 | 
				
			||||||
After=multi-user.target
 | 
					After=network.target
 | 
				
			||||||
 | 
					
 | 
				
			||||||
[Service]
 | 
					[Service]
 | 
				
			||||||
Type=simple
 | 
					Type=simple
 | 
				
			||||||
| 
						 | 
					
 | 
				
			||||||
| 
						 | 
					@ -6,7 +6,7 @@
 | 
				
			||||||
      - libflac-dev
 | 
					      - libflac-dev
 | 
				
			||||||
      - libfaad2
 | 
					      - libfaad2
 | 
				
			||||||
      - libmad0
 | 
					      - libmad0
 | 
				
			||||||
      - perl-openssl-abi-1.1
 | 
					      - perl-openssl-defaults
 | 
				
			||||||
      - libnet-ssleay-perl
 | 
					      - libnet-ssleay-perl
 | 
				
			||||||
      - libio-socket-ssl-perl
 | 
					      - libio-socket-ssl-perl
 | 
				
			||||||
      - nasm
 | 
					      - nasm
 | 
				
			||||||
| 
						 | 
					
 | 
				
			||||||
| 
						 | 
					@ -6,4 +6,5 @@
 | 
				
			||||||
    - server-exthdd-mount
 | 
					    - server-exthdd-mount
 | 
				
			||||||
    - server-nfs
 | 
					    - server-nfs
 | 
				
			||||||
    - server-link-aggregation
 | 
					    - server-link-aggregation
 | 
				
			||||||
 | 
					    - bluetooth-monitor
 | 
				
			||||||
 | 
					
 | 
				
			||||||
| 
						 | 
					
 | 
				
			||||||
| 
						 | 
					@ -0,0 +1,20 @@
 | 
				
			||||||
 | 
					---
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					- hosts: server
 | 
				
			||||||
 | 
					  roles:
 | 
				
			||||||
 | 
					    - bluetooth-monitor
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					- hosts: musikserverwohnzimmeroben
 | 
				
			||||||
 | 
					  roles:
 | 
				
			||||||
 | 
					    - bluetooth-monitor
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					- hosts: kitchenpi  
 | 
				
			||||||
 | 
					  roles:
 | 
				
			||||||
 | 
					    - pi-disable-onboard-bluetooth
 | 
				
			||||||
 | 
					    - bluetooth-monitor
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					- hosts: bedroompi
 | 
				
			||||||
 | 
					  roles:
 | 
				
			||||||
 | 
					    - pi-disable-onboard-bluetooth
 | 
				
			||||||
 | 
					    - bluetooth-monitor
 | 
				
			||||||
 | 
					
 | 
				
			||||||
		Loading…
	
		Reference in New Issue