<repo_name>kacpercierpiak/SensorTagPython<file_sep>/SensorTagPython/main.py
import asyncio
import threading
import time
import curses
from os import name, system
from bleak import discover,BleakClient
from sensor_calcs import *

stdscr = curses.initscr()
curses.noecho()

class gatt_set:
    def __init__(self, data, config, extra = ''):        
        self.data = data
        self.config = config
        self.extra = extra

global sensor_data
sensor_data = {}
sensor_data['sensor_tag'] = ''
sensor_data['key'] = 'None'
sensor_data['temp'] = 0
sensor_data['humid'] = 0
sensor_data['bar'] = 0
sensor_data['accel'] = [0.0,0.0,0.0]
sensor_data['gyro'] = [0.0,0.0,0.0]
sensor_data['magn'] = [0.0,0.0,0.0]

gatt_services = dict({
   'Temp': gatt_set('f000aa01-0451-4000-b000-000000000000','f000aa02-0451-4000-b000-000000000000'),
   'Accel': gatt_set('f000aa11-0451-4000-b000-000000000000','f000aa12-0451-4000-b000-000000000000','f000aa13-0451-4000-b000-000000000000'),
   'Humid': gatt_set('f000aa21-0451-4000-b000-000000000000','f000aa22-0451-4000-b000-000000000000'),
   'Mag': gatt_set('f000aa31-0451-4000-b000-000000000000','f000aa32-0451-4000-b000-000000000000','f000aa33-0451-4000-b000-000000000000'),
   'Barometer': gatt_set('f000aa41-0451-4000-b000-000000000000','f000aa42-0451-4000-b000-000000000000','f000aa43-0451-4000-b000-000000000000'),
   'Gyro': gatt_set('f000aa51-0451-4000-b000-000000000000','f000aa52-0451-4000-b000-000000000000'),
   'Key': gatt_set('0<KEY>','')
   }) 

def print_info(info):
    stdscr.clear()
    stdscr.addstr(str(info))
    stdscr.refresh()  


def draw():     
    while True:        
        info = "SensorTag: " + sensor_data['sensor_tag'] + '\n'
        info += "Key status: " + sensor_data['key'] + '\n'
        info += "Temp: %.2f" % sensor_data['temp'] + '\n'
        info += "Humid: %.2f" % sensor_data['humid'] + '\n'
        info += "Accell: [%.2f, %.2f, %.2f]" % (sensor_data['accel'][0],sensor_data['accel'][1],sensor_data['accel'][2]) + '\n'
        info += "Gyro: [%.2f, %.2f, %.2f]" % (sensor_data['gyro'][0],sensor_data['gyro'][1],sensor_data['gyro'][2]) + '\n'
        info += "Magn: [%.2f, %.2f, %.2f]" % (sensor_data['magn'][0],sensor_data['magn'][1],sensor_data['magn'][2]) + '\n'
        print_info(info)
        time.sleep(0.1)   

def notification_handler(sender, data):
    if sender == 36: # Temp
        sensor_data['temp'] = temp_calc(data)
    elif sender == 44: # Accel
        sensor_data['accel'] = accel_calc(data)
    elif sender == 55: # Humid
        sensor_data['humid'] = humid_calc(data)
    elif sender == 63: # Magn
        sensor_data['magn'] = magn_calc(data)
    elif sender == 74: # Barometer
        sensor_data['bar'] = bar_calc(data)
    elif sender == 86: # Gyro
        sensor_data['gyro'] = gyro_calc(data)
    elif sender == 94: # Key
        sensor_data['key'] = key_calc(data)

async def main(): 
    sensor_data['sensor_tag'] = 'Discover devices'
    devices = await discover()
    for d in devices:
        if d.name == 'SensorTag':
            sensor_data['sensor_tag'] = d.address
            async with BleakClient(d.address) as client:
                client.pair()
                for service in gatt_services:                 
                    if service == 'Gyro':
                        await client.write_gatt_char(gatt_services[service].config,[0x07])
                    else:
                        char_conf = gatt_services[service].config
                        if char_conf != '':
                            await client.write_gatt_char(char_conf,[0x01]) 
                            
                    await client.start_notify(gatt_services[service].data, notification_handler)   
                    
                while(True):                
                    await asyncio.sleep(1.0)                    

if __name__ == "__main__":    
    x = threading.Thread(target=draw)
    x.start()
    asyncio.run(main())  <file_sep>/SensorTagPython/sensor_calcs.py
from os import name, system

tosigned = lambda n: float(n-0x10000) if n>0x7fff else float(n)
tosignedbyte = lambda n: float(n-0x100) if n>0x7f else float(n)

def calcTmpTarget(objT, ambT):
        
    objT = tosigned(objT)
    ambT = tosigned(ambT)

    m_tmpAmb = ambT/128.0
    Vobj2 = objT * 0.00000015625
    Tdie2 = m_tmpAmb + 273.15
    S0 = 6.4E-14            # Calibration factor
    a1 = 1.75E-3
    a2 = -1.678E-5
    b0 = -2.94E-5
    b1 = -5.7E-7
    b2 = 4.63E-9
    c2 = 13.4
    Tref = 298.15
    S = S0*(1+a1*(Tdie2 - Tref)+a2*pow((Tdie2 - Tref),2))
    Vos = b0 + b1*(Tdie2 - Tref) + b2*pow((Tdie2 - Tref),2)
    fObj = (Vobj2 - Vos) + c2*pow((Vobj2 - Vos),2)
    tObj = pow(pow(Tdie2,4) + (fObj/S),.25)
    tObj = (tObj - 273.15)
    return tObj

def temp_calc(data):  
    objT = (data[1]<<8)+data[0]
    ambT = (data[3]<<8)+data[2]
    return calcTmpTarget(objT, ambT)   

def accel_calc(data):
    acc = lambda v: tosignedbyte(v) / 64.0
    return [acc(data[0]), acc(data[1]), acc(data[2]) * -1]     

def humid_calc(data): 
    rawH = (data[3]<<8)+data[2]   
    rawH = float(int(rawH) & ~0x0003)
    return -6.0 + 125.0/65536.0 * rawH  

def magn_calc(data):
    x = (data[1] & 0xFF <<8)+data[0] & 0xFF
    y = (data[3] & 0xFF <<8)+data[2] & 0xFF
    z = (data[5] & 0xFF <<8)+data[4] & 0xFF
    magforce = lambda v: (tosigned(v) * 1.0) / (65536.0/2000.0)
    return [magforce(x),magforce(y),magforce(z)]

def bar_calc(data):
    m_raw_temp = tosigned((data[1]<<8)+data[0])
    m_raw_pres = (data[3]<<8)+data[2]
    return 1000.0 

def gyro_calc(data):
    x = (data[1]<<8)+data[0]
    y = (data[3]<<8)+data[2]
    z = (data[5]<<8)+data[4]
    result = lambda v: (tosigned(v) * 1.0) / (65536.0/500.0)
    return [result(x),result(y) ,result(z)]

def key_calc(data):
    i = int.from_bytes(data, "big")
    result =''
    if i == 1:
        result = 'Right'
    elif i == 2:
        result = 'Left'
    elif i == 3:
        result = 'Left and Right'
    else:
        result = 'None'
    return result